lox_orbits/orbits/
cartesian.rs

1// SPDX-FileCopyrightText: 2025 Helge Eichhorn <git@helgeeichhorn.de>
2//
3// SPDX-License-Identifier: MPL-2.0
4
5use std::ops::Sub;
6
7use glam::{DMat3, DVec3};
8use lox_bodies::{
9    DynOrigin, Origin, PointMass, RotationalElements, Spheroid, TryPointMass, TrySpheroid,
10    UndefinedOriginPropertyError,
11};
12use lox_core::coords::Cartesian;
13use lox_core::coords::FromBodyFixedError;
14use lox_core::coords::LonLatAlt;
15use lox_ephem::Ephemeris;
16use lox_frames::{
17    DynFrame, Iau, Icrf, ReferenceFrame, TryBodyFixed, rotations::TryRotation, traits::frame_id,
18};
19use lox_time::offsets::{DefaultOffsetProvider, Offset};
20use lox_time::time_scales::{Tdb, TimeScale};
21use thiserror::Error;
22
23use crate::ground::{DynGroundLocation, GroundLocation};
24
25use super::{CartesianOrbit, DynCartesianOrbit, KeplerianOrbit, Orbit};
26
27impl<T, O, R> CartesianOrbit<T, O, R>
28where
29    T: TimeScale,
30    O: Origin,
31    R: ReferenceFrame,
32{
33    /// Constructs a new Cartesian orbit from position/velocity state, epoch, origin, and frame.
34    pub const fn new(cartesian: Cartesian, time: lox_time::Time<T>, origin: O, frame: R) -> Self {
35        Self::from_state(cartesian, time, origin, frame)
36    }
37
38    /// Returns the position vector in meters.
39    pub fn position(&self) -> DVec3 {
40        self.state().position()
41    }
42
43    /// Returns the velocity vector in meters per second.
44    pub fn velocity(&self) -> DVec3 {
45        self.state().velocity()
46    }
47
48    /// Converts this Cartesian orbit to Keplerian elements.
49    pub fn to_keplerian(&self) -> KeplerianOrbit<T, O, R>
50    where
51        T: Copy,
52        O: Copy + PointMass,
53        R: Copy,
54    {
55        Orbit::from_state(
56            self.state().to_keplerian(self.gravitational_parameter()),
57            self.time(),
58            self.origin(),
59            self.reference_frame(),
60        )
61    }
62
63    /// Converts this Cartesian orbit to Keplerian elements, returning an error if the gravitational parameter is undefined.
64    pub fn try_to_keplerian(&self) -> Result<KeplerianOrbit<T, O, R>, UndefinedOriginPropertyError>
65    where
66        T: Copy,
67        O: Copy + TryPointMass,
68        R: Copy,
69    {
70        Ok(Orbit::from_state(
71            self.state()
72                .to_keplerian(self.try_gravitational_parameter()?),
73            self.time(),
74            self.origin(),
75            self.reference_frame(),
76        ))
77    }
78
79    /// Transforms this orbit into a different reference frame using the given rotation provider.
80    pub fn try_to_frame<R1, P>(
81        &self,
82        frame: R1,
83        provider: &P,
84    ) -> Result<CartesianOrbit<T, O, R1>, P::Error>
85    where
86        R: Copy,
87        P: TryRotation<R, R1, T>,
88        R1: ReferenceFrame + Copy,
89        O: Copy,
90        T: Copy,
91    {
92        if let (Some(id0), Some(id1)) = (frame_id(&self.reference_frame()), frame_id(&frame))
93            && id0 == id1
94        {
95            return Ok(CartesianOrbit::new(
96                self.state(),
97                self.time(),
98                self.origin(),
99                frame,
100            ));
101        }
102        let rot = provider.try_rotation(self.reference_frame(), frame, self.time())?;
103        let (r1, v1) = rot.rotate_state(self.state().position(), self.state().velocity());
104        Ok(CartesianOrbit::new(
105            Cartesian::from_vecs(r1, v1),
106            self.time(),
107            self.origin(),
108            frame,
109        ))
110    }
111}
112
113impl<T, O> CartesianOrbit<T, O, Iau<O>>
114where
115    T: TimeScale,
116    O: Origin + RotationalElements + Spheroid + Copy,
117{
118    /// Converts this body-fixed Cartesian orbit to a ground location.
119    pub fn to_ground_location(&self) -> Result<GroundLocation<O>, FromBodyFixedError> {
120        let origin = self.origin();
121        let coords = LonLatAlt::from_body_fixed(
122            self.position(),
123            origin.equatorial_radius(),
124            origin.flattening(),
125        )?;
126        Ok(GroundLocation::new(coords, origin))
127    }
128}
129
130/// Errors that can occur when converting a dynamic Cartesian orbit to a ground location.
131#[derive(Debug, Error)]
132pub enum StateToDynGroundError {
133    /// The origin body does not define equatorial radius and flattening.
134    #[error("equatorial radius and flattening factor are not available for origin `{}`", .0.name())]
135    UndefinedSpheroid(DynOrigin),
136    /// Failed to convert from body-fixed coordinates.
137    #[error(transparent)]
138    FromBodyFixed(#[from] FromBodyFixedError),
139    /// The frame is not a body-fixed frame.
140    #[error("not a body-fixed frame {0}")]
141    NonBodyFixedFrame(String),
142}
143
144fn rotation_lvlh(position: DVec3, velocity: DVec3) -> DMat3 {
145    let r = position.normalize();
146    let v = velocity.normalize();
147    let z = -r;
148    let y = -r.cross(v);
149    let x = y.cross(z);
150    DMat3::from_cols(x, y, z)
151}
152
153impl<T, O> CartesianOrbit<T, O, Icrf>
154where
155    T: TimeScale,
156    O: Origin,
157{
158    /// Returns the rotation matrix from ICRF to the Local Vertical Local Horizontal (LVLH) frame.
159    pub fn rotation_lvlh(&self) -> DMat3 {
160        rotation_lvlh(self.position(), self.velocity())
161    }
162}
163
164impl<T, O> CartesianOrbit<T, O, Icrf>
165where
166    T: TimeScale + Copy,
167    O: Origin + Copy,
168{
169    /// Transforms this orbit to a different central body origin using an ephemeris.
170    pub fn to_origin<O1: Origin + Copy, E: Ephemeris>(
171        &self,
172        target: O1,
173        ephemeris: &E,
174    ) -> Result<CartesianOrbit<T, O1, Icrf>, E::Error>
175    where
176        DefaultOffsetProvider: Offset<T, Tdb>,
177    {
178        if self.origin().id() == target.id() {
179            return Ok(CartesianOrbit::new(self.state(), self.time(), target, Icrf));
180        }
181        let tdb = self.time().to_scale(Tdb);
182        let delta = ephemeris.state(tdb, self.origin(), target)?;
183        Ok(CartesianOrbit::new(
184            self.state() - delta,
185            self.time(),
186            target,
187            Icrf,
188        ))
189    }
190}
191
192impl DynCartesianOrbit {
193    /// Transforms this dynamic orbit to a different central body origin using an ephemeris.
194    pub fn try_to_origin<E: Ephemeris>(
195        &self,
196        target: DynOrigin,
197        ephemeris: &E,
198    ) -> Result<DynCartesianOrbit, E::Error> {
199        if self.origin() == target {
200            return Ok(CartesianOrbit::new(
201                self.state(),
202                self.time(),
203                target,
204                self.reference_frame(),
205            ));
206        }
207        let tdb = self
208            .time()
209            .try_to_scale(Tdb, &DefaultOffsetProvider)
210            .unwrap();
211        let delta = ephemeris.state(tdb, self.origin(), target)?;
212        Ok(CartesianOrbit::new(
213            self.state() - delta,
214            self.time(),
215            target,
216            DynFrame::Icrf,
217        ))
218    }
219
220    /// Converts this dynamic Cartesian orbit to a ground location.
221    pub fn try_to_ground_location(&self) -> Result<DynGroundLocation, StateToDynGroundError> {
222        let frame = self.reference_frame();
223        let origin = self.origin();
224        if frame.try_body_fixed().is_err() {
225            return Err(StateToDynGroundError::NonBodyFixedFrame(
226                frame.name().to_string(),
227            ));
228        }
229        let (Ok(r_eq), Ok(f)) = (origin.try_equatorial_radius(), origin.try_flattening()) else {
230            return Err(StateToDynGroundError::UndefinedSpheroid(origin));
231        };
232
233        let coords = LonLatAlt::from_body_fixed(self.position(), r_eq, f)?;
234        Ok(DynGroundLocation::try_new(coords, origin).unwrap())
235    }
236
237    /// Returns the LVLH rotation matrix, returning an error if the frame is not ICRF.
238    pub fn try_rotation_lvlh(&self) -> Result<DMat3, &'static str> {
239        if self.reference_frame() != DynFrame::Icrf {
240            return Err("only valid for ICRF");
241        }
242        Ok(rotation_lvlh(self.position(), self.velocity()))
243    }
244}
245
246impl<T, O, R> Sub for CartesianOrbit<T, O, R>
247where
248    T: TimeScale + Copy,
249    O: Origin + Copy,
250    R: ReferenceFrame + Copy,
251{
252    type Output = Self;
253
254    fn sub(self, rhs: Self) -> Self::Output {
255        let state = Cartesian::from_vecs(
256            self.position() - rhs.position(),
257            self.velocity() - rhs.velocity(),
258        );
259        CartesianOrbit::new(state, self.time(), self.origin(), self.reference_frame())
260    }
261}
262
263#[cfg(test)]
264mod tests {
265    use std::sync::OnceLock;
266
267    use glam::DVec3;
268    use lox_bodies::{Earth, Jupiter, Venus};
269    use lox_core::coords::Cartesian;
270    use lox_ephem::Ephemeris;
271    use lox_ephem::spk::parser::Spk;
272    use lox_frames::providers::DefaultRotationProvider;
273    use lox_test_utils::{assert_approx_eq, data_file};
274    use lox_time::{Time, time, time_scales::Tdb, utc::Utc};
275
276    use super::*;
277
278    #[test]
279    fn test_bodyfixed() {
280        let iau_jupiter = Iau::new(Jupiter);
281
282        let r0 = DVec3::new(6068.27927, -1692.84394, -2516.61918);
283        let v0 = DVec3::new(-0.660415582, 5.495938726, -5.303093233);
284        let r1 = DVec3::new(3922.220687351738, 5289.381014412637, -1631.4837924820245);
285        let v1 = DVec3::new(-1.852284168309543, -0.8227941105651749, -7.14175174489828);
286
287        let tdb = time!(Tdb, 2000, 1, 1, 12).unwrap();
288        let s = CartesianOrbit::new(Cartesian::from_vecs(r0, v0), tdb, Jupiter, Icrf);
289        let s1 = s
290            .try_to_frame(iau_jupiter, &DefaultRotationProvider)
291            .unwrap();
292        let s0 = s1.try_to_frame(Icrf, &DefaultRotationProvider).unwrap();
293
294        assert_approx_eq!(s0.position(), r0, rtol <= 1e-8);
295        assert_approx_eq!(s0.velocity(), v0, rtol <= 1e-8);
296
297        assert_approx_eq!(s1.position(), r1, rtol <= 1e-8);
298        assert_approx_eq!(s1.velocity(), v1, rtol <= 1e-8);
299    }
300
301    #[test]
302    fn test_cartesian_to_keplerian_roundtrip() {
303        let time = time!(Tdb, 2023, 3, 25, 21, 8, 0.0).expect("time should be valid");
304        let pos = DVec3::new(
305            -0.107622532467967e7,
306            -0.676589636432773e7,
307            -0.332308783350379e6,
308        );
309        let vel = DVec3::new(
310            0.935685775154103e4,
311            -0.331234775037644e4,
312            -0.118801577532701e4,
313        );
314
315        let cartesian = CartesianOrbit::new(Cartesian::from_vecs(pos, vel), time, Earth, Icrf);
316        let cartesian1 = cartesian.to_keplerian().to_cartesian();
317
318        assert_eq!(cartesian1.time(), time);
319        assert_eq!(cartesian1.origin(), Earth);
320        assert_eq!(cartesian1.reference_frame(), Icrf);
321
322        assert_approx_eq!(cartesian.position(), cartesian1.position(), rtol <= 1e-8);
323        assert_approx_eq!(cartesian.velocity(), cartesian1.velocity(), rtol <= 1e-6);
324    }
325
326    #[test]
327    fn test_to_ground_location() {
328        let lat_exp = 51.484f64.to_radians();
329        let lon_exp = -35.516f64.to_radians();
330        let alt_exp = 237.434; // km
331
332        let position = DVec3::new(3359927.0, -2398072.0, 5153000.0);
333        let velocity = DVec3::new(5065.7, 5485.0, -744.0);
334        let time = time!(Tdb, 2012, 7, 1).unwrap();
335        let state = CartesianOrbit::new(
336            Cartesian::from_vecs(position, velocity),
337            time,
338            Earth,
339            Iau::new(Earth),
340        );
341        let ground = state.to_ground_location().unwrap();
342        assert_approx_eq!(ground.latitude(), lat_exp, rtol <= 1e-4);
343        assert_approx_eq!(ground.longitude(), lon_exp, rtol <= 1e-4);
344        assert_approx_eq!(ground.altitude(), alt_exp, rtol <= 1e-4);
345    }
346
347    #[test]
348    fn test_to_origin() {
349        let r = DVec3::new(6068279.27, -1692843.94, -2516619.18);
350        let v = DVec3::new(-660.415582, 5495.938726, -5303.093233);
351
352        let utc = Utc::from_iso("2016-05-30T12:00:00.000").unwrap();
353        let tai = utc.to_time();
354        let tdb = tai.to_scale(Tdb);
355
356        // Compute the expected ephemeris delta using the new trait directly
357        let delta = ephemeris().state(tdb, Earth, Venus).unwrap();
358        let r_exp = r - delta.position();
359        let v_exp = v - delta.velocity();
360
361        let s_earth = CartesianOrbit::new(Cartesian::from_vecs(r, v), tai, Earth, Icrf);
362        let s_venus = s_earth.to_origin(Venus, ephemeris()).unwrap();
363
364        assert_approx_eq!(s_venus.position(), r_exp);
365        assert_approx_eq!(s_venus.velocity(), v_exp);
366    }
367
368    #[test]
369    fn test_rotation_lvlh() {
370        let time = time!(Tdb, 2023, 3, 25, 21, 8, 0.0).unwrap();
371        let pos = DVec3::new(6678.0, 0.0, 0.0);
372        let vel = DVec3::new(0.0, 7.73, 0.0);
373
374        let state = CartesianOrbit::new(Cartesian::from_vecs(pos, vel), time, Earth, Icrf);
375        let rot = state.rotation_lvlh();
376
377        // For a prograde equatorial orbit at x-axis, LVLH z should point to -x (nadir),
378        // y should point to -z (cross-track), x should point to +y (velocity direction)
379        let z = rot.col(2);
380        let x = rot.col(0);
381        assert_approx_eq!(z, -DVec3::X, atol <= 1e-10);
382        assert_approx_eq!(x, DVec3::Y, atol <= 1e-10);
383    }
384
385    #[test]
386    fn test_sub_operator() {
387        let time = time!(Tdb, 2023, 3, 25, 21, 8, 0.0).unwrap();
388        let s1 = CartesianOrbit::new(
389            Cartesian::from_vecs(DVec3::new(10.0, 20.0, 30.0), DVec3::new(1.0, 2.0, 3.0)),
390            time,
391            Earth,
392            Icrf,
393        );
394        let s2 = CartesianOrbit::new(
395            Cartesian::from_vecs(DVec3::new(3.0, 5.0, 7.0), DVec3::new(0.5, 1.0, 1.5)),
396            time,
397            Earth,
398            Icrf,
399        );
400        let diff = s1 - s2;
401        assert_approx_eq!(diff.position(), DVec3::new(7.0, 15.0, 23.0));
402        assert_approx_eq!(diff.velocity(), DVec3::new(0.5, 1.0, 1.5));
403    }
404
405    #[test]
406    fn test_into_dyn() {
407        use lox_bodies::DynOrigin;
408        use lox_frames::DynFrame;
409        use lox_time::time_scales::DynTimeScale;
410
411        let time = time!(Tdb, 2023, 3, 25, 21, 8, 0.0).unwrap();
412        let pos = DVec3::new(1000.0, 2000.0, 3000.0);
413        let vel = DVec3::new(1.0, 2.0, 3.0);
414        let state = CartesianOrbit::new(Cartesian::from_vecs(pos, vel), time, Earth, Icrf);
415        let dyn_state = state.into_dyn();
416
417        assert_eq!(dyn_state.origin(), DynOrigin::Earth);
418        assert_eq!(dyn_state.reference_frame(), DynFrame::Icrf);
419        assert_eq!(dyn_state.time().scale(), DynTimeScale::Tdb);
420        assert_approx_eq!(dyn_state.position(), pos);
421        assert_approx_eq!(dyn_state.velocity(), vel);
422    }
423
424    #[test]
425    fn test_try_to_frame_identity() {
426        use lox_bodies::DynOrigin;
427        use lox_frames::DynFrame;
428
429        let time = Utc::from_iso("2024-07-05T09:09:18.173")
430            .unwrap()
431            .to_dyn_time();
432        let pos = DVec3::new(6068.27927, -1692.84394, -2516.61918);
433        let vel = DVec3::new(-0.660415582, 5.495938726, -5.303093233);
434        let state = CartesianOrbit::new(
435            Cartesian::from_vecs(pos, vel),
436            time,
437            DynOrigin::Earth,
438            DynFrame::Icrf,
439        );
440
441        // Converting ICRF→ICRF should be a no-op that preserves state exactly
442        let same = state
443            .try_to_frame(DynFrame::Icrf, &DefaultRotationProvider)
444            .unwrap();
445        assert_eq!(same.position(), pos);
446        assert_eq!(same.velocity(), vel);
447    }
448
449    #[test]
450    fn test_to_origin_same_origin() {
451        let r = DVec3::new(6068279.27, -1692843.94, -2516619.18);
452        let v = DVec3::new(-660.415582, 5495.938726, -5303.093233);
453        let utc = Utc::from_iso("2016-05-30T12:00:00.000").unwrap();
454        let tai = utc.to_time();
455
456        let state = CartesianOrbit::new(Cartesian::from_vecs(r, v), tai, Earth, Icrf);
457        // Same origin should return identical state without needing ephemeris
458        let same = state.to_origin(Earth, ephemeris()).unwrap();
459        assert_eq!(same.position(), r);
460        assert_eq!(same.velocity(), v);
461    }
462
463    #[test]
464    fn test_try_to_origin_same_origin() {
465        use lox_bodies::DynOrigin;
466        use lox_frames::DynFrame;
467
468        let r = DVec3::new(6068279.27, -1692843.94, -2516619.18);
469        let v = DVec3::new(-660.415582, 5495.938726, -5303.093233);
470        let utc = Utc::from_iso("2016-05-30T12:00:00.000").unwrap();
471        let time = utc.to_dyn_time();
472
473        let state = CartesianOrbit::new(
474            Cartesian::from_vecs(r, v),
475            time,
476            DynOrigin::Earth,
477            DynFrame::Icrf,
478        );
479        let same = state.try_to_origin(DynOrigin::Earth, ephemeris()).unwrap();
480        assert_eq!(same.position(), r);
481        assert_eq!(same.velocity(), v);
482    }
483
484    fn ephemeris() -> &'static Spk {
485        static EPHEMERIS: OnceLock<Spk> = OnceLock::new();
486        EPHEMERIS.get_or_init(|| Spk::from_file(data_file("spice/de440s.bsp")).unwrap())
487    }
488}
lox_orbits/orbits/cartesian.rs

lox_orbits/orbits/
cartesian.rs
