lox_orbits/orbits/
trajectory.rs

1// SPDX-FileCopyrightText: 2025 Helge Eichhorn <git@helgeeichhorn.de>
2//
3// SPDX-License-Identifier: MPL-2.0
4
5use std::num::ParseFloatError;
6
7use glam::DVec3;
8use lox_bodies::{DynOrigin, Origin};
9use lox_core::coords::{Cartesian, CartesianTrajectory, TimeStampedCartesian};
10use lox_ephem::Ephemeris;
11use lox_frames::{DynFrame, Icrf, ReferenceFrame, rotations::TryRotation, traits::frame_id};
12use lox_time::{
13    Time,
14    deltas::TimeDelta,
15    offsets::{DefaultOffsetProvider, Offset},
16    time_scales::{DynTimeScale, Tai, Tdb, TimeScale},
17    utc::Utc,
18};
19use thiserror::Error;
20
21use lox_time::intervals::TimeInterval;
22
23use crate::propagators::Propagator;
24
25use super::{CartesianOrbit, Orbit, TrajectorError};
26
27/// A time-ordered sequence of Cartesian orbital states with Hermite interpolation.
28#[derive(Debug, Clone)]
29pub struct Trajectory<T: TimeScale, O: Origin, R: ReferenceFrame> {
30    epoch: Time<T>,
31    origin: O,
32    frame: R,
33    data: CartesianTrajectory,
34}
35
36impl<T, O, R> Trajectory<T, O, R>
37where
38    T: TimeScale + Copy,
39    O: Origin + Copy,
40    R: ReferenceFrame + Copy,
41{
42    /// Constructs a trajectory from an iterator of Cartesian orbital states.
43    pub fn new(states: impl IntoIterator<Item = CartesianOrbit<T, O, R>>) -> Self {
44        let mut states = states.into_iter().peekable();
45        let first = states.peek().unwrap();
46        let epoch = first.time();
47        let origin = first.origin();
48        let frame = first.reference_frame();
49        let data = states
50            .map(|orb| {
51                let time = orb.time() - epoch;
52                TimeStampedCartesian {
53                    time,
54                    state: orb.state(),
55                }
56            })
57            .collect();
58        Self::from_parts(epoch, origin, frame, data)
59    }
60
61    /// Constructs a trajectory from its constituent parts.
62    pub fn from_parts(epoch: Time<T>, origin: O, frame: R, data: CartesianTrajectory) -> Self {
63        Self {
64            epoch,
65            origin,
66            frame,
67            data,
68        }
69    }
70
71    /// Decomposes this trajectory into its constituent parts.
72    pub fn into_parts(self) -> (Time<T>, O, R, CartesianTrajectory) {
73        (self.epoch, self.origin, self.frame, self.data)
74    }
75
76    /// Constructs a trajectory, returning an error if fewer than 2 states are provided.
77    pub fn try_new(
78        states: impl IntoIterator<Item = CartesianOrbit<T, O, R>>,
79    ) -> Result<Self, TrajectorError> {
80        let mut states = states.into_iter().peekable();
81        for i in 0..2 {
82            states.peek().ok_or(TrajectorError::InsufficientStates(i))?;
83        }
84        Ok(Self::new(states))
85    }
86
87    /// Interpolates the trajectory at the given absolute time.
88    pub fn at(&self, time: Time<T>) -> CartesianOrbit<T, O, R> {
89        let t = (time - self.epoch).to_seconds().to_f64();
90        let state = self.data.at(t);
91        Orbit::from_state(state, time, self.origin, self.frame)
92    }
93
94    /// Transforms the entire trajectory into a different reference frame.
95    pub fn into_frame<R1, P>(
96        self,
97        frame: R1,
98        provider: &P,
99    ) -> Result<Trajectory<T, O, R1>, Box<dyn std::error::Error>>
100    where
101        R1: ReferenceFrame + Copy,
102        P: TryRotation<R, R1, T>,
103    {
104        if frame_id(&self.frame) == frame_id(&frame) {
105            return Ok(Trajectory::from_parts(
106                self.epoch,
107                self.origin,
108                frame,
109                self.data,
110            ));
111        }
112
113        let data: Result<CartesianTrajectory, P::Error> = self
114            .data
115            .into_iter()
116            .map(|TimeStampedCartesian { time, state }| {
117                let t = self.epoch + time;
118                provider
119                    .try_rotation(self.frame, frame, t)
120                    .map(|rot| TimeStampedCartesian {
121                        time,
122                        state: rot * state,
123                    })
124            })
125            .collect();
126
127        Ok(Trajectory::from_parts(
128            self.epoch,
129            self.origin,
130            frame,
131            data?,
132        ))
133    }
134
135    /// Returns the reference epoch of the trajectory.
136    pub fn epoch(&self) -> Time<T> {
137        self.epoch
138    }
139
140    /// Returns the central body origin.
141    pub fn origin(&self) -> O {
142        self.origin
143    }
144
145    /// Returns the reference frame.
146    pub fn reference_frame(&self) -> R {
147        self.frame
148    }
149
150    /// Returns the start time of the trajectory (same as the epoch).
151    pub fn start_time(&self) -> Time<T> {
152        self.epoch
153    }
154
155    /// Returns the end time of the trajectory.
156    pub fn end_time(&self) -> Time<T> {
157        let time_steps = self.data.time_steps();
158        let last = time_steps.last().copied().unwrap_or(0.0);
159        self.epoch + TimeDelta::from_seconds_f64(last)
160    }
161
162    /// Returns the absolute times of all data points in the trajectory.
163    pub fn times(&self) -> Vec<Time<T>> {
164        let time_steps = self.data.time_steps();
165        time_steps
166            .iter()
167            .map(|&t| self.epoch + TimeDelta::from_seconds_f64(t))
168            .collect()
169    }
170
171    /// Returns all orbital states at the original data points.
172    pub fn states(&self) -> Vec<CartesianOrbit<T, O, R>> {
173        let time_steps = self.data.time_steps();
174        time_steps
175            .iter()
176            .map(|&t| {
177                let state = self.data.at(t);
178                let time = self.epoch + TimeDelta::from_seconds_f64(t);
179                Orbit::from_state(state, time, self.origin, self.frame)
180            })
181            .collect()
182    }
183
184    /// Returns the trajectory as a vector of `[t, x, y, z, vx, vy, vz]` rows.
185    pub fn to_vec(&self) -> Vec<Vec<f64>> {
186        let time_steps = self.data.time_steps();
187        time_steps
188            .iter()
189            .map(|&t| {
190                let state = self.data.at(t);
191                vec![
192                    t,
193                    state.position().x,
194                    state.position().y,
195                    state.position().z,
196                    state.velocity().x,
197                    state.velocity().y,
198                    state.velocity().z,
199                ]
200            })
201            .collect()
202    }
203
204    /// Interpolates the trajectory at the given time delta from the epoch.
205    pub fn interpolate(&self, dt: TimeDelta) -> CartesianOrbit<T, O, R> {
206        let t = dt.to_seconds().to_f64();
207        let state = self.data.at(t);
208        Orbit::from_state(state, self.epoch + dt, self.origin, self.frame)
209    }
210
211    /// Interpolates the trajectory at the given absolute time.
212    pub fn interpolate_at(&self, time: Time<T>) -> CartesianOrbit<T, O, R> {
213        self.interpolate(time - self.epoch)
214    }
215
216    /// Returns the interpolated position at time `t` seconds from the epoch.
217    pub fn position(&self, t: f64) -> DVec3 {
218        self.data.position(t)
219    }
220
221    /// Returns the interpolated velocity at time `t` seconds from the epoch.
222    pub fn velocity(&self, t: f64) -> DVec3 {
223        self.data.velocity(t)
224    }
225
226    /// Find zero-crossing events of `func` evaluated along this trajectory.
227    ///
228    /// The closure receives the interpolated [`CartesianOrbit`] at each sample
229    /// time and must return a scalar whose sign changes define events.
230    pub fn find_events<F>(
231        &self,
232        func: F,
233        step: TimeDelta,
234    ) -> Result<Vec<crate::events::Event<T>>, crate::events::DetectError>
235    where
236        F: Fn(CartesianOrbit<T, O, R>) -> f64,
237    {
238        let interval = TimeInterval::new(self.start_time(), self.end_time());
239        crate::events::find_events(|t| func(self.interpolate_at(t)), interval, step)
240    }
241
242    /// Find zero-crossing events of a fallible `func` evaluated along this
243    /// trajectory.
244    pub fn try_find_events<F, E>(
245        &self,
246        func: F,
247        step: TimeDelta,
248    ) -> Result<Vec<crate::events::Event<T>>, crate::events::DetectError>
249    where
250        F: Fn(CartesianOrbit<T, O, R>) -> Result<f64, E>,
251        E: std::error::Error + Send + Sync + 'static,
252    {
253        let interval = TimeInterval::new(self.start_time(), self.end_time());
254        crate::events::try_find_events(|t| func(self.interpolate_at(t)), interval, step)
255    }
256
257    /// Find time intervals where `func` is positive, evaluated along this
258    /// trajectory.
259    pub fn find_windows<F>(
260        &self,
261        func: F,
262        step: TimeDelta,
263    ) -> Result<Vec<TimeInterval<T>>, crate::events::DetectError>
264    where
265        F: Fn(CartesianOrbit<T, O, R>) -> f64,
266    {
267        let interval = TimeInterval::new(self.start_time(), self.end_time());
268        crate::events::find_windows(|t| func(self.interpolate_at(t)), interval, step)
269    }
270
271    /// Find time intervals where a fallible `func` is positive, evaluated
272    /// along this trajectory.
273    pub fn try_find_windows<F, E>(
274        &self,
275        func: F,
276        step: TimeDelta,
277    ) -> Result<Vec<TimeInterval<T>>, crate::events::DetectError>
278    where
279        F: Fn(CartesianOrbit<T, O, R>) -> Result<f64, E>,
280        E: std::error::Error + Send + Sync + 'static,
281    {
282        let interval = TimeInterval::new(self.start_time(), self.end_time());
283        crate::events::try_find_windows(|t| func(self.interpolate_at(t)), interval, step)
284    }
285}
286
287impl<T, O, R> Trajectory<T, O, R>
288where
289    T: TimeScale + Copy + Into<DynTimeScale>,
290    O: Origin + Copy + Into<DynOrigin>,
291    R: ReferenceFrame + Copy + Into<DynFrame>,
292{
293    /// Converts this trajectory into a dynamically-typed trajectory.
294    pub fn into_dyn(self) -> DynTrajectory {
295        Trajectory::from_parts(
296            self.epoch.into_dyn(),
297            self.origin.into(),
298            self.frame.into(),
299            self.data,
300        )
301    }
302}
303
304impl<T, O, R> Propagator<T, O> for Trajectory<T, O, R>
305where
306    T: TimeScale + Copy + PartialOrd,
307    O: Origin + Copy,
308    R: ReferenceFrame + Copy,
309{
310    type Frame = R;
311    type Error = TrajectorError;
312
313    fn state_at(&self, time: Time<T>) -> Result<CartesianOrbit<T, O, R>, TrajectorError> {
314        Ok(self.at(time))
315    }
316
317    fn propagate(&self, interval: TimeInterval<T>) -> Result<Trajectory<T, O, R>, Self::Error> {
318        let mut states = Vec::new();
319        states.push(self.at(interval.start()));
320        for s in self.states() {
321            if s.time() > interval.start() && s.time() < interval.end() {
322                states.push(s);
323            }
324        }
325        states.push(self.at(interval.end()));
326        Trajectory::try_new(states)
327    }
328}
329
330/// Errors that can occur when constructing or parsing a trajectory.
331#[derive(Clone, Debug, Error, PartialEq)]
332pub enum TrajectoryError {
333    /// Too few states were provided.
334    #[error("`states` must have at least 2 elements but had {0}")]
335    InsufficientStates(usize),
336    /// An error occurred while parsing CSV data.
337    #[error("CSV error: {0}")]
338    CsvError(String),
339}
340
341impl From<csv::Error> for TrajectoryError {
342    fn from(err: csv::Error) -> Self {
343        TrajectoryError::CsvError(err.to_string())
344    }
345}
346
347/// Errors that can occur when transforming a trajectory to a different origin.
348#[derive(Debug, Error)]
349pub enum TrajectoryTransformationError {
350    /// The underlying trajectory construction failed.
351    #[error(transparent)]
352    TrajectoryError(#[from] TrajectorError),
353    /// A state transformation (e.g. origin change) failed.
354    #[error("state transformation failed: {0}")]
355    StateTransformationError(String),
356}
357
358impl<T, O> Trajectory<T, O, Icrf>
359where
360    T: TimeScale + Copy,
361    O: Origin + Copy,
362    DefaultOffsetProvider: Offset<T, Tdb>,
363{
364    /// Transforms the entire trajectory to a different central body origin using an ephemeris.
365    pub fn to_origin<O1: Origin + Copy, E: Ephemeris>(
366        &self,
367        target: O1,
368        ephemeris: &E,
369    ) -> Result<Trajectory<T, O1, Icrf>, TrajectoryTransformationError> {
370        if self.origin().id() == target.id() {
371            return Ok(Trajectory::from_parts(
372                self.epoch,
373                target,
374                Icrf,
375                self.data.clone(),
376            ));
377        }
378        let states: Result<Vec<_>, _> = self
379            .states()
380            .into_iter()
381            .map(|state| {
382                state.to_origin(target, ephemeris).map_err(|e| {
383                    TrajectoryTransformationError::StateTransformationError(e.to_string())
384                })
385            })
386            .collect();
387        Ok(Trajectory::new(states?))
388    }
389}
390
391impl<O, R> Trajectory<Tai, O, R>
392where
393    O: Origin + Copy,
394    R: ReferenceFrame + Copy,
395{
396    /// Parses a trajectory from CSV data with columns `[time, x, y, z, vx, vy, vz]` in km and km/s.
397    pub fn from_csv(csv: &str, origin: O, frame: R) -> Result<Self, TrajectoryError> {
398        let states = parse_csv_states(csv, origin, frame)?;
399        if states.len() < 2 {
400            return Err(TrajectoryError::InsufficientStates(states.len()));
401        }
402        Ok(Trajectory::new(states))
403    }
404}
405
406/// A dynamically-typed trajectory with runtime time scale, origin, and frame.
407pub type DynTrajectory = Trajectory<DynTimeScale, DynOrigin, DynFrame>;
408
409impl DynTrajectory {
410    /// Parses a dynamically-typed trajectory from CSV data.
411    pub fn from_csv_dyn(
412        csv: &str,
413        origin: DynOrigin,
414        frame: DynFrame,
415    ) -> Result<DynTrajectory, TrajectoryError> {
416        Ok(Trajectory::from_csv(csv, origin, frame)?.into_dyn())
417    }
418}
419
420impl<T, O, R> FromIterator<CartesianOrbit<T, O, R>> for Trajectory<T, O, R>
421where
422    T: TimeScale + Copy,
423    O: Origin + Copy,
424    R: ReferenceFrame + Copy,
425{
426    fn from_iter<U: IntoIterator<Item = CartesianOrbit<T, O, R>>>(iter: U) -> Self {
427        Self::new(iter)
428    }
429}
430
431fn parse_csv_f64(record: &csv::StringRecord, idx: usize) -> Result<f64, TrajectoryError> {
432    record
433        .get(idx)
434        .unwrap()
435        .parse()
436        .map_err(|e: ParseFloatError| TrajectoryError::CsvError(format!("invalid value: {e}")))
437}
438
439fn parse_csv_vec3(
440    record: &csv::StringRecord,
441    i0: usize,
442    i1: usize,
443    i2: usize,
444) -> Result<DVec3, TrajectoryError> {
445    Ok(DVec3::new(
446        parse_csv_f64(record, i0)?,
447        parse_csv_f64(record, i1)?,
448        parse_csv_f64(record, i2)?,
449    ))
450}
451
452fn parse_csv_states<O: Origin + Copy, R: ReferenceFrame + Copy>(
453    csv: &str,
454    origin: O,
455    frame: R,
456) -> Result<Vec<CartesianOrbit<Tai, O, R>>, TrajectoryError> {
457    let mut reader = csv::Reader::from_reader(csv.as_bytes());
458    let mut states = Vec::new();
459    for result in reader.records() {
460        let record = result?;
461        if record.len() != 7 {
462            return Err(TrajectoryError::CsvError(
463                "invalid record length".to_string(),
464            ));
465        }
466        let time: Time<Tai> = Utc::from_iso(record.get(0).unwrap())
467            .map_err(|e| TrajectoryError::CsvError(e.to_string()))?
468            .to_time();
469        // CSV data is in km and km/s, convert to m and m/s
470        let position = parse_csv_vec3(&record, 1, 2, 3)? * 1e3;
471        let velocity = parse_csv_vec3(&record, 4, 5, 6)? * 1e3;
472        let state = Cartesian::from_vecs(position, velocity);
473        states.push(CartesianOrbit::new(state, time, origin, frame));
474    }
475    Ok(states)
476}
477
478#[cfg(test)]
479mod tests {
480    use super::*;
481    use glam::DVec3;
482    use lox_bodies::{DynOrigin, Earth};
483    use lox_frames::{DynFrame, Icrf};
484    use lox_time::time_scales::DynTimeScale;
485    use lox_time::{time, time_scales::Tdb};
486
487    fn sample_trajectory() -> Trajectory<Tdb, Earth, Icrf> {
488        let t0 = time!(Tdb, 2023, 1, 1, 12).unwrap();
489        let t1 = t0 + lox_time::deltas::TimeDelta::from_seconds(60);
490        let t2 = t0 + lox_time::deltas::TimeDelta::from_seconds(120);
491        let s0 = CartesianOrbit::new(
492            Cartesian::from_vecs(DVec3::new(7000e3, 0.0, 0.0), DVec3::new(0.0, 7500.0, 0.0)),
493            t0,
494            Earth,
495            Icrf,
496        );
497        let s1 = CartesianOrbit::new(
498            Cartesian::from_vecs(
499                DVec3::new(6999e3, 100e3, 0.0),
500                DVec3::new(-10.0, 7499.0, 0.0),
501            ),
502            t1,
503            Earth,
504            Icrf,
505        );
506        let s2 = CartesianOrbit::new(
507            Cartesian::from_vecs(
508                DVec3::new(6996e3, 200e3, 0.0),
509                DVec3::new(-20.0, 7498.0, 0.0),
510            ),
511            t2,
512            Earth,
513            Icrf,
514        );
515        Trajectory::new(vec![s0, s1, s2])
516    }
517
518    #[test]
519    fn test_trajectory_into_dyn() {
520        let traj = sample_trajectory();
521        let first_pos = traj.states().first().unwrap().position();
522        let dyn_traj = traj.into_dyn();
523
524        assert_eq!(dyn_traj.origin(), DynOrigin::Earth);
525        assert_eq!(dyn_traj.reference_frame(), DynFrame::Icrf);
526        assert_eq!(
527            dyn_traj.states().first().unwrap().time().scale(),
528            DynTimeScale::Tdb
529        );
530        assert_eq!(dyn_traj.states().first().unwrap().position(), first_pos);
531    }
532
533    #[test]
534    fn test_trajectory_into_parts() {
535        let traj = sample_trajectory();
536        let epoch_before = traj.epoch();
537        let (epoch, origin, frame, _data) = traj.into_parts();
538
539        assert_eq!(origin, Earth);
540        assert_eq!(frame, Icrf);
541        assert_eq!(epoch, epoch_before);
542    }
543}
lox_orbits/orbits/trajectory.rs

lox_orbits/orbits/
trajectory.rs
