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

nyx_space/md/trajectory/
interpolatable.rs
