|
| 1 | +use peroxide::fuga::*; |
| 2 | + |
| 3 | +pub const MU: f64 = 398600.4418; // Standard gravitational parameter of Earth |
| 4 | +pub const R_EARTH: f64 = 6378.137; // Radius of Earth in km |
| 5 | +pub const J2: f64 = 1.08262668e-3; // J2 coefficient of Earth |
| 6 | + |
| 7 | +fn main() -> Result<(), Box<dyn std::error::Error>> { |
| 8 | + let selected_orbit = OrbitType::Molniya.create_orbit(); |
| 9 | + let initial_state = selected_orbit.initial_state(); |
| 10 | + |
| 11 | + let t0 = 0.0; |
| 12 | + let tf = 86400.0; |
| 13 | + let dt = 60.0; |
| 14 | + |
| 15 | + let problem = KeplerProblem; |
| 16 | + let gl4 = GL4 { |
| 17 | + solver: ImplicitSolver::FixedPoint, |
| 18 | + tol: 1e-10, |
| 19 | + max_step_iter: 100, |
| 20 | + }; |
| 21 | + let rk4 = RK4; |
| 22 | + |
| 23 | + let gl4_solver = BasicODESolver::new(gl4); |
| 24 | + let rk4_solver = BasicODESolver::new(rk4); |
| 25 | + |
| 26 | + let y0 = Vec::from(initial_state); |
| 27 | + y0.print(); |
| 28 | + let (t, y_gl4) = gl4_solver.solve( |
| 29 | + &problem, |
| 30 | + (t0, tf), |
| 31 | + dt, |
| 32 | + &y0, |
| 33 | + )?; |
| 34 | + let (_, y_rk4) = rk4_solver.solve( |
| 35 | + &problem, |
| 36 | + (t0, tf), |
| 37 | + dt, |
| 38 | + &y0, |
| 39 | + )?; |
| 40 | + |
| 41 | + let y_gl4 = py_matrix(y_gl4); |
| 42 | + let y_rk4 = py_matrix(y_rk4); |
| 43 | + |
| 44 | + let mut df = DataFrame::new(vec![]); |
| 45 | + df.push("t", Series::new(t)); |
| 46 | + df.push("x_gl4", Series::new(y_gl4.col(0))); |
| 47 | + df.push("y_gl4", Series::new(y_gl4.col(1))); |
| 48 | + df.push("z_gl4", Series::new(y_gl4.col(2))); |
| 49 | + df.push("vx_gl4", Series::new(y_gl4.col(3))); |
| 50 | + df.push("vy_gl4", Series::new(y_gl4.col(4))); |
| 51 | + df.push("vz_gl4", Series::new(y_gl4.col(5))); |
| 52 | + df.push("x_rk4", Series::new(y_rk4.col(0))); |
| 53 | + df.push("y_rk4", Series::new(y_rk4.col(1))); |
| 54 | + df.push("z_rk4", Series::new(y_rk4.col(2))); |
| 55 | + df.push("vx_rk4", Series::new(y_rk4.col(3))); |
| 56 | + df.push("vy_rk4", Series::new(y_rk4.col(4))); |
| 57 | + df.push("vz_rk4", Series::new(y_rk4.col(5))); |
| 58 | + |
| 59 | + df.print(); |
| 60 | + |
| 61 | + Ok(()) |
| 62 | +} |
| 63 | + |
| 64 | +pub struct KeplerProblem; |
| 65 | + |
| 66 | +impl ODEProblem for KeplerProblem { |
| 67 | + fn rhs(&self, _t: f64, y: &[f64], dy: &mut [f64]) -> anyhow::Result<()> { |
| 68 | + let state = State::from(y.to_vec()); |
| 69 | + let r = state.r(); |
| 70 | + let r3 = r.powi(3); |
| 71 | + |
| 72 | + dy[0] = state.vx; |
| 73 | + dy[1] = state.vy; |
| 74 | + dy[2] = state.vz; |
| 75 | + dy[3] = -MU * state.x / r3; |
| 76 | + dy[4] = -MU * state.y / r3; |
| 77 | + dy[5] = -MU * state.z / r3; |
| 78 | + |
| 79 | + Ok(()) |
| 80 | + } |
| 81 | +} |
| 82 | + |
| 83 | +#[derive(Debug, Clone, Copy)] |
| 84 | +pub struct State { |
| 85 | + pub x: f64, |
| 86 | + pub y: f64, |
| 87 | + pub z: f64, |
| 88 | + pub vx: f64, |
| 89 | + pub vy: f64, |
| 90 | + pub vz: f64, |
| 91 | +} |
| 92 | + |
| 93 | +impl State { |
| 94 | + pub fn r(&self) -> f64 { |
| 95 | + (self.x.powi(2) + self.y.powi(2) + self.z.powi(2)).sqrt() |
| 96 | + } |
| 97 | +} |
| 98 | + |
| 99 | +impl From<Vec<f64>> for State { |
| 100 | + fn from(v: Vec<f64>) -> Self { |
| 101 | + State { |
| 102 | + x: v[0], |
| 103 | + y: v[1], |
| 104 | + z: v[2], |
| 105 | + vx: v[3], |
| 106 | + vy: v[4], |
| 107 | + vz: v[5], |
| 108 | + } |
| 109 | + } |
| 110 | +} |
| 111 | + |
| 112 | +impl From<State> for Vec<f64> { |
| 113 | + fn from(s: State) -> Self { |
| 114 | + vec![s.x, s.y, s.z, s.vx, s.vy, s.vz] |
| 115 | + } |
| 116 | +} |
| 117 | + |
| 118 | +pub enum OrbitType { |
| 119 | + LEO, |
| 120 | + GEO, |
| 121 | + Molniya, |
| 122 | +} |
| 123 | + |
| 124 | +impl ToString for OrbitType { |
| 125 | + fn to_string(&self) -> String { |
| 126 | + match self { |
| 127 | + OrbitType::LEO => "LEO", |
| 128 | + OrbitType::GEO => "GEO", |
| 129 | + OrbitType::Molniya => "Molniya", |
| 130 | + }.to_string() |
| 131 | + } |
| 132 | +} |
| 133 | + |
| 134 | +impl OrbitType { |
| 135 | + fn create_orbit(&self) -> Orbit { |
| 136 | + match self { |
| 137 | + OrbitType::LEO => Orbit { |
| 138 | + a: R_EARTH + 500.0, |
| 139 | + e: 0.0, |
| 140 | + i: 0.0, |
| 141 | + raan: 0.0, |
| 142 | + w: 0.0, |
| 143 | + ta: 0.0, |
| 144 | + }, |
| 145 | + OrbitType::GEO => Orbit { |
| 146 | + a: R_EARTH + 35786.0, |
| 147 | + e: 0.0, |
| 148 | + i: 0.0, |
| 149 | + raan: 0.0, |
| 150 | + w: 0.0, |
| 151 | + ta: 0.0, |
| 152 | + }, |
| 153 | + OrbitType::Molniya => Orbit { |
| 154 | + a: R_EARTH + 26600.0, |
| 155 | + e: 0.74, |
| 156 | + i: 63.4f64.to_radians(), |
| 157 | + raan: 0.0, |
| 158 | + w: 270.0f64.to_radians(), |
| 159 | + ta: 0.0, |
| 160 | + } |
| 161 | + } |
| 162 | + } |
| 163 | +} |
| 164 | + |
| 165 | +pub struct Orbit { |
| 166 | + pub a: f64, // Semi-major axis |
| 167 | + pub e: f64, // Eccentricity |
| 168 | + pub i: f64, // Inclination |
| 169 | + pub raan: f64, // Right ascension of ascending node |
| 170 | + pub w: f64, // Argument of perigee |
| 171 | + pub ta: f64, // True anomaly |
| 172 | +} |
| 173 | + |
| 174 | +impl Orbit { |
| 175 | + pub fn r(&self) -> f64 { |
| 176 | + self.a * (1.0 - self.e.powi(2)) / (1.0 + self.e * self.ta.cos()) |
| 177 | + } |
| 178 | + |
| 179 | + #[allow(non_snake_case)] |
| 180 | + pub fn initial_state(&self) -> State { |
| 181 | + let r_pf = vec![ |
| 182 | + self.r() * self.ta.cos(), |
| 183 | + self.r() * self.ta.sin(), |
| 184 | + 0f64 |
| 185 | + ]; |
| 186 | + |
| 187 | + let p_orbit = self.a * (1.0 - self.e.powi(2)); |
| 188 | + let v_pf = vec![ |
| 189 | + - (MU / p_orbit).sqrt() * self.ta.sin(), |
| 190 | + (MU / p_orbit).sqrt() * (self.e + self.ta.cos()), |
| 191 | + 0f64 |
| 192 | + ]; |
| 193 | + |
| 194 | + let Q = perifocal_to_eci_matrix(&self); |
| 195 | + let r_eci = &Q * &r_pf; |
| 196 | + let v_eci = &Q * &v_pf; |
| 197 | + |
| 198 | + State { |
| 199 | + x: r_eci[0], |
| 200 | + y: r_eci[1], |
| 201 | + z: r_eci[2], |
| 202 | + vx: v_eci[0], |
| 203 | + vy: v_eci[1], |
| 204 | + vz: v_eci[2], |
| 205 | + } |
| 206 | + } |
| 207 | +} |
| 208 | + |
| 209 | +pub fn rot_x(theta: f64) -> Matrix { |
| 210 | + let (s, c) = theta.sin_cos(); |
| 211 | + matrix(vec![ |
| 212 | + 1f64, 0f64, 0f64, |
| 213 | + 0f64, c, -s, |
| 214 | + 0f64, s, c |
| 215 | + ], 3, 3, Row) |
| 216 | +} |
| 217 | + |
| 218 | +pub fn rot_z(theta: f64) -> Matrix { |
| 219 | + let (s, c) = theta.sin_cos(); |
| 220 | + matrix(vec![ |
| 221 | + c, -s, 0f64, |
| 222 | + s, c, 0f64, |
| 223 | + 0f64, 0f64, 1f64 |
| 224 | + ], 3, 3, Row) |
| 225 | +} |
| 226 | + |
| 227 | +#[allow(non_snake_case)] |
| 228 | +pub fn perifocal_to_eci_matrix(orbit: &Orbit) -> Matrix { |
| 229 | + let i = orbit.i; |
| 230 | + let raan = orbit.raan; |
| 231 | + let w = orbit.w; |
| 232 | + |
| 233 | + let R3_w = rot_z(w); |
| 234 | + let R1_i = rot_x(i); |
| 235 | + let R3_raan = rot_z(raan); |
| 236 | + |
| 237 | + R3_raan * R1_i * R3_w |
| 238 | +} |
0 commit comments