add array operation;saa slpit; sky unit parameter (fffdcf28) · Commits · csst-planning / csst_survey_sim

survey_sim/constraints/_utils.py

+120 −24

Original line number	Diff line number	Diff line
		@@ -325,6 +325,21 @@ def quaternion_multiply(q1, q2):
		])


		def quaternion_multiply_array(q1, q2):
		"""
		计算两个四元数的乘积 q = q1 * q2
		四元数格式为 [q0, q1, q2, q3]
		"""
		w1, x1, y1, z1 = q1.T
		w2, x2, y2, z2 = q2.T
		return np.array([
		w1w2 - x1x2 - y1y2 - z1z2,
		w1x2 + x1w2 + y1z2 - z1y2,
		w1y2 - x1z2 + y1w2 + z1x2,
		w1z2 + x1y2 - y1x2 + z1w2
		]).T


		def quaternion_conjugate(q):
		"""
		计算两个四元数的共轭
		@@ -377,6 +392,32 @@ def axis_angle_from_quaternion(q):
		return axis, angle


		def angle_from_quaternion_array(q):
		"""
		将单位四元数转换为旋转轴和旋转角（弧度）

		参数:
		q: 四元数 [q0, q1, q2, q3]

		返回:
		axis: 旋转轴（单位向量）
		angle: 旋转角（单位：弧度）
		"""
		q0, q1, q2, q3 = q
		q0 = np.clip(q0, -1.0, 1.0)
		angle = 2 * np.arccos(q0)

		sin_half_angle = np.sqrt(1 - q0*q0)

		if sin_half_angle < 1e-8:
		# 接近0度旋转，轴任意，这里返回Z轴
		axis = np.array([0.0, 0.0, 1.0])
		else:
		axis = np.array([q1, q2, q3]) / sin_half_angle

		return axis, angle


		def transferlonlat2cardisian(lon_rad=0., lat_rad=0.):
		return [math.cos(lat_rad) * math.cos(
		lon_rad), math.cos(lat_rad) * math.sin(lon_rad), math.sin(lat_rad)]
		@@ -428,6 +469,55 @@ def rotate_quaternion_byself(lon=90., lat=90., pa=0., quaternion=[1, 0, 0, 0]):
		return q_total


		def get_RotationAngle_twoQuaternion_onlyAngle(q1, q2):
		"""
		计算两个四元数之间的夹角（以弧度为单位）

		参数:
		q1, q2 -- 输入的四元数，格式为 [w, x, y, z]

		返回:
		两个四元数之间的夹角（弧度）
		"""
		# 确保四元数是单位四元数
		q1 = np.array(q1) / np.linalg.norm(q1)
		q2 = np.array(q2) / np.linalg.norm(q2)

		# 计算点积（考虑四元数可能是相反方向但表示相同旋转）
		dot = np.dot(q1, q2)
		dot = np.clip(dot, -1.0, 1.0) # 确保数值稳定性

		# 计算夹角（弧度）
		angle = 2 * math.acos(abs(dot))

		return math.degrees(angle)


		def get_RotationAngle_twoQuaternion_array(q_array, q_old):
		"""
		四元数数组和一个四元数

		参数:
		q_array 四元数数字,np.array([w1,x1,y1,z1],[w2,x2,y2,z2],[w3,x3,y3,z3])
		q_old -- 输入的四元数，格式为 [w, x, y, z]
		必须都是单位四元数
		返回:
		两个四元数之间的夹角（弧度）
		"""
		# 确保四元数是单位四元数
		# q1 = np.array(q1) / np.linalg.norm(q1)
		# q2 = np.array(q2) / np.linalg.norm(q2)

		# 计算点积（考虑四元数可能是相反方向但表示相同旋转）
		dot = q_array @ q_old
		dot = np.clip(dot, -1.0, 1.0) # 确保数值稳定性

		# 计算夹角（弧度）
		angle = 2 * np.arccos(abs(dot))

		return angleM_1_PI180


		def get_RotationAngle_twoQuaternion(q_old=[1., 0., 0., 0.], q_new=[1., 0., 0., 0.]):
		q_old_conj = quaternion_conjugate(q_old)
		q_final = quaternion_multiply(q_new, q_old_conj)
		@@ -734,27 +824,33 @@ def calculateTransTime(transAngle=1.0, surveyCons=None):
		tTime = angleVStime[1, -1]
		return tTime + surveyCons.SHUTTER_TIME * 2.0

		# int i = 0;
		# double tTime = 0;

		# // if(transAngle > 180) {
		# // printf("%f \n",transAngle);
		# // }

		# if(transAngle < 1) {
		# tTime = 70;
		# } else if(transAngle == 1) {
		# tTime = 80;
		# } else {
		# for(i = 0; i < 3 ; i ++) {
		# if(transAngle>data[i][0] && transAngle <= data[i + 1][0] ) {
		# tTime = data[i][1] * ((transAngle - data[i+1][0])) / (((data[i][0] - data[i+1][0])))
		# + data[i+1][1] * ((transAngle - data[i][0])) / (((data[i+1][0] - data[i][0])));
		# break;
		# }
		# }
		# }
		# return tTime + SHUTTER_TIME*2.0; // 此处增加了快门打开和关闭所需要的时间 @2018-11-06


		# endif

		def calculateTransTime_timeArray(transAngle=np.array([1., 3.,]), surveyCons=None):

		# //double data[9][2] = { {0.5, 70},{1, 80}, {5, 95},
		# //{ 10, 105 }, { 15, 115 }, { 20, 120 }, { 30, 135 }, { 45, 150 },{180,200} };

		# //double data[4][2] = { {0.1, 70},{1, 80}, {45, 161},{180,200} };
		# double data[4][2] = { {1, 80}, {20,127},{45, 196},{180,581} };
		# // double data[4][2] = { {1, 45}, {20,92},{45, 196},{180,581} }; // 减少稳定时间
		# // double data[4][2] = { {1, 76}, {20,123},{45, 192},{180,577} };
		# // double data[3][2] = { {1, 80}, {45,170},{180,445} };
		timeResult = np.zeros_like(transAngle)
		angleVStime = np.array([[1, 20, 45, 180], [80, 127, 196, 581]])

		ids1 = transAngle < angleVStime[0, 0]
		timeResult[ids1] = 70
		ids1 = transAngle == angleVStime[0, 0]
		timeResult[ids1] = angleVStime[1, 0]

		for i in np.arange(1, 4, 1):
		ids1 = (transAngle > angleVStime[0, i-1]
		) & (transAngle <= angleVStime[0, i])
		timeResult[ids1] = angleVStime[1, i-1] * ((transAngle[ids1] - angleVStime[0, i])) / (((angleVStime[0, i-1] - angleVStime[0, i]))) + \
		angleVStime[1, i] * ((transAngle[ids1] - angleVStime[0, i-1])) / \
		(((angleVStime[0, i] - angleVStime[0, i-1])))

		ids1 = transAngle > angleVStime[0, -1]
		timeResult[ids1] = angleVStime[1, -1]
		timeResult = timeResult + surveyCons.SHUTTER_TIME * 2.0
		return timeResult

survey_sim/constraints/obscure_constraint.py

+68 −24

Original line number	Diff line number	Diff line
		@@ -14,6 +14,8 @@ from survey_sim.ephemeris import locate_sun

		from survey_sim.satOrbit import locateSat, loadSatOrbitDat

		from survey_sim.constraints import _utils

		MAX_VAL = 100000

		"""
		@@ -156,7 +158,7 @@ def IsObscureByEarth(sat=None, sun=None, p=None, constr=None):
		# 完全阳照区
		if isInSunSide == 1:
		angleValue = withLocalZenithAngle
		if withLocalZenithAngle >= constr.satZenith_angle_light_min_cos:
		if withLocalZenithAngle >= constr.satZenith_angle_light_max_cos:
		isObscure = 0
		else:
		isObscure = 1
		@@ -169,32 +171,13 @@ def IsObscureByEarth(sat=None, sun=None, p=None, constr=None):
		isObscure = 1
		# 有可能阳照区、有可能阴影区,后面需要再看看是否可以优化这部分？？？？？
		else:
		sat0 = sat[0] / modSat
		sat1 = sat[1] / modSat
		sat2 = sat[2] / modSat

		# sat_sp_sat = sat0 * sat0 + sat1 * sat1 + sat2 * sat2
		sat_sp_sat = 1.0
		sat_sp_point = sat0 * p[0] + sat1 * p[1] + sat2 * p[2]

		v1[0] = p[0] * sat_sp_sat - sat0 * sat_sp_point
		v1[1] = p[1] * sat_sp_sat - sat1 * sat_sp_point
		v1[2] = p[2] * sat_sp_sat - sat2 * sat_sp_point

		v1mod = np.sqrt(v1[0] * v1[0] + v1[1] * v1[1] + v1[2] * v1[2])
		v1[0] = v1[0] / v1mod
		v1[1] = v1[1] / v1mod
		v1[2] = v1[2] / v1mod

		v2[0] = tanPE * v1[0] + sat0
		v2[1] = tanPE * v1[1] + sat1
		v2[2] = tanPE * v1[2] + sat2
		p_to_sun_cos = (p[0] * sun[0] + p[1] * sun[1] + p[2] * sun[2]) / (
		modPoint * modSun)

		sun_sp_v2 = sun[0] * v2[0] + sun[1] * v2[1] + sun[2] * v2[2]

		if sun_sp_v2 >= 0:
		if p_to_sun_cos >= 0:
		angleValue = withLocalZenithAngle
		if withLocalZenithAngle >= constr.satZenith_angle_light_min_cos:
		if withLocalZenithAngle >= constr.satZenith_angle_light_max_cos:
		isObscure = 0
		else:
		isObscure = 1
		@@ -204,6 +187,42 @@ def IsObscureByEarth(sat=None, sun=None, p=None, constr=None):
		isObscure = 0
		else:
		isObscure = 1

		# sat0 = sat[0] / modSat
		# sat1 = sat[1] / modSat
		# sat2 = sat[2] / modSat

		# # sat_sp_sat = sat0 * sat0 + sat1 * sat1 + sat2 * sat2
		# sat_sp_sat = 1.0
		# sat_sp_point = sat0 * p[0] + sat1 * p[1] + sat2 * p[2]

		# v1[0] = p[0] * sat_sp_sat - sat0 * sat_sp_point
		# v1[1] = p[1] * sat_sp_sat - sat1 * sat_sp_point
		# v1[2] = p[2] * sat_sp_sat - sat2 * sat_sp_point

		# v1mod = np.sqrt(v1[0] * v1[0] + v1[1] * v1[1] + v1[2] * v1[2])
		# v1[0] = v1[0] / v1mod
		# v1[1] = v1[1] / v1mod
		# v1[2] = v1[2] / v1mod

		# v2[0] = tanPE * v1[0] + sat0
		# v2[1] = tanPE * v1[1] + sat1
		# v2[2] = tanPE * v1[2] + sat2

		# sun_sp_v2 = sun[0] * v2[0] + sun[1] * v2[1] + sun[2] * v2[2]

		# if sun_sp_v2 >= 0:
		# angleValue = withLocalZenithAngle
		# if withLocalZenithAngle >= constr.satZenith_angle_light_min_cos:
		# isObscure = 0
		# else:
		# isObscure = 1
		# else:
		# angleValue = MAX_VAL
		# if withLocalZenithAngle >= constr.satZenith_angle_dark_cos:
		# isObscure = 0
		# else:
		# isObscure = 1
		return isObscure, angleValue


		@@ -305,3 +324,28 @@ def IsObscureByEarth_firstCut_array(sat=None, p=None):
		)

		return withLocalZenithAngle >= 0


		#
		def IsObscureSolarPlane(skymap_ids=None, sun_ecl_car=[0., 0., 0.], dist_sun=1.5e8, skyMap=None, cos_sun_plane_angle=-1.0):
		satPlane_norms = [skyMap.skymap[k].solar_plane_norm for k in skymap_ids]
		value_sun_angle_to_nomals = np.dot(satPlane_norms, sun_ecl_car)/dist_sun
		norm_ids = value_sun_angle_to_nomals < 0
		value_sun_angle_to_nomals[norm_ids] = - \
		1 * value_sun_angle_to_nomals[norm_ids]

		norm_ids1 = value_sun_angle_to_nomals >= cos_sun_plane_angle

		for id in skymap_ids[norm_ids & norm_ids1]:
		obseveSkyUnit = skyMap.skymap[id]
		curSatQ = _utils.rotate_quaternion_byself(
		lon=obseveSkyUnit.ecl_lon, lat=obseveSkyUnit.ecl_lat, pa=180., quaternion=obseveSkyUnit.p_quaternion)
		obseveSkyUnit.p_quaternion = curSatQ
		# for i, id in enumerate(skymap_ids[norm_ids1]):
		# # skyMap.skymap[id].cos_sun_planeNorm = value_sun_angle_to_nomals[norm_ids1][i]
		# obseveSkyUnit = skyMap.skymap[id]
		# obseveSkyUnit.cos_sun_planeNorm = value_sun_angle_to_nomals[norm_ids1][i]

		# return skymap_ids[norm_ids1]
		# return np.stack((skymap_ids[norm_ids1], value_sun_angle_to_nomals[norm_ids1]), axis=1)
		return skymap_ids[norm_ids1], value_sun_angle_to_nomals[norm_ids1]

survey_sim/constraints/saa_constraint.py

+144 −2

Original line number	Diff line number	Diff line
		@@ -172,8 +172,150 @@ def get_saa_time(startTime=2459778., endTime=2459779, orbitData=None):
		return saa_time_seg_out


		class SAA_constraint(object):
		def __init__(self, startTime=2459769., endTime=2459769.+365, orbitData=None):
		self.startTime = startTime
		self.endTime = endTime
		self.saaTimeSeg = self.get_saa_time(
		self.startTime, self.endTime, orbitData)

		def get_saa_time(self, startTime=2459778., endTime=2459779, orbitData=None):
		if orbitData is None:
		return
		orbDataLen = len(orbitData)

		i_start = 0
		i_end = 0

		if startTime < orbitData[0, 0]:
		print("ERROR: start Time is not in the range of orbit data!!!!!!!!!!!!")
		return
		if endTime > orbitData[orbDataLen - 1, 0]:
		print("ERROR: end Time is not in the range of orbit data!!!!!!!!!!!!")
		return

		for i in np.arange(0, orbDataLen, 1):
		t = orbitData[i, 0]
		if startTime < t:
		i_start = i
		break

		for i in np.arange(i_start, orbDataLen, 1):
		t = orbitData[i, 0]
		if endTime < t:
		i_end = i - 1
		break
		if i_end < i_start:
		print(
		"ERROR: the time between start and end is tooooo short, it must be large than the data interval (about 120s)"
		)
		return
		tmp_orbit_len = i_end + 1 - i_start + 2
		tmp_orbitData = np.zeros([tmp_orbit_len, 4])
		sat_start, _, nid = locateSat(
		time=startTime, OrbitData=orbitData, orbDataLen=orbDataLen
		)
		sat_end, _, _ = locateSat(
		time=endTime,
		OrbitData=orbitData,
		startId=nid,
		orbDataLen=orbDataLen,
		)
		tmp_orbitData[0, :] = np.array(
		[startTime, sat_start[0], sat_start[1], sat_start[2]]
		)
		tmp_orbitData[-1, :] = np.array(
		[endTime, sat_end[0], sat_end[1], sat_end[2]]
		)
		tmp_orbitData[1:-1] = orbitData[i_start: i_end + 1, 0:4]

		orbitSegStartIds = [0]

		for i in np.arange(1, tmp_orbit_len, 1):
		t1 = tmp_orbitData[i, 0]
		t2 = tmp_orbitData[i - 1, 0]
		if t1 - t2 > 0.5:
		orbitSegStartIds.append(i)

		saa_time_seg = []
		# underStarPoints = []

		segNum = len(orbitSegStartIds)
		orbitDataId = 0
		in_saa = 0
		for k in np.arange(len(orbitSegStartIds)):
		seg_start_i = orbitSegStartIds[k]
		if k + 1 < segNum:
		seg_end_i = orbitSegStartIds[k + 1]
		else:
		seg_end_i = tmp_orbit_len

		in_beta = 0
		time_seg_start = 0
		time_seg_end = 0

		for i in np.arange(seg_start_i, seg_end_i - 1, 1):

		t1 = tmp_orbitData[i, 0]
		t2 = tmp_orbitData[i + 1, 0]
		if t2 - t1 < 1.1574074074074074e-08: # 1ms
		continue
		curTime = 0.5 * (t1 + t2)

		sat, _, orbitDataId = locateSat(
		time=curTime, OrbitData=orbitData, startId=orbitDataId
		)

		underStarPoint = _utils.getSatSubpoint(sat, curTime)

		in_saa_n = IsInSAA(uderStarPoint=underStarPoint)
		# underStarPoints.append(underStarPoint)
		# print(t1, t2, (t2 - t1) * 86400, in_beta, sunAngle)
		if in_saa_n == 1:
		if in_saa == 0:
		time_seg_start = t1
		in_saa = 1
		time_seg_end = t1
		else:
		# elif np.fabs(sunAngle) >= surveyCosntraint.BETA_ANGLE:
		if in_saa == 1:
		time_seg_end = t1
		in_saa = 0
		saa_time_seg.append([time_seg_start, time_seg_end])

		if in_saa == 1:
		saa_time_seg.append([time_seg_start, time_seg_end])

		if not saa_time_seg:
		return np.array([])

		saa_time_seg = np.array(saa_time_seg)
		saa_time = saa_time_seg[:, 1] - saa_time_seg[:, 0]

		d_ids = np.where(saa_time < 0.0006944444444444445)

		d_ids_flat = np.hstack((d_ids[0] * 2, d_ids[0] * 2 + 1))

		saa_time_seg_d_flat = np.delete(saa_time_seg, d_ids_flat)
		saa_time_seg_out = saa_time_seg_d_flat.reshape(
		saa_time_seg.shape[0] - d_ids[0].shape[0], 2
		)
		return saa_time_seg_out

		def get_survey_time_segment_MSC(self):
		MSC_time = np.hstack(
		(np.array(self.startTime), self.saaTimeSeg.flatten()))
		MSC_time = np.hstack((MSC_time, np.array(self.endTime)))

		self.MSC_time = MSC_time.reshape(self.saaTimeSeg.shape[0] + 1, 2)
		# return MSC_time


		def test():
		orbitDat = loadSatOrbitDat()

		saaSeg = get_saa_time(
		startTime=2459769., endTime=2459769.+365, orbitData=orbitDat)
		# saaSeg = get_saa_time(
		# startTime=2459769., endTime=2459769.+365, orbitData=orbitDat)

		saaCon = SAA_constraint(
		startTime=2459769., endTime=2459769.+10, orbitData=orbitDat)

survey_sim/skyField/skyPatchUnit.py

+3 −0

Original line number	Diff line number	Diff line
		@@ -45,6 +45,9 @@ class SkyPatchUnit(object):
		[0., 0., 1.], self.p_quaternion)

		self.sat_Attitude = self.p_quaternion
		self.solar_plane_norm = -self.p_local_y_axis
		# self.cos_sun_planeNorm = 0.0 # 太阳和帆板法线的夹角，这里只是为了记录方便

		# equ_cor = ecl_cor.transform_to("icrs") # 转换到icrs下ra，dec
		# self.ra = equ_cor.ra.value # 赤经 J2000 太阳系质心坐标系ICRS
		# self.dec = equ_cor.dec.value # 赤纬 J2000 太阳系质心坐标系ICRS

survey_sim/strategies/SurveySim_MSC_mpi.py

+369 −203

File changed.

Preview size limit exceeded, changes collapsed.