From 3b39c3d2079f79f7b7ea5f91079aa94fc68d9200 Mon Sep 17 00:00:00 2001 From: Helge Eichhorn Date: Thu, 12 Dec 2024 19:29:52 +0100 Subject: [PATCH] WIP --- Cargo.lock | 1 + crates/lox-earth/src/lib.rs | 17 +- crates/lox-orbits/Cargo.toml | 1 + crates/lox-orbits/src/elements.rs | 140 +++++++------ crates/lox-orbits/src/frames.rs | 78 +++---- crates/lox-orbits/src/frames/iau.rs | 0 crates/lox-orbits/src/frames/iers.rs | 15 ++ crates/lox-orbits/src/ground.rs | 89 ++++++-- crates/lox-orbits/src/python.rs | 86 ++++---- crates/lox-orbits/src/states.rs | 296 ++++++++++++++++++--------- 10 files changed, 447 insertions(+), 276 deletions(-) create mode 100644 crates/lox-orbits/src/frames/iau.rs create mode 100644 crates/lox-orbits/src/frames/iers.rs diff --git a/Cargo.lock b/Cargo.lock index 40643bcc..7a61cdff 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -550,6 +550,7 @@ dependencies = [ "itertools", "libm", "lox-bodies", + "lox-earth", "lox-ephem", "lox-math", "lox-time", diff --git a/crates/lox-earth/src/lib.rs b/crates/lox-earth/src/lib.rs index 18f23933..746025a0 100644 --- a/crates/lox-earth/src/lib.rs +++ b/crates/lox-earth/src/lib.rs @@ -6,13 +6,10 @@ * file, you can obtain one at https://mozilla.org/MPL/2.0/. */ -// TODO: Remove this once all module components are actively used. -#![allow(dead_code)] - -mod cio; -mod cip; -mod coordinate_transformations; -mod nutation; -mod rotation_angle; -mod tides; -mod tio; +pub mod cio; +pub mod cip; +pub mod coordinate_transformations; +pub mod nutation; +pub mod rotation_angle; +pub mod tides; +pub mod tio; diff --git a/crates/lox-orbits/Cargo.toml b/crates/lox-orbits/Cargo.toml index 12b65e0c..45ad9395 100644 --- a/crates/lox-orbits/Cargo.toml +++ b/crates/lox-orbits/Cargo.toml @@ -10,6 +10,7 @@ repository.workspace = true [dependencies] lox-bodies.workspace = true +lox-earth.workspace = true lox-ephem.workspace = true lox-time.workspace = true lox-math.workspace = true diff --git a/crates/lox-orbits/src/elements.rs b/crates/lox-orbits/src/elements.rs index c6ecc065..4862bc87 100644 --- a/crates/lox-orbits/src/elements.rs +++ b/crates/lox-orbits/src/elements.rs @@ -11,21 +11,28 @@ use std::f64::consts::TAU; use float_eq::float_eq; use glam::{DMat3, DVec3}; -use lox_bodies::{DynOrigin, Origin, PointMass}; +use lox_bodies::{DynOrigin, MaybePointMass, Origin, PointMass}; use lox_time::deltas::TimeDelta; use lox_time::TimeLike; -use crate::frames::{CoordinateSystem, Icrf}; -use crate::states::{State, ToCartesian}; +use crate::frames::{CoordinateSystem, DynFrame, Icrf, ReferenceFrame}; +use crate::states::State; -pub trait ToKeplerian { - fn to_keplerian(&self) -> Keplerian; +#[derive(Debug, Clone, PartialEq)] +pub(crate) struct KeplerianElements { + pub semi_major_axis: f64, + pub eccentricity: f64, + pub inclination: f64, + pub longitude_of_ascending_node: f64, + pub argument_of_periapsis: f64, + pub true_anomaly: f64, } #[derive(Debug, Clone, PartialEq)] -pub struct Keplerian { +pub struct Keplerian { time: T, origin: O, + frame: R, semi_major_axis: f64, eccentricity: f64, inclination: f64, @@ -34,12 +41,12 @@ pub struct Keplerian { true_anomaly: f64, } -pub type DynKeplerian = Keplerian; +pub type DynKeplerian = Keplerian; -impl Keplerian +impl Keplerian where T: TimeLike, - O: Origin, + O: PointMass, { #[allow(clippy::too_many_arguments)] pub fn new( @@ -55,6 +62,7 @@ where Self { time, origin, + frame: Icrf, semi_major_axis, eccentricity, inclination, @@ -63,7 +71,46 @@ where true_anomaly, } } +} +impl DynKeplerian +where + T: TimeLike, +{ + #[allow(clippy::too_many_arguments)] + pub fn with_dynamic( + time: T, + origin: DynOrigin, + semi_major_axis: f64, + eccentricity: f64, + inclination: f64, + longitude_of_ascending_node: f64, + argument_of_periapsis: f64, + true_anomaly: f64, + ) -> Result { + if origin.maybe_gravitational_parameter().is_none() { + return Err("undefined gravitational parameter"); + } + Ok(Self { + time, + origin, + frame: DynFrame::Icrf, + semi_major_axis, + eccentricity, + inclination, + longitude_of_ascending_node, + argument_of_periapsis, + true_anomaly, + }) + } +} + +impl Keplerian +where + T: TimeLike, + O: MaybePointMass, + R: ReferenceFrame, +{ pub fn origin(&self) -> O where O: Clone, @@ -78,6 +125,12 @@ where self.time.clone() } + pub fn gravitational_parameter(&self) -> f64 { + self.origin + .maybe_gravitational_parameter() + .expect("gravitational parameter should be available") + } + pub fn semi_major_axis(&self) -> f64 { self.semi_major_axis } @@ -109,15 +162,8 @@ where self.semi_major_axis * (1.0 - self.eccentricity.powi(2)) } } -} - -impl Keplerian -where - T: TimeLike, - O: PointMass, -{ pub fn to_perifocal(&self) -> (DVec3, DVec3) { - let grav_param = self.origin.gravitational_parameter(); + let grav_param = self.gravitational_parameter(); let semiparameter = self.semiparameter(); let (sin_nu, cos_nu) = self.true_anomaly.sin_cos(); let sqrt_mu_p = (grav_param / semiparameter).sqrt(); @@ -130,29 +176,38 @@ where } pub fn orbital_period(&self) -> TimeDelta { - let mu = self.origin.gravitational_parameter(); + let mu = self.gravitational_parameter(); let a = self.semi_major_axis(); TimeDelta::from_decimal_seconds(TAU * (a.powi(3) / mu).sqrt()).unwrap() } } -impl CoordinateSystem for Keplerian { - fn reference_frame(&self) -> Icrf { - Icrf +impl CoordinateSystem + for Keplerian +{ + fn reference_frame(&self) -> R { + self.frame.clone() } } -impl ToCartesian for Keplerian +impl Keplerian where T: TimeLike + Clone, - O: PointMass + Clone, + O: MaybePointMass + Clone, + R: ReferenceFrame + Clone, { - fn to_cartesian(&self) -> State { + pub(crate) fn to_cartesian(&self) -> State { let (pos, vel) = self.to_perifocal(); let rot = DMat3::from_rotation_z(self.longitude_of_ascending_node) * DMat3::from_rotation_x(self.inclination) * DMat3::from_rotation_z(self.argument_of_periapsis); - State::new(self.time(), rot * pos, rot * vel, self.origin(), Icrf) + State::new( + self.time(), + rot * pos, + rot * vel, + self.origin(), + self.reference_frame(), + ) } } @@ -164,41 +219,6 @@ pub fn is_circular(eccentricity: f64) -> bool { float_eq!(eccentricity, 0.0, abs <= 1e-8) } -enum OrbitShape { - SemiMajorAndEccentricity { - semi_major_axis: f64, - eccentricity: f64, - }, - Radii { - periapsis_radius: f64, - apoapsis_radius: f64, - }, - Altitudes { - periapsis_altitude: f64, - apoapsis_altitude: f64, - }, -} - -enum Anomaly { - True(f64), - Eccentric(f64), - Mean(f64), -} - -impl Default for Anomaly { - fn default() -> Self { - Anomaly::True(0.0) - } -} - -pub struct KeplerianBuilder { - shape: OrbitShape, - inclination: Option, - longitude_of_ascending_node: Option, - argument_of_periapisis: Option, - anomaly: Option, -} - #[cfg(test)] mod tests { use super::*; diff --git a/crates/lox-orbits/src/frames.rs b/crates/lox-orbits/src/frames.rs index 6425c63b..84c6c65e 100644 --- a/crates/lox-orbits/src/frames.rs +++ b/crates/lox-orbits/src/frames.rs @@ -6,10 +6,8 @@ * file, you can obtain one at https://mozilla.org/MPL/2.0/. */ -use std::f64::consts::FRAC_PI_2; use std::{convert::Infallible, str::FromStr}; -use crate::ground::GroundLocation; use glam::{DMat3, DVec3}; use lox_bodies::{DynOrigin, MaybeRotationalElements, Origin, RotationalElements, Spheroid}; use lox_math::types::units::Seconds; @@ -18,6 +16,9 @@ use thiserror::Error; use crate::rotations::Rotation; +mod iau; +mod iers; + pub trait ReferenceFrame { fn name(&self) -> String; fn abbreviation(&self) -> String; @@ -149,26 +150,6 @@ impl ReferenceFrame for BodyFixed { } } -// #[derive(Clone, Debug)] -// pub struct Topocentric(GroundLocation); - -// impl Topocentric { -// pub fn new(location: GroundLocation) -> Self { -// Topocentric(location) -// } -// -// pub fn from_coords(longitude: f64, latitude: f64, altitude: f64, body: B) -> Self { -// let location = GroundLocation::new(longitude, latitude, altitude, body); -// Topocentric(location) -// } -// -// pub fn rotation_from_body_fixed(&self) -> DMat3 { -// let r1 = DMat3::from_rotation_z(self.0.longitude()).transpose(); -// let r2 = DMat3::from_rotation_y(FRAC_PI_2 - self.0.latitude()).transpose(); -// r2 * r1 -// } -// } - #[derive(Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord)] pub enum DynFrame { Icrf, @@ -256,34 +237,43 @@ impl FromStr for DynFrame { } } +pub trait RotateTo { + type Time; + + fn rotation(&self, frame: &T, time: Self::Time) -> Rotation; +} + +pub trait TryRotateTo { + type Time; + type Error; + + fn try_rotation(&self, frame: &T) -> Result; +} + +impl> TryRotateTo for U { + type Error = Infallible; + + fn try_rotation(&self, frame: &T) -> Result { + Ok(self.rotation(frame)) + } +} + +impl TryRotateTo for DynFrame { + type Error = (); + + fn try_rotation(&self, frame: &DynFrame) -> Result { + match self { + _ => todo!(), + } + } +} + #[cfg(test)] mod tests { use super::*; - use lox_bodies::Earth; - use lox_math::assert_close; use lox_math::is_close::IsClose; use rstest::rstest; - // #[test] - // fn test_topocentric() { - // let topo = Topocentric::from_coords(8.0, 50.0, 0.0, Earth); - // let r = topo.rotation_from_body_fixed(); - // let x_axis = DVec3::new( - // 0.038175550084451906, - // -0.9893582466233818, - // -0.14040258976976597, - // ); - // let y_axis = DVec3::new( - // -0.25958272521858694, - // -0.14550003380861354, - // 0.9546970980000851, - // ); - // let z_axis = DVec3::new(-0.9649660284921128, 0.0, -0.2623748537039304); - // assert_close!(r.x_axis, x_axis); - // assert_close!(r.y_axis, y_axis); - // assert_close!(r.z_axis, z_axis); - // } - #[rstest] #[case("IAU_EARTH", Some(DynFrame::BodyFixed(DynOrigin::Earth)))] #[case("FOO_EARTH", None)] diff --git a/crates/lox-orbits/src/frames/iau.rs b/crates/lox-orbits/src/frames/iau.rs new file mode 100644 index 00000000..e69de29b diff --git a/crates/lox-orbits/src/frames/iers.rs b/crates/lox-orbits/src/frames/iers.rs new file mode 100644 index 00000000..c7a2f712 --- /dev/null +++ b/crates/lox-orbits/src/frames/iers.rs @@ -0,0 +1,15 @@ +/* + * Copyright (c) 2024. Helge Eichhorn and the LOX contributors + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, you can obtain one at https://mozilla.org/MPL/2.0/. + */ +use crate::frames::{Cirf, Icrf, RotateTo}; +use crate::rotations::Rotation; + +impl RotateTo for Icrf { + fn rotation(&self, _: &Cirf) -> Rotation { + todo!() + } +} diff --git a/crates/lox-orbits/src/ground.rs b/crates/lox-orbits/src/ground.rs index 0d7fb854..e181e124 100644 --- a/crates/lox-orbits/src/ground.rs +++ b/crates/lox-orbits/src/ground.rs @@ -11,13 +11,16 @@ use std::f64::consts::FRAC_PI_2; use glam::{DMat3, DVec3}; use thiserror::Error; -use lox_bodies::{RotationalElements, Spheroid}; +use lox_bodies::{DynOrigin, MaybeSpheroid, RotationalElements, Spheroid}; use lox_math::types::units::Radians; use lox_time::prelude::Tdb; use lox_time::transformations::TryToScale; use lox_time::TimeLike; -use crate::frames::{BodyFixed, CoordinateSystem, FrameTransformationProvider, Icrf, TryToFrame}; +use crate::frames::{ + BodyFixed, CoordinateSystem, DynFrame, FrameTransformationProvider, Icrf, ReferenceFrame, + TryToFrame, +}; use crate::propagators::Propagator; use crate::states::State; use crate::trajectories::TrajectoryError; @@ -57,13 +60,15 @@ impl Observables { } #[derive(Clone, Debug)] -pub struct GroundLocation { +pub struct GroundLocation { longitude: f64, latitude: f64, altitude: f64, body: B, } +pub type DynGroundLocation = GroundLocation; + impl GroundLocation { pub fn new(longitude: f64, latitude: f64, altitude: f64, body: B) -> Self { GroundLocation { @@ -73,16 +78,28 @@ impl GroundLocation { body, } } +} - pub fn with_body(self, body: T) -> GroundLocation { - GroundLocation { - longitude: self.longitude, - latitude: self.latitude, - altitude: self.altitude, - body, +impl DynGroundLocation { + pub fn with_dynamic( + longitude: f64, + latitude: f64, + altitude: f64, + body: DynOrigin, + ) -> Result { + if body.maybe_equatorial_radius().is_none() { + return Err("no spheroid"); } + Ok(GroundLocation { + longitude, + latitude, + altitude, + body, + }) } +} +impl GroundLocation { pub fn origin(&self) -> B where B: Clone, @@ -102,12 +119,24 @@ impl GroundLocation { self.altitude } + fn equatorial_radius(&self) -> f64 { + self.body + .maybe_equatorial_radius() + .expect("equatorial radius should be available") + } + + fn flattening(&self) -> f64 { + self.body + .maybe_flattening() + .expect("flattening should be available") + } + pub fn body_fixed_position(&self) -> DVec3 { let alt = self.altitude; let (lon_sin, lon_cos) = self.longitude.sin_cos(); let (lat_sin, lat_cos) = self.latitude.sin_cos(); - let f = self.body.flattening(); - let r_eq = self.body.equatorial_radius(); + let f = self.flattening(); + let r_eq = self.equatorial_radius(); let e = (2.0 * f - f.powi(2)).sqrt(); let c = r_eq / (1.0 - e.powi(2) * lat_sin.powi(2)).sqrt(); let s = c * (1.0 - e.powi(2)); @@ -150,41 +179,61 @@ pub enum GroundPropagatorError { TrajectoryError(#[from] TrajectoryError), } -pub struct GroundPropagator { +pub struct GroundPropagator { location: GroundLocation, + frame: R, // FIXME: We should not take ownership of the provider here provider: P, } -impl GroundPropagator +pub type DynGroundPropagator = + GroundPropagator; + +impl GroundPropagator where B: Spheroid, P: FrameTransformationProvider, { pub fn new(location: GroundLocation, provider: P) -> Self { - GroundPropagator { location, provider } + GroundPropagator { + location, + frame: Icrf, + provider, + } + } +} + +impl DynGroundPropagator

{ + pub fn with_dynamic(location: DynGroundLocation, provider: P) -> Self { + GroundPropagator { + location, + frame: DynFrame::Icrf, + provider, + } } } -impl CoordinateSystem for GroundPropagator +impl CoordinateSystem for GroundPropagator where O: Spheroid, + R: ReferenceFrame + Clone, P: FrameTransformationProvider, { - fn reference_frame(&self) -> Icrf { - Icrf + fn reference_frame(&self) -> R { + self.frame.clone() } } -impl Propagator for GroundPropagator +impl Propagator for GroundPropagator where T: TimeLike + TryToScale + Clone, O: Spheroid + RotationalElements + Clone, + R: ReferenceFrame + Clone, P: FrameTransformationProvider, { type Error = GroundPropagatorError; - fn propagate(&self, time: T) -> Result, Self::Error> { + fn propagate(&self, time: T) -> Result, Self::Error> { State::new( time, self.location.body_fixed_position(), @@ -192,7 +241,7 @@ where self.location.body.clone(), BodyFixed(self.location.body.clone()), ) - .try_to_frame(Icrf, &self.provider) + .try_to_frame(self.frame.clone(), &self.provider) .map_err(|err| GroundPropagatorError::FrameTransformationError(err.to_string())) } } diff --git a/crates/lox-orbits/src/python.rs b/crates/lox-orbits/src/python.rs index a6ac54b1..b405c225 100644 --- a/crates/lox-orbits/src/python.rs +++ b/crates/lox-orbits/src/python.rs @@ -14,7 +14,7 @@ use pyo3::{ exceptions::PyValueError, pyclass, pyfunction, pymethods, types::{PyAnyMethods, PyList}, - Bound, PyAny, PyErr, PyObject, PyResult, Python, ToPyObject, + Bound, PyAny, PyErr, PyResult, Python, ToPyObject, }; use python::PyOrigin; use sgp4::Elements; @@ -30,16 +30,19 @@ use lox_time::transformations::TryToScale; use lox_time::{python::time::PyTime, ut1::DeltaUt1Tai, Time}; use crate::analysis::{ElevationMask, ElevationMaskError}; -use crate::elements::{Keplerian, ToKeplerian}; +use crate::elements::{DynKeplerian, Keplerian}; use crate::events::{Event, FindEventError, Window}; use crate::frames::{ - BodyFixed, CoordinateSystem, DynFrame, Icrf, ReferenceFrame, TryToFrame, UnknownFrameError, + BodyFixed, CoordinateSystem, DynFrame, ReferenceFrame, TryToFrame, UnknownFrameError, +}; +use crate::ground::{ + DynGroundLocation, DynGroundPropagator, GroundLocation, GroundPropagator, + GroundPropagatorError, Observables, }; -use crate::ground::{GroundLocation, GroundPropagator, GroundPropagatorError, Observables}; use crate::propagators::semi_analytical::{Vallado, ValladoError}; use crate::propagators::sgp4::{Sgp4, Sgp4Error}; use crate::propagators::Propagator; -use crate::states::{DynState, ToCartesian}; +use crate::states::DynState; use crate::trajectories::TrajectoryTransformationError; use crate::{ frames::FrameTransformationProvider, @@ -229,7 +232,11 @@ impl PyState { "only inertial frames are supported for conversion to Keplerian elements", )); } - Ok(PyKeplerian(self.0.to_keplerian())) + Ok(PyKeplerian( + self.0 + .try_to_keplerian() + .map_err(|err| PyValueError::new_err(err))?, + )) } fn rotation_lvlh<'py>(&self, py: Python<'py>) -> PyResult>> { @@ -238,35 +245,25 @@ impl PyState { "only inertial frames are supported for the LVLH rotation matrix", )); } - let rot = self.0.rotation_lvlh(); + let rot = self + .0 + .try_rotation_lvlh() + .map_err(|err| PyValueError::new_err(err))?; let rot: Vec> = rot.to_cols_array_2d().iter().map(|v| v.to_vec()).collect(); Ok(PyArray2::from_vec2_bound(py, &rot)?) } fn to_ground_location(&self, py: Python<'_>) -> PyResult { - if self.0.reference_frame() != DynFrame::Icrf { - return Err(PyValueError::new_err( - "only inertial frames are supported for the ground location transformations", - )); - } - let origin: PyPlanet = self.origin().extract(py)?; - if origin.name() != "Earth" { - return Err(PyValueError::new_err( - "only Earth-based frames are supported for the ground location transformations", - )); - } Ok(PyGroundLocation( self.0 - .try_to_frame(BodyFixed(Earth), &PyNoOpOffsetProvider)? - .to_ground_location() - .map_err(|err| PyValueError::new_err(err.to_string()))? - .with_body(origin), + .to_dyn_ground_location() + .map_err(|err| PyValueError::new_err(err.to_string()))?, )) } } #[pyclass(name = "Keplerian", module = "lox_space", frozen)] -pub struct PyKeplerian(pub Keplerian); +pub struct PyKeplerian(pub DynKeplerian); #[pymethods] impl PyKeplerian { @@ -293,16 +290,19 @@ impl PyKeplerian { origin: Option, ) -> PyResult { let origin = origin.map(|origin| origin.0).unwrap_or_default(); - Ok(PyKeplerian(Keplerian::new( - time, - origin, - semi_major_axis, - eccentricity, - inclination, - longitude_of_ascending_node, - argument_of_periapsis, - true_anomaly, - ))) + Ok(PyKeplerian( + Keplerian::with_dynamic( + time, + origin, + semi_major_axis, + eccentricity, + inclination, + longitude_of_ascending_node, + argument_of_periapsis, + true_anomaly, + ) + .map_err(|err| PyValueError::new_err(err.to_string()))?, + )) } fn time(&self) -> PyTime { @@ -558,23 +558,27 @@ impl PyVallado { #[pyclass(name = "GroundLocation", module = "lox_space", frozen)] #[derive(Clone)] -pub struct PyGroundLocation(pub GroundLocation); +pub struct PyGroundLocation(pub DynGroundLocation); #[pymethods] impl PyGroundLocation { #[new] - fn new(origin: PyOrigin, longitude: f64, latitude: f64, altitude: f64) -> Self { - PyGroundLocation(GroundLocation::new(longitude, latitude, altitude, origin.0)) + fn new(origin: PyOrigin, longitude: f64, latitude: f64, altitude: f64) -> PyResult { + Ok(PyGroundLocation( + DynGroundLocation::with_dynamic(longitude, latitude, altitude, origin.0) + .map_err(|err| PyValueError::new_err(err))?, + )) } - #[pyo3(signature = (state, provider=None))] + #[pyo3(signature = (state, provider=None, frame=None))] fn observables( &self, state: PyState, provider: Option<&Bound<'_, PyUt1Provider>>, + frame: Option, ) -> PyResult { - // FIXME - let state = state.to_frame(PyFrame::Earth, provider)?; + let frame = frame.unwrap_or(PyFrame(DynFrame::BodyFixed(state.0.origin()))); + let state = state.to_frame(frame, provider)?; let rot = self.0.rotation_to_topocentric(); let position = rot * (state.0.position() - self.0.body_fixed_position()); let velocity = rot * state.0.velocity(); @@ -607,7 +611,7 @@ impl PyGroundLocation { } #[pyclass(name = "GroundPropagator", module = "lox_space", frozen)] -pub struct PyGroundPropagator(GroundPropagator); +pub struct PyGroundPropagator(DynGroundPropagator); impl From for PyErr { fn from(err: GroundPropagatorError) -> Self { @@ -619,7 +623,7 @@ impl From for PyErr { impl PyGroundPropagator { #[new] fn new(location: PyGroundLocation, provider: PyUt1Provider) -> Self { - PyGroundPropagator(GroundPropagator::new(location.0, provider)) + PyGroundPropagator(DynGroundPropagator::with_dynamic(location.0, provider)) } fn propagate<'py>( diff --git a/crates/lox-orbits/src/states.rs b/crates/lox-orbits/src/states.rs index 05a4eef4..255e80b5 100644 --- a/crates/lox-orbits/src/states.rs +++ b/crates/lox-orbits/src/states.rs @@ -7,27 +7,25 @@ */ use glam::{DMat3, DVec3}; use itertools::Itertools; -use std::f64::consts::{PI, TAU}; -use std::ops::Sub; - -use lox_bodies::{DynOrigin, Origin, PointMass, RotationalElements, Spheroid}; +use lox_bodies::{ + DynOrigin, MaybePointMass, MaybeSpheroid, Origin, PointMass, RotationalElements, Spheroid, +}; use lox_ephem::{path_from_ids, Ephemeris}; use lox_math::glam::Azimuth; use lox_math::math::{mod_two_pi, normalize_two_pi}; use lox_math::roots::{BracketError, FindRoot, Secant}; use lox_time::{julian_dates::JulianDate, time_scales::Tdb, transformations::TryToScale, TimeLike}; +use std::f64::consts::{PI, TAU}; +use std::ops::Sub; +use thiserror::Error; use crate::anomalies::{eccentric_to_true, hyperbolic_to_true}; -use crate::elements::{is_circular, is_equatorial, Keplerian, ToKeplerian}; +use crate::elements::{is_circular, is_equatorial, DynKeplerian, Keplerian, KeplerianElements}; use crate::frames::{ BodyFixed, CoordinateSystem, DynFrame, FrameTransformationProvider, Icrf, ReferenceFrame, TryToFrame, }; -use crate::ground::GroundLocation; - -pub trait ToCartesian { - fn to_cartesian(&self) -> State; -} +use crate::ground::{DynGroundLocation, GroundLocation}; #[derive(Debug, Clone, Copy, PartialEq)] pub struct State { @@ -79,21 +77,74 @@ where } } +fn rotation_lvlh(position: DVec3, velocity: DVec3) -> DMat3 { + let r = position.normalize(); + let v = velocity.normalize(); + let z = -r; + let y = -r.cross(v); + let x = y.cross(z); + DMat3::from_cols(x, y, z) +} + impl State where T: TimeLike, O: Origin, { pub fn rotation_lvlh(&self) -> DMat3 { - let r = self.position().normalize(); - let v = self.velocity().normalize(); - let z = -r; - let y = -r.cross(v); - let x = y.cross(z); - DMat3::from_cols(x, y, z) + rotation_lvlh(self.position(), self.velocity()) } } +impl DynState +where + T: TimeLike, +{ + pub fn try_rotation_lvlh(&self) -> Result { + if self.frame != DynFrame::Icrf { + // TODO: better error type + return Err("only valid for ICRF"); + } + Ok(rotation_lvlh(self.position(), self.velocity())) + } +} + +type LonLatAlt = (f64, f64, f64); + +fn rv_to_lla(r: DVec3, r_eq: f64, f: f64) -> Result { + let rm = r.length(); + let r_delta = (r.x.powi(2) + r.y.powi(2)).sqrt(); + let mut lon = r.y.atan2(r.x); + + if lon.abs() >= PI { + if lon < 0.0 { + lon += TAU; + } else { + lon -= TAU; + } + } + + let delta = (r.z / rm).asin(); + + let root_finder = Secant::default(); + + let lat = root_finder.find( + |lat| { + let e = (2.0 * f - f.powi(2)).sqrt(); + let c = r_eq / (1.0 - e.powi(2) * lat.sin().powi(2)).sqrt(); + (r.z + c * e.powi(2) * lat.sin()) / r_delta - lat.tan() + }, + delta, + )?; + + let e = (2.0 * f - f.powi(2)).sqrt(); + let c = r_eq / (1.0 - e.powi(2) * lat.sin().powi(2)).sqrt(); + + let alt = r_delta / lat.cos() - c; + + Ok((lon, lat, alt)) +} + impl State> where T: TimeLike, @@ -101,39 +152,36 @@ where { pub fn to_ground_location(&self) -> Result, BracketError> { let r = self.position(); - let rm = r.length(); - let r_delta = (r.x.powi(2) + r.y.powi(2)).sqrt(); - let mut lon = r.y.atan2(r.x); - - if lon.abs() >= PI { - if lon < 0.0 { - lon += TAU; - } else { - lon -= TAU; - } - } - - let delta = (r.z / rm).asin(); - - let root_finder = Secant::default(); let r_eq = self.origin.equatorial_radius(); let f = self.origin.flattening(); + let (lon, lat, alt) = rv_to_lla(r, r_eq, f)?; + + Ok(GroundLocation::new(lon, lat, alt, self.origin())) + } +} - let lat = root_finder.find( - |lat| { - let e = (2.0 * f - f.powi(2)).sqrt(); - let c = r_eq / (1.0 - e.powi(2) * lat.sin().powi(2)).sqrt(); - (r.z + c * e.powi(2) * lat.sin()) / r_delta - lat.tan() - }, - delta, - )?; +#[derive(Debug, Clone, Error, Eq, PartialEq)] +pub enum StateToDynGroundError { + #[error("equatorial radius and flattening factor are not available for origin `{}`", .0.name())] + UndefinedSpheroid(DynOrigin), + #[error(transparent)] + BracketError(#[from] BracketError), +} - let e = (2.0 * f - f.powi(2)).sqrt(); - let c = r_eq / (1.0 - e.powi(2) * lat.sin().powi(2)).sqrt(); +impl DynState { + pub fn to_dyn_ground_location(&self) -> Result { + // TODO: Check/transform frame + let (Some(r_eq), Some(f)) = ( + self.origin.maybe_equatorial_radius(), + self.origin.maybe_flattening(), + ) else { + return Err(StateToDynGroundError::UndefinedSpheroid(self.origin)); + }; + let r = self.position(); - let alt = r_delta / lat.cos() - c; + let (lon, lat, alt) = rv_to_lla(r, r_eq, f)?; - Ok(GroundLocation::new(lon, lat, alt, self.origin())) + Ok(DynGroundLocation::with_dynamic(lon, lat, alt, self.origin).unwrap()) } } @@ -152,6 +200,14 @@ where } } +fn eccentricity_vector(r: DVec3, v: DVec3, mu: f64) -> DVec3 { + let rm = r.length(); + let v2 = v.dot(v); + let rv = r.dot(v); + + ((v2 - mu / rm) * r - rv * v) / mu +} + impl State where T: TimeLike, @@ -256,75 +312,113 @@ where } } -impl ToKeplerian for State +pub(crate) fn rv_to_keplerian(r: DVec3, v: DVec3, mu: f64) -> KeplerianElements { + let rm = r.length(); + let vm = v.length(); + let h = r.cross(v); + let hm = h.length(); + let node = DVec3::Z.cross(h); + let e = eccentricity_vector(r, v, mu); + let eccentricity = e.length(); + let inclination = h.angle_between(DVec3::Z); + + let equatorial = is_equatorial(inclination); + let circular = is_circular(eccentricity); + + let semi_major_axis = if circular { + hm.powi(2) / mu + } else { + -mu / (2.0 * (vm.powi(2) / 2.0 - mu / rm)) + }; + + let longitude_of_ascending_node; + let argument_of_periapsis; + let true_anomaly; + if equatorial && !circular { + longitude_of_ascending_node = 0.0; + argument_of_periapsis = e.azimuth(); + true_anomaly = (h.dot(e.cross(r)) / hm).atan2(r.dot(e)); + } else if !equatorial && circular { + longitude_of_ascending_node = node.azimuth(); + argument_of_periapsis = 0.0; + true_anomaly = (r.dot(h.cross(node)) / hm).atan2(r.dot(node)); + } else if equatorial && circular { + longitude_of_ascending_node = 0.0; + argument_of_periapsis = 0.0; + true_anomaly = r.azimuth(); + } else { + if semi_major_axis > 0.0 { + let e_se = r.dot(v) / (mu * semi_major_axis).sqrt(); + let e_ce = (rm * vm.powi(2)) / mu - 1.0; + true_anomaly = eccentric_to_true(e_se.atan2(e_ce), eccentricity); + } else { + let e_sh = r.dot(v) / (-mu * semi_major_axis).sqrt(); + let e_ch = (rm * vm.powi(2)) / mu - 1.0; + true_anomaly = + hyperbolic_to_true(((e_ch + e_sh) / (e_ch - e_sh)).ln() / 2.0, eccentricity); + } + longitude_of_ascending_node = node.azimuth(); + let px = r.dot(node); + let py = r.dot(h.cross(node)) / hm; + argument_of_periapsis = py.atan2(px) - true_anomaly; + } + + KeplerianElements { + semi_major_axis, + eccentricity, + inclination, + longitude_of_ascending_node: mod_two_pi(longitude_of_ascending_node), + argument_of_periapsis: mod_two_pi(argument_of_periapsis), + true_anomaly: normalize_two_pi(true_anomaly, 0.0), + } +} + +impl State where T: TimeLike + Clone, O: PointMass + Clone, - R: ReferenceFrame, { - fn to_keplerian(&self) -> Keplerian { - let mu = self.origin.gravitational_parameter(); + pub(crate) fn to_keplerian(&self) -> Keplerian { let r = self.position(); let v = self.velocity(); - let rm = r.length(); - let vm = v.length(); - let h = r.cross(v); - let hm = h.length(); - let node = DVec3::Z.cross(h); - let e = self.eccentricity_vector(); - let eccentricity = e.length(); - let inclination = h.angle_between(DVec3::Z); - - let equatorial = is_equatorial(inclination); - let circular = is_circular(eccentricity); - - let semi_major_axis = if circular { - hm.powi(2) / mu - } else { - -mu / (2.0 * (vm.powi(2) / 2.0 - mu / rm)) + let mu = self.origin.gravitational_parameter(); + let elements = rv_to_keplerian(r, v, mu); + + Keplerian::new( + self.time(), + self.origin(), + elements.semi_major_axis, + elements.eccentricity, + elements.inclination, + elements.longitude_of_ascending_node, + elements.argument_of_periapsis, + elements.true_anomaly, + ) + } +} + +impl DynState +where + T: TimeLike + Clone, +{ + pub fn try_to_keplerian(&self) -> Result, &'static str> { + let Some(mu) = self.origin.maybe_gravitational_parameter() else { + return Err("no gravitational parameter"); }; - let longitude_of_ascending_node; - let argument_of_periapsis; - let true_anomaly; - if equatorial && !circular { - longitude_of_ascending_node = 0.0; - argument_of_periapsis = e.azimuth(); - true_anomaly = (h.dot(e.cross(r)) / hm).atan2(r.dot(e)); - } else if !equatorial && circular { - longitude_of_ascending_node = node.azimuth(); - argument_of_periapsis = 0.0; - true_anomaly = (r.dot(h.cross(node)) / hm).atan2(r.dot(node)); - } else if equatorial && circular { - longitude_of_ascending_node = 0.0; - argument_of_periapsis = 0.0; - true_anomaly = r.azimuth(); - } else { - if semi_major_axis > 0.0 { - let e_se = r.dot(v) / (mu * semi_major_axis).sqrt(); - let e_ce = (rm * vm.powi(2)) / mu - 1.0; - true_anomaly = eccentric_to_true(e_se.atan2(e_ce), eccentricity); - } else { - let e_sh = r.dot(v) / (-mu * semi_major_axis).sqrt(); - let e_ch = (rm * vm.powi(2)) / mu - 1.0; - true_anomaly = - hyperbolic_to_true(((e_ch + e_sh) / (e_ch - e_sh)).ln() / 2.0, eccentricity); - } - longitude_of_ascending_node = node.azimuth(); - let px = r.dot(node); - let py = r.dot(h.cross(node)) / hm; - argument_of_periapsis = py.atan2(px) - true_anomaly; - } + let r = self.position(); + let v = self.velocity(); + let elements = rv_to_keplerian(r, v, mu); - Keplerian::new( + Keplerian::with_dynamic( self.time(), self.origin(), - semi_major_axis, - eccentricity, - inclination, - mod_two_pi(longitude_of_ascending_node), - mod_two_pi(argument_of_periapsis), - normalize_two_pi(true_anomaly, 0.0), + elements.semi_major_axis, + elements.eccentricity, + elements.inclination, + elements.longitude_of_ascending_node, + elements.argument_of_periapsis, + elements.true_anomaly, ) } }