nyx_space/md/trajectory/
interpolatable.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 anise::analysis::prelude::OrbitalElement;
20use anise::math::interpolation::{hermite_eval, InterpolationError};
21
22pub(crate) const INTERPOLATION_SAMPLES: usize = 13;
23
24use super::StateParameter;
25use crate::cosmic::Frame;
26use crate::linalg::allocator::Allocator;
27use crate::linalg::DefaultAllocator;
28use crate::time::Epoch;
29use crate::{Orbit, Spacecraft, State};
30
31/// States that can be interpolated should implement this trait.
32pub trait Interpolatable: State
33where
34    Self: Sized,
35    DefaultAllocator:
36        Allocator<Self::Size> + Allocator<Self::Size, Self::Size> + Allocator<Self::VecLength>,
37{
38    /// Interpolates a new state at the provided epochs given a slice of states.
39    fn interpolate(self, epoch: Epoch, states: &[Self]) -> Result<Self, InterpolationError>;
40
41    /// Returns the frame of this state
42    fn frame(&self) -> Frame;
43
44    /// Sets the frame of this state
45    fn set_frame(&mut self, frame: Frame);
46
47    /// List of state parameters that will be exported to a trajectory file in addition to the epoch (provided in this different formats).
48    fn export_params() -> Vec<StateParameter>;
49}
50
51impl Interpolatable for Spacecraft {
52    fn interpolate(mut self, epoch: Epoch, states: &[Self]) -> Result<Self, InterpolationError> {
53        // Interpolate the Orbit first
54        // Statically allocated arrays of the maximum number of samples
55        let mut epochs_tdb = [0.0; INTERPOLATION_SAMPLES];
56        let mut xs = [0.0; INTERPOLATION_SAMPLES];
57        let mut ys = [0.0; INTERPOLATION_SAMPLES];
58        let mut zs = [0.0; INTERPOLATION_SAMPLES];
59        let mut vxs = [0.0; INTERPOLATION_SAMPLES];
60        let mut vys = [0.0; INTERPOLATION_SAMPLES];
61        let mut vzs = [0.0; INTERPOLATION_SAMPLES];
62
63        for (cno, state) in states.iter().enumerate() {
64            xs[cno] = state.orbit.radius_km.x;
65            ys[cno] = state.orbit.radius_km.y;
66            zs[cno] = state.orbit.radius_km.z;
67            vxs[cno] = state.orbit.velocity_km_s.x;
68            vys[cno] = state.orbit.velocity_km_s.y;
69            vzs[cno] = state.orbit.velocity_km_s.z;
70            epochs_tdb[cno] = state.epoch().to_et_seconds();
71        }
72
73        // Ensure that if we don't have enough states, we only interpolate using what we have instead of INTERPOLATION_SAMPLES
74        let n = states.len();
75
76        let (x_km, vx_km_s) =
77            hermite_eval(&epochs_tdb[..n], &xs[..n], &vxs[..n], epoch.to_et_seconds())?;
78
79        let (y_km, vy_km_s) =
80            hermite_eval(&epochs_tdb[..n], &ys[..n], &vys[..n], epoch.to_et_seconds())?;
81
82        let (z_km, vz_km_s) =
83            hermite_eval(&epochs_tdb[..n], &zs[..n], &vzs[..n], epoch.to_et_seconds())?;
84
85        self.orbit = Orbit::new(
86            x_km,
87            y_km,
88            z_km,
89            vx_km_s,
90            vy_km_s,
91            vz_km_s,
92            epoch,
93            self.orbit.frame,
94        );
95
96        // Fuel is linearly interpolated -- should really be a Lagrange interpolation here
97        let first = states.first().unwrap();
98        let last = states.last().unwrap();
99        let prop_kg_dt = (last.mass.prop_mass_kg - first.mass.prop_mass_kg)
100            / (last.epoch() - first.epoch()).to_seconds();
101
102        self.mass.prop_mass_kg += prop_kg_dt * (epoch - first.epoch()).to_seconds();
103
104        Ok(self)
105    }
106
107    fn frame(&self) -> Frame {
108        self.orbit.frame
109    }
110
111    fn set_frame(&mut self, frame: Frame) {
112        self.orbit.frame = frame;
113    }
114
115    fn export_params() -> Vec<StateParameter> {
116        vec![
117            StateParameter::Element(OrbitalElement::X),
118            StateParameter::Element(OrbitalElement::Y),
119            StateParameter::Element(OrbitalElement::Z),
120            StateParameter::Element(OrbitalElement::VX),
121            StateParameter::Element(OrbitalElement::VY),
122            StateParameter::Element(OrbitalElement::VZ),
123            StateParameter::Element(OrbitalElement::SemiMajorAxis),
124            StateParameter::Element(OrbitalElement::Eccentricity),
125            StateParameter::Element(OrbitalElement::Inclination),
126            StateParameter::Element(OrbitalElement::RAAN),
127            StateParameter::Element(OrbitalElement::AoP),
128            StateParameter::Element(OrbitalElement::TrueAnomaly),
129            StateParameter::Element(OrbitalElement::AoL),
130            StateParameter::Element(OrbitalElement::TrueLongitude),
131            StateParameter::DryMass,
132            StateParameter::PropMass,
133            StateParameter::Cr,
134            StateParameter::Cd,
135            StateParameter::Isp,
136            StateParameter::GuidanceMode,
137            StateParameter::Thrust,
138        ]
139    }
140}
nyx_space/md/trajectory/interpolatable.rs

nyx_space/md/trajectory/
interpolatable.rs
