nyx_space/dynamics/
orbital.rs

1/*
2    Nyx, blazing fast astrodynamics
3    Copyright (C) 2018-onwards Christopher Rabotin <christopher.rabotin@gmail.com>
4
5    This program is free software: you can redistribute it and/or modify
6    it under the terms of the GNU Affero General Public License as published
7    by the Free Software Foundation, either version 3 of the License, or
8    (at your option) any later version.
9
10    This program is distributed in the hope that it will be useful,
11    but WITHOUT ANY WARRANTY; without even the implied warranty of
12    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13    GNU Affero General Public License for more details.
14
15    You should have received a copy of the GNU Affero General Public License
16    along with this program.  If not, see <https://www.gnu.org/licenses/>.
17*/
18
19use super::{
20    AccelModel, DynamicsAlmanacSnafu, DynamicsAstroSnafu, DynamicsError, DynamicsPlanetarySnafu,
21};
22use crate::cosmic::{AstroPhysicsSnafu, Frame, Orbit};
23use crate::linalg::{Const, Matrix3, Matrix6, OVector, Vector3, Vector6};
24
25use anise::almanac::Almanac;
26use anise::astro::Aberration;
27use hyperdual::linalg::norm;
28use hyperdual::{extract_jacobian_and_result, hyperspace_from_vector, Float, OHyperdual};
29use snafu::ResultExt;
30use std::f64;
31use std::fmt;
32use std::sync::Arc;
33
34pub use super::sph_harmonics::Harmonics;
35
36/// `OrbitalDynamics` provides the equations of motion for any celestial dynamic, without state transition matrix computation.
37#[derive(Clone)]
38pub struct OrbitalDynamics {
39    pub accel_models: Vec<Arc<dyn AccelModel + Sync>>,
40}
41
42impl OrbitalDynamics {
43    /// Initializes the point masses gravities with the provided list of bodies
44    pub fn point_masses(celestial_objects: Vec<i32>) -> Self {
45        // Create the point masses
46        Self::new(vec![PointMasses::new(celestial_objects)])
47    }
48
49    /// Initializes a OrbitalDynamics which does not simulate the gravity pull of other celestial objects but the primary one.
50    pub fn two_body() -> Self {
51        Self::new(vec![])
52    }
53
54    /// Initialize orbital dynamics with a list of acceleration models
55    pub fn new(accel_models: Vec<Arc<dyn AccelModel + Sync>>) -> Self {
56        Self { accel_models }
57    }
58
59    /// Initialize new orbital mechanics with the provided model.
60    /// **Note:** Orbital dynamics _always_ include two body dynamics, these cannot be turned off.
61    pub fn from_model(accel_model: Arc<dyn AccelModel + Sync>) -> Self {
62        Self::new(vec![accel_model])
63    }
64}
65
66impl fmt::Display for OrbitalDynamics {
67    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
68        let models: Vec<String> = self.accel_models.iter().map(|x| format!("{x}")).collect();
69        write!(f, "Orbital dynamics: {}", models.join("; "))
70    }
71}
72
73impl OrbitalDynamics {
74    pub(crate) fn eom(
75        &self,
76        osc: &Orbit,
77        almanac: Arc<Almanac>,
78    ) -> Result<OVector<f64, Const<42>>, DynamicsError> {
79        // Still return something of size 42, but the STM will be zeros.
80        let body_acceleration = (-osc
81            .frame
82            .mu_km3_s2()
83            .context(AstroPhysicsSnafu)
84            .context(DynamicsAstroSnafu)?
85            / osc.rmag_km().powi(3))
86            * osc.radius_km;
87
88        let mut d_x = Vector6::from_iterator(
89            osc.velocity_km_s
90                .iter()
91                .chain(body_acceleration.iter())
92                .cloned(),
93        );
94
95        // Apply the acceleration models
96        for model in &self.accel_models {
97            let model_acc = model.eom(osc, almanac.clone())?;
98            for i in 0..3 {
99                d_x[i + 3] += model_acc[i];
100            }
101        }
102
103        Ok(OVector::<f64, Const<42>>::from_iterator(
104            d_x.iter()
105                .chain(OVector::<f64, Const<36>>::zeros().iter())
106                .cloned(),
107        ))
108    }
109
110    pub fn dual_eom(
111        &self,
112        _delta_t_s: f64,
113        osc: &Orbit,
114        almanac: Arc<Almanac>,
115    ) -> Result<(Vector6<f64>, Matrix6<f64>), DynamicsError> {
116        // Extract data from hyperspace
117        // Build full state vector with partials in the right position (hence building with all six components)
118        let state: Vector6<OHyperdual<f64, Const<7>>> =
119            hyperspace_from_vector(&osc.to_cartesian_pos_vel());
120
121        let radius = state.fixed_rows::<3>(0).into_owned();
122        let velocity = state.fixed_rows::<3>(3).into_owned();
123
124        // Code up math as usual
125        let rmag = norm(&radius);
126        let body_acceleration = radius
127            * (OHyperdual::<f64, Const<7>>::from_real(
128                -osc.frame
129                    .mu_km3_s2()
130                    .context(AstroPhysicsSnafu)
131                    .context(DynamicsAstroSnafu)?,
132            ) / rmag.powi(3));
133
134        // Extract result into Vector6 and Matrix6
135        let mut dx = Vector6::zeros();
136        let mut grad = Matrix6::zeros();
137        for i in 0..6 {
138            dx[i] = if i < 3 {
139                velocity[i].real()
140            } else {
141                body_acceleration[i - 3].real()
142            };
143            for j in 1..7 {
144                grad[(i, j - 1)] = if i < 3 {
145                    velocity[i][j]
146                } else {
147                    body_acceleration[i - 3][j]
148                };
149            }
150        }
151
152        // Apply the acceleration models
153        for model in &self.accel_models {
154            let (model_acc, model_grad) = model.dual_eom(osc, almanac.clone())?;
155            for i in 0..3 {
156                dx[i + 3] += model_acc[i];
157                for j in 1..4 {
158                    grad[(i + 3, j - 1)] += model_grad[(i, j - 1)];
159                }
160            }
161        }
162
163        // This function returns the time derivative of each function. The propagator will add this to the state vector (which has the previous STM).
164        // This is why we don't multiply the gradient (A matrix) with the previous STM
165        Ok((dx, grad))
166    }
167}
168
169/// PointMasses model
170pub struct PointMasses {
171    pub celestial_objects: Vec<i32>,
172    /// Light-time correction computation if extra point masses are needed
173    pub correction: Option<Aberration>,
174}
175
176impl PointMasses {
177    /// Initializes the point masses gravities with the provided list of bodies
178    pub fn new(celestial_objects: Vec<i32>) -> Arc<Self> {
179        Arc::new(Self {
180            celestial_objects,
181            correction: None,
182        })
183    }
184
185    /// Initializes the point masses gravities with the provided list of bodies, and accounting for some light time correction
186    pub fn with_correction(celestial_objects: Vec<i32>, correction: Aberration) -> Self {
187        Self {
188            celestial_objects,
189            correction: Some(correction),
190        }
191    }
192}
193
194impl fmt::Display for PointMasses {
195    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
196        let masses: Vec<String> = self
197            .celestial_objects
198            .iter()
199            .map(|third_body| format!("{}", Frame::from_ephem_j2000(*third_body)))
200            .collect();
201        write!(f, "Point masses of {}", masses.join(", "))
202    }
203}
204
205impl AccelModel for PointMasses {
206    fn eom(&self, osc: &Orbit, almanac: Arc<Almanac>) -> Result<Vector3<f64>, DynamicsError> {
207        let mut d_x = Vector3::zeros();
208        // Get all of the position vectors between the center body and the third bodies
209        for third_body in self.celestial_objects.iter().copied() {
210            if osc.frame.ephem_origin_id_match(third_body) {
211                // Ignore the contribution of the integration frame, that's handled by OrbitalDynamics
212                continue;
213            }
214
215            let third_body_frame = almanac
216                .frame_from_uid(osc.frame.with_ephem(third_body))
217                .context(DynamicsPlanetarySnafu {
218                    action: "planetary data from third body not loaded",
219                })?;
220
221            // Orbit of j-th body as seen from primary body
222            let st_ij = almanac
223                .transform(third_body_frame, osc.frame, osc.epoch, self.correction)
224                .context(DynamicsAlmanacSnafu {
225                    action: "computing third body gravitational pull",
226                })?;
227
228            let r_ij = st_ij.radius_km;
229            let r_ij3 = st_ij.rmag_km().powi(3);
230            let r_j = osc.radius_km - r_ij; // sc as seen from 3rd body
231            let r_j3 = r_j.norm().powi(3);
232            d_x += -third_body_frame
233                .mu_km3_s2()
234                .context(AstroPhysicsSnafu)
235                .context(DynamicsAstroSnafu)?
236                * (r_j / r_j3 + r_ij / r_ij3);
237        }
238        Ok(d_x)
239    }
240
241    fn dual_eom(
242        &self,
243        osc: &Orbit,
244        almanac: Arc<Almanac>,
245    ) -> Result<(Vector3<f64>, Matrix3<f64>), DynamicsError> {
246        // Build the hyperdual space of the radius vector
247        let radius: Vector3<OHyperdual<f64, Const<7>>> = hyperspace_from_vector(&osc.radius_km);
248        // Extract result into Vector6 and Matrix6
249        let mut fx = Vector3::zeros();
250        let mut grad = Matrix3::zeros();
251
252        // Get all of the position vectors between the center body and the third bodies
253        for third_body in &self.celestial_objects {
254            let third_body_frame = almanac
255                .frame_from_uid(Frame::from_ephem_j2000(*third_body))
256                .context(DynamicsPlanetarySnafu {
257                    action: "planetary data from third body not loaded",
258                })?;
259
260            if osc.frame.ephem_origin_match(third_body_frame) {
261                // Ignore the contribution of the integration frame, that's handled by OrbitalDynamics
262                continue;
263            }
264
265            let gm_d = OHyperdual::<f64, Const<7>>::from_real(
266                -third_body_frame
267                    .mu_km3_s2()
268                    .context(AstroPhysicsSnafu)
269                    .context(DynamicsAstroSnafu)?,
270            );
271
272            // Orbit of j-th body as seen from primary body
273            let st_ij = almanac
274                .transform(third_body_frame, osc.frame, osc.epoch, self.correction)
275                .context(DynamicsAlmanacSnafu {
276                    action: "computing third body gravitational pull",
277                })?;
278
279            let r_ij: Vector3<OHyperdual<f64, Const<7>>> = hyperspace_from_vector(&st_ij.radius_km);
280            let r_ij3 = norm(&r_ij).powi(3);
281
282            // The difference leads to the dual parts nulling themselves out, so let's fix that.
283            let mut r_j = radius - r_ij; // sc as seen from 3rd body
284            r_j[0][1] = 1.0;
285            r_j[1][2] = 1.0;
286            r_j[2][3] = 1.0;
287
288            let r_j3 = norm(&r_j).powi(3);
289            let mut third_body_acc_d = r_j / r_j3 + r_ij / r_ij3;
290            third_body_acc_d[0] *= gm_d;
291            third_body_acc_d[1] *= gm_d;
292            third_body_acc_d[2] *= gm_d;
293
294            let (fxp, gradp) = extract_jacobian_and_result::<_, 3, 3, 7>(&third_body_acc_d);
295            fx += fxp;
296            grad += gradp;
297        }
298
299        Ok((fx, grad))
300    }
301}
nyx_space/dynamics/orbital.rs

nyx_space/dynamics/
orbital.rs
