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

nyx_space/md/trajectory/
interpolatable.rs
