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::dynamics::guidance::LocalFrame;
27use crate::linalg::allocator::Allocator;
28use crate::linalg::DefaultAllocator;
29use crate::time::Epoch;
30use crate::{Orbit, Spacecraft, State};
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        // Thrust direction is a discrete guidance output and should not be interpolated.
105        self.thrust_direction = None;
106
107        Ok(self)
108    }
109
110    fn frame(&self) -> Frame {
111        self.orbit.frame
112    }
113
114    fn set_frame(&mut self, frame: Frame) {
115        self.orbit.frame = frame;
116    }
117
118    fn export_params() -> Vec<StateParameter> {
119        vec![
120            StateParameter::Element(OrbitalElement::X),
121            StateParameter::Element(OrbitalElement::Y),
122            StateParameter::Element(OrbitalElement::Z),
123            StateParameter::Element(OrbitalElement::VX),
124            StateParameter::Element(OrbitalElement::VY),
125            StateParameter::Element(OrbitalElement::VZ),
126            StateParameter::Element(OrbitalElement::SemiMajorAxis),
127            StateParameter::Element(OrbitalElement::Eccentricity),
128            StateParameter::Element(OrbitalElement::Inclination),
129            StateParameter::Element(OrbitalElement::RAAN),
130            StateParameter::Element(OrbitalElement::AoP),
131            StateParameter::Element(OrbitalElement::TrueAnomaly),
132            StateParameter::Element(OrbitalElement::AoL),
133            StateParameter::Element(OrbitalElement::TrueLongitude),
134            StateParameter::DryMass(),
135            StateParameter::PropMass(),
136            StateParameter::Cr(),
137            StateParameter::Cd(),
138            StateParameter::Isp(),
139            StateParameter::GuidanceMode(),
140            StateParameter::Thrust(),
141            StateParameter::ThrustX(),
142            StateParameter::ThrustY(),
143            StateParameter::ThrustZ(),
144            StateParameter::ThrustInPlane(LocalFrame::RCN),
145            StateParameter::ThrustOutOfPlane(LocalFrame::RCN),
146        ]
147    }
148}
nyx_space/md/trajectory/interpolatable.rs

nyx_space/md/trajectory/
interpolatable.rs
