trajectory

import numpy as np
import plotly.graph_objects as go

class OrbitPlotter:
    def __init__(self):
        self.fig = go.Figure()


    def add_vector(self, start_point, end_point, color='black'):
        # コーンと線の色
        # color = 'blue'

        # ベクトルの方向を求める
        u = end_point[0] - start_point[0]
        v = end_point[1] - start_point[1]
        w = end_point[2] - start_point[2]

        # ベクトルの長さを計算
        length = np.linalg.norm([u, v, w])

        # コーンのサイズ調整（ベクトルの長さに応じてスケーリング）
        cone_size = length * 0.1
        cone_size=300
        if length <= 500:
            cone_size = cone_size * 0.5

        # コーンの追加
        self.fig.add_trace(go.Cone(
            x=[end_point[0]], y=[end_point[1]], z=[end_point[2]],  # コーンの位置（先端）
            u=[u * 0.1], v=[v * 0.1], w=[w * 0.1],  # コーンの向き
            showscale=False, 
            colorscale=[[0, color], [1, color]],  # コーンの色を設定
            # hoverinfo='skip',
            sizemode='absolute',  # 絶対サイズモードに設定
            sizeref=cone_size,  # サイズのスケールを調整
            anchor="tip"  # コーンをベクトルの先端に配置
        ))

        # ベクトル（線分）の追加
        self.fig.add_trace(go.Scatter3d(
            x=[start_point[0], end_point[0]], y=[start_point[1], end_point[1]], z=[start_point[2], end_point[2]],
            mode='lines',
            line=dict(color=color, width=2),  # 線の太さを調整し、色を設定
            hoverinfo='skip',
        ))

    def add_orbit(self, semi_major_axis, eccentricity, inclination, raan, arg_periapsis, true_anomaly, num_points=1000):
        print(type(num_points))
        # 注意: 春分点方向は+Xの方向に一致していると思う
        # semi_major_axis: 長半径: 楕円軌道の長い方の軸の半分の長さで、軌道のサイズを表します。
        # eccentricity: 離心率: 楕円の形状を示す無次元数で、円からの偏り具合を表します。
        # inclination: 軌道傾斜角: 軌道面が基準面（通常は地球の赤道面）に対して傾いている角度。
        # raan: 昇交点赤経: 昇交点（軌道が赤道面を北向きに通過する点）と基準方向（通常は春分点）との間の角度。
        # arg_periapsis: 近日点引数: 軌道の昇交点から近日点までの角度。
        # true_anomaly: 真近点離角: 衛星の現在位置と近日点との間の角度。

        # 平均運動の計算
        mu = 398600.436  # 地球の標準重力定数（km^3/s^2）
        n = np.sqrt(mu / semi_major_axis**3)

        # 時間配列の計算
        periods = 1  # 計算する軌道周期の数
        t = np.linspace(0, periods * 2 * np.pi / n, num_points)

        # ケプラー軌道要素の計算
        mean_anomaly = n * t
        eccentric_anomaly = mean_anomaly.copy()
        for _ in range(10):  # 離心近似の反復計算
            eccentric_anomaly = mean_anomaly + eccentricity * np.sin(eccentric_anomaly)
        true_anomaly_calculated = 2 * np.arctan(np.sqrt((1 + eccentricity) / (1 - eccentricity)) * np.tan(eccentric_anomaly / 2))

        # 軌道面内の位置の計算
        r = semi_major_axis * (1 - eccentricity**2) / (1 + eccentricity * np.cos(true_anomaly_calculated))
        x_orbital_plane = r * np.cos(true_anomaly_calculated)
        y_orbital_plane = r * np.sin(true_anomaly_calculated)

        # 慣性フレームへの回転
        x = x_orbital_plane * (np.cos(arg_periapsis) * np.cos(raan) - np.sin(arg_periapsis) * np.cos(inclination) * np.sin(raan)) - \
            y_orbital_plane * (np.sin(arg_periapsis) * np.cos(raan) + np.cos(arg_periapsis) * np.cos(inclination) * np.sin(raan))
        y = x_orbital_plane * (np.cos(arg_periapsis) * np.sin(raan) + np.sin(arg_periapsis) * np.cos(inclination) * np.cos(raan)) + \
            y_orbital_plane * (np.cos(arg_periapsis) * np.cos(inclination) * np.cos(raan) - np.sin(arg_periapsis) * np.sin(raan))
        z = x_orbital_plane * np.sin(arg_periapsis) * np.sin(inclination) + y_orbital_plane * np.cos(arg_periapsis) * np.sin(inclination)

        # 軌道をプロット
        self.fig.add_trace(go.Scatter3d(
            x=x, y=y, z=z,
            mode='lines',
            name='Orbit'
        ))

# 使用例
plotter = OrbitPlotter()

plotter.add_vector([0,0,0], [5000, 0, 0])

# 軌道パラメータの定義（半長軸、離心率、軌道傾斜角、昇交点赤経、近日点引数、真近点角）
semi_major_axis = 7000  # キロメートル単位
eccentricity = 0.1
inclination = np.radians(60)  # ラジアン単位
raan = np.radians(30)  # ラジアン単位
arg_periapsis = np.radians(45)  # ラジアン単位
true_anomaly = np.radians(0)  # ラジアン単位
# プロットする点の数
num_points = 1000
# 軌道の計算と描写
plotter.add_orbit(semi_major_axis, eccentricity, inclination, raan, arg_periapsis, true_anomaly)


# 軌道パラメータの定義（半長軸、離心率、軌道傾斜角、昇交点赤経、近日点引数、真近点角）
semi_major_axis = 7000  # キロメートル単位
eccentricity = 0.1
inclination = np.radians(60)  # ラジアン単位
raan = np.radians(90)  # ラジアン単位
arg_periapsis = np.radians(0)  # ラジアン単位
true_anomaly = np.radians(0)  # ラジアン単位
# プロットする点の数
num_points = 1000
# 軌道の計算と描写
num_points = 10
plotter.add_orbit(semi_major_axis, eccentricity, inclination, raan, arg_periapsis, true_anomaly)



# プロットの表示
plotter.fig.update_layout(scene=dict(aspectratio=dict(x=1, y=1, z=1)), scene_aspectmode='cube')
plotter.fig.update_layout(
    width=1000,  # グラフの幅と高さの設定
    height=1000,
    hoverlabel_font_size=20
)
plotter.fig.show()