JMOT.extra

  1import math
  2import numpy as np
  3
  4def tuple2array(vec:tuple[float, float, float])->np.ndarray:
  5    '''Convert a tuple to a numpy array, and swap the second and third elements'''
  6    vec = list(vec)
  7    tmp = vec[1]
  8    vec[1] = vec[2]
  9    vec[2] = tmp
 10    return np.array(vec)
 11
 12def array2tuple(vec:np.ndarray)->tuple[float, float, float]:
 13    '''Convert a numpy array to a tuple, and swap the second and third elements'''
 14    vec = vec.tolist()
 15    tmp = vec[1]
 16    vec[1] = vec[2]
 17    vec[2] = tmp
 18    vec = tuple(vec)
 19    return vec
 20
 21    """
 22    修复 JMOT 底层暴力拆解导致的列表错位问题。
 23    将被切碎的 '[1'、'3)]' 等残骸重新拼接回完整的数字和元组。
 24    """
 25    if not raw_list:
 26        return []
 27        
 28    result = []
 29    i = 0
 30    
 31    while i < len(raw_list):
 32        item = str(raw_list[i])
 33        
 34        # 1. 处理被切断的列表开头：比如 '[1' 
 35        if item.startswith('['):
 36            clean_num = item.lstrip("[").strip("' ")
 37            try:
 38                result.append(float(clean_num))
 39            except ValueError:
 40                result.append(clean_num)
 41                
 42        # 2. 处理被切断的元组：寻找以 '(' 开头但不以 ')' 结尾的碎片
 43        elif item.startswith('(') and not item.endswith(')'):
 44            tuple_parts = [item]
 45            # 一直往后找，直到遇到以 ')' 结尾的碎片
 46            j = i + 1
 47            while j < len(raw_list):
 48                tuple_parts.append(str(raw_list[j]))
 49                if str(raw_list[j]).endswith(')'):
 50                    break
 51                j += 1
 52            
 53            # 把碎片拼起来，例如 "(1" + "2.0" + "3)" -> "(12.03)"
 54            full_tuple_str = "".join(tuple_parts)
 55            
 56            # 提取括号内的内容并转为浮点数元组
 57            inner = full_tuple_str.strip("()")
 58            try:
 59                nums = [float(x.strip()) for x in inner.split(",")]
 60                result.append(tuple(nums))
 61            except ValueError:
 62                result.append(full_tuple_str)  # 转换失败则保留原样
 63                
 64            i = j  # 跳过已经处理过的碎片
 65            
 66        # 3. 处理普通的中间元素（如 "'aaa'" 或 3.14）
 67        else:
 68            clean_item = item.strip("' ")
 69            try:
 70                result.append(float(clean_item))
 71            except ValueError:
 72                result.append(clean_item)
 73                
 74        i += 1
 75        
 76    return result
 77
 78def eci2ecef(r_eci, total_seconds):
 79    '''DO NOT USE THIS FUNCTION, IT IS NOT ACCURATE ENOUGH'''
 80    theta = 2 * np.pi * (total_seconds / 86400)
 81    r_eci = np.array([r_eci[0], r_eci[1], r_eci[2]])
 82    Rz = np.array([
 83        [np.cos(theta), np.sin(theta), 0],
 84        [-np.sin(theta), np.cos(theta), 0],
 85        [0, 0, 1]
 86    ])
 87    
 88    r_ecef = Rz @ r_eci
 89    return r_ecef
 90
 91def lla2ecef(lon, lat, h, R=6371010.0):
 92    '''DO NOT USE THIS FUNCTION, IT IS NOT ACCURATE ENOUGH'''
 93    lon_rad = np.radians(lon)
 94    lat_rad = np.radians(lat)
 95    X = (R + h) * np.cos(lat_rad) * np.cos(lon_rad)
 96    Y = (R + h) * np.cos(lat_rad) * np.sin(lon_rad)
 97    Z = (R + h) * np.sin(lat_rad)
 98    return np.array([X, Y, Z])
 99
100
101def ecef2enu(lon, lat, v_ECEF):
102    '''DO NOT USE THIS FUNCTION, IT IS NOT ACCURATE ENOUGH'''
103    lon = np.radians(lon)
104    lat = np.radians(lat)
105    v_ECEF = np.array([v_ECEF[0], v_ECEF[1], v_ECEF[2]])
106
107    R = np.array([
108        [-np.sin(lon), np.cos(lon), 0],
109        [-np.sin(lat) * np.cos(lon), -np.sin(lat) * np.sin(lon), np.cos(lat)],
110        [np.cos(lat) * np.cos(lon), np.cos(lat) * np.sin(lon), np.sin(lat)]
111    ])
112    
113    return R @ v_ECEF
114
115
116def enu_to_pitch_heading(v_e, v_n, v_u):
117    '''DO NOT USE THIS FUNCTION, IT IS NOT ACCURATE ENOUGH'''
118    heading = np.degrees(np.arctan2(v_e, v_n))
119    
120    norm = np.sqrt(v_e**2 + v_n**2 + v_u**2)
121    pitch = np.degrees(np.arcsin(v_u / norm))
122    
123    return pitch, heading
124
125
126def eci_to_enu_velocity(r_eci, v_eci):
127    '''DO NOT USE THIS FUNCTION, IT IS NOT ACCURATE ENOUGH'''
128    """Convert ECI velocity to ENU velocity."""
129    x, y, z = r_eci
130    vx, vy, vz = v_eci
131    
132    # Compute longitude (lambda) and latitude (phi)
133    lambda_ = math.atan2(y, x)  # Longitude
134    phi = math.atan2(z, math.sqrt(x**2 + y**2))  # Latitude
135    
136    # ECI to ENU rotation matrix
137    R = np.array([
138        [-math.sin(lambda_),           math.cos(lambda_),           0          ],
139        [-math.sin(phi)*math.cos(lambda_), -math.sin(phi)*math.sin(lambda_),  math.cos(phi)   ],
140        [ math.cos(phi)*math.cos(lambda_),  math.cos(phi)*math.sin(lambda_),  math.sin(phi)   ]
141    ])
142    
143    v_enu = R @ np.array([vx, vy, vz])
144    return v_enu
145
146def compute_heading_angle(r_eci, v_eci):
147    """Compute heading angle from ECI position and velocity."""
148    '''DO NOT USE THIS FUNCTION, IT IS NOT ACCURATE ENOUGH'''
149    v_enu = eci_to_enu_velocity(r_eci, v_eci)
150    v_east, v_north, v_up = v_enu
151    
152    heading = math.atan2(v_east, v_north)  # [-π, π]
153    heading_deg = np.degrees(heading) % 360  # [0°, 360°]
154    return heading_deg
155
156def calculate_roll_angle(pitch_axis, roll_axis, gravity_vector):
157    """
158    计算航天器的滚转角（roll angle）。
159    
160    参数:\n
161        pitch_axis: ECI坐标系下的pitch轴矢量 (3维数组)\n
162        roll_axis: ECI坐标系下的roll轴矢量 (3维数组)\n
163        gravity_vector: ECI坐标系下的重力矢量 (3维数组)\n
164    
165    返回:
166        滚转角（弧度），范围 [-pi, pi]
167    """
168    '''DO NOT USE THIS FUNCTION, IT IS NOT ACCURATE ENOUGH'''
169    roll_axis_normalized = roll_axis / np.linalg.norm(roll_axis)
170    gravity_normalized = gravity_vector / np.linalg.norm(gravity_vector)
171    
172    up_direction = -gravity_normalized
173    up_direction = up_direction - np.dot(up_direction, roll_axis_normalized) * roll_axis_normalized
174    up_direction_normalized = up_direction / np.linalg.norm(up_direction)
175    
176    pitch_projection = pitch_axis - np.dot(pitch_axis, roll_axis_normalized) * roll_axis_normalized
177    pitch_projection_normalized = pitch_projection / np.linalg.norm(pitch_projection)
178    
179    right_direction = np.cross(roll_axis_normalized, up_direction_normalized)
180    right_direction_normalized = right_direction / np.linalg.norm(right_direction)
181    
182    roll_angle = np.arctan2(
183        np.dot(pitch_projection_normalized, right_direction_normalized),
184        np.dot(pitch_projection_normalized, up_direction_normalized)
185    )
186    
187    return np.degrees(roll_angle) -90