diff --git a/Cargo.lock b/Cargo.lock index cb02da2..78b9d75 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -83,6 +83,12 @@ version = "0.1.0" [[package]] name = "apsis-kepler" version = "0.1.0" +dependencies = [ + "apsis-core", + "apsis-math", + "apsis-time", + "apsis-tle", +] [[package]] name = "apsis-leap" @@ -193,6 +199,12 @@ version = "0.1.0" [[package]] name = "apsis-sgp4" version = "0.1.0" +dependencies = [ + "apsis-core", + "apsis-math", + "apsis-time", + "apsis-tle", +] [[package]] name = "apsis-sim" @@ -217,6 +229,10 @@ dependencies = [ [[package]] name = "apsis-tle" version = "0.1.0" +dependencies = [ + "apsis-core", + "apsis-time", +] [[package]] name = "apsis-topology" diff --git a/Cargo.toml b/Cargo.toml index 175bede..7804892 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -77,6 +77,12 @@ authors = ["ApsisRS contributors"] unsafe_code = "forbid" missing_docs = "deny" +[workspace.lints.clippy] +# Numeric literals here follow the grouping of the quantity they express, +# such as the digits of a reference value, rather than a fixed width. The +# style lint would force a regrouping that obscures the source of a value. +inconsistent_digit_grouping = "allow" + [workspace.dependencies] apsis-core = { path = "crates/foundation/apsis-core" } apsis-units = { path = "crates/foundation/apsis-units" } diff --git a/crates/orbit/apsis-kepler/Cargo.toml b/crates/orbit/apsis-kepler/Cargo.toml index 68c207a..95096a8 100644 --- a/crates/orbit/apsis-kepler/Cargo.toml +++ b/crates/orbit/apsis-kepler/Cargo.toml @@ -11,3 +11,7 @@ publish = false workspace = true [dependencies] +apsis-core.workspace = true +apsis-math.workspace = true +apsis-time.workspace = true +apsis-tle.workspace = true diff --git a/crates/orbit/apsis-kepler/src/anomaly.rs b/crates/orbit/apsis-kepler/src/anomaly.rs new file mode 100644 index 0000000..d5d82d4 --- /dev/null +++ b/crates/orbit/apsis-kepler/src/anomaly.rs @@ -0,0 +1,125 @@ +//! Conversions between the three anomalies. +//! +//! An anomaly is an angle that measures how far a satellite has travelled +//! around its orbit. The three forms used here are: +//! +//! - The true anomaly, the angle from perigee to the satellite as seen from +//! the focus where the central body sits. It is the only one with a direct +//! geometric meaning. +//! - The eccentric anomaly, the angle from perigee to the projection of the +//! satellite onto the auxiliary circle of the ellipse, as seen from the +//! center of the ellipse. +//! - The mean anomaly, a linear function of time that is proportional to the +//! area swept since perigee. It has no direct geometric meaning, but it is +//! the form in which an orbit propagates in time. +//! +//! Kepler's equation relates the mean and eccentric anomalies and has no +//! closed form solution. It is solved here by Newton's method from a +//! starting guess that is accurate enough to converge in a few iterations +//! for every eccentricity below one. +//! +//! All angles are in radians. The functions reduce their results into +//! `[-pi, pi)` so that a value is comparable without further wrapping. + +use core::f64::consts::PI; + +/// The maximum number of Newton iterations used to solve Kepler's equation. +const MAX_ITERATIONS: usize = 30; + +/// The convergence tolerance of the Newton iteration, in radians. +const TOLERANCE: f64 = 1e-14; + +/// Reduces an angle into `[-pi, pi)`. +pub fn wrap_to_pi(angle: f64) -> f64 { + let wrapped = (angle + PI).rem_euclid(2.0 * PI) - PI; + // `rem_euclid` can return exactly pi for an input just below pi, which + // the half-open interval excludes. + if wrapped >= PI { + wrapped - 2.0 * PI + } else { + wrapped + } +} + +/// Returns the true anomaly for an eccentric anomaly and eccentricity. +/// +/// The relation follows from the geometry of the auxiliary circle: +/// +/// ```text +/// tan(nu / 2) = sqrt((1 + e) / (1 - e)) tan(E / 2) +/// ``` +/// +/// The half angle form is used rather than the direct cosine relation so +/// that the result is correct in all four quadrants without a quadrant +/// check. +pub fn true_from_eccentric(eccentric_anomaly: f64, eccentricity: f64) -> f64 { + let half = eccentric_anomaly / 2.0; + let factor = ((1.0 + eccentricity) / (1.0 - eccentricity)).sqrt(); + wrap_to_pi(2.0 * (factor * half.tan()).atan()) +} + +/// Returns the eccentric anomaly for a true anomaly and eccentricity. +/// +/// This is the inverse of [`true_from_eccentric`]. +pub fn eccentric_from_true(true_anomaly: f64, eccentricity: f64) -> f64 { + let half = true_anomaly / 2.0; + let factor = ((1.0 - eccentricity) / (1.0 + eccentricity)).sqrt(); + wrap_to_pi(2.0 * (factor * half.tan()).atan()) +} + +/// Returns the eccentric anomaly for a mean anomaly and eccentricity. +/// +/// This solves Kepler's equation +/// +/// ```text +/// M = E - e sin E +/// ``` +/// +/// by Newton's method. The starting guess is the mean anomaly plus a term +/// proportional to the eccentricity, which keeps the iteration stable for +/// eccentricities approaching one. The iteration always terminates, and the +/// result is the value reached after at most [`MAX_ITERATIONS`] steps. +pub fn eccentric_from_mean(mean_anomaly: f64, eccentricity: f64) -> f64 { + let mean = wrap_to_pi(mean_anomaly); + // A first order correction to the circular guess. + let mut eccentric = mean + eccentricity * mean.sin() * (1.0 + eccentricity * mean.cos()); + for _ in 0..MAX_ITERATIONS { + let residual = eccentric - eccentricity * eccentric.sin() - mean; + let derivative = 1.0 - eccentricity * eccentric.cos(); + if derivative.abs() < f64::MIN_POSITIVE { + break; + } + let step = residual / derivative; + eccentric -= step; + if step.abs() < TOLERANCE { + break; + } + } + wrap_to_pi(eccentric) +} + +/// Returns the mean anomaly for an eccentric anomaly and eccentricity. +/// +/// This is Kepler's equation evaluated directly, `M = E - e sin E`. +pub fn mean_from_eccentric(eccentric_anomaly: f64, eccentricity: f64) -> f64 { + wrap_to_pi(eccentric_anomaly - eccentricity * eccentric_anomaly.sin()) +} + +/// Returns the mean anomaly for a true anomaly and eccentricity. +/// +/// The true anomaly is first converted to the eccentric anomaly, and the +/// mean anomaly then follows from Kepler's equation. Going through the +/// eccentric anomaly avoids the separate closed form for the swept area and +/// keeps a single expression responsible for the relation. +pub fn mean_from_true(true_anomaly: f64, eccentricity: f64) -> f64 { + let eccentric_anomaly = eccentric_from_true(true_anomaly, eccentricity); + mean_from_eccentric(eccentric_anomaly, eccentricity) +} + +/// Returns the true anomaly for a mean anomaly and eccentricity. +pub fn true_from_mean(mean_anomaly: f64, eccentricity: f64) -> f64 { + true_from_eccentric( + eccentric_from_mean(mean_anomaly, eccentricity), + eccentricity, + ) +} diff --git a/crates/orbit/apsis-kepler/src/elements.rs b/crates/orbit/apsis-kepler/src/elements.rs new file mode 100644 index 0000000..09d1e63 --- /dev/null +++ b/crates/orbit/apsis-kepler/src/elements.rs @@ -0,0 +1,359 @@ +//! Keplerian elements and their conversion to a Cartesian state. + +use crate::anomaly::{eccentric_from_mean, mean_from_eccentric, true_from_eccentric, wrap_to_pi}; +use crate::parameter::GravityParameter; +use apsis_math::Vec3; + +/// The six Keplerian elements of an orbit. +/// +/// The shape and size are given by the semi-major axis and the eccentricity, +/// the orientation by the inclination, the right ascension of the ascending +/// node, and the argument of perigee, and the position along the orbit by +/// the true anomaly. +/// +/// Angles are in radians and the semi-major axis is in kilometers. The orbit +/// is elliptical, so the eccentricity must be below one. +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct KeplerianElements { + /// Semi-major axis, in kilometers. Must be positive. + pub semi_major_axis: f64, + /// Eccentricity, dimensionless. Must be in `[0, 1)`. + pub eccentricity: f64, + /// Inclination, in radians. Must be in `[0, pi]`. + pub inclination: f64, + /// Right ascension of the ascending node, in radians. + pub right_ascension: f64, + /// Argument of perigee, in radians. + pub argument_of_perigee: f64, + /// True anomaly, in radians. + pub true_anomaly: f64, +} + +/// The position and velocity of a satellite in the inertial frame the +/// elements are referred to. +/// +/// Position is in kilometers and velocity is in kilometers per second. The +/// frame has its first axis toward the reference direction, its third axis +/// along the orbit normal, and its second completing a right-handed set. +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct State { + /// Position, in kilometers. + pub position: Vec3, + /// Velocity, in kilometers per second. + pub velocity: Vec3, +} + +impl KeplerianElements { + /// Returns `true` when the elements describe an elliptical orbit. + pub fn is_valid(self) -> bool { + self.semi_major_axis.is_finite() + && self.semi_major_axis > 0.0 + && self.eccentricity.is_finite() + && (0.0..1.0).contains(&self.eccentricity) + && self.inclination.is_finite() + && (0.0..=core::f64::consts::PI).contains(&self.inclination) + && self.right_ascension.is_finite() + && self.argument_of_perigee.is_finite() + && self.true_anomaly.is_finite() + } + + /// Returns the semi-latus rectum, in kilometers. + /// + /// This is `a (1 - e^2)`, the distance from the focus to the orbit + /// measured perpendicular to the major axis. + pub fn semi_latus_rectum(self) -> f64 { + self.semi_major_axis * (1.0 - self.eccentricity * self.eccentricity) + } + + /// Returns the semi-minor axis, in kilometers. + pub fn semi_minor_axis(self) -> f64 { + self.semi_major_axis * (1.0 - self.eccentricity * self.eccentricity).sqrt() + } + + /// Returns the perigee radius, in kilometers. + /// + /// This is the closest approach to the focus, `a (1 - e)`. + pub fn perigee_radius(self) -> f64 { + self.semi_major_axis * (1.0 - self.eccentricity) + } + + /// Returns the apogee radius, in kilometers. + /// + /// This is the farthest point from the focus, `a (1 + e)`. + pub fn apogee_radius(self) -> f64 { + self.semi_major_axis * (1.0 + self.eccentricity) + } + + /// Returns the orbital period, in seconds. + /// + /// This is Kepler's third law, `T = 2 pi sqrt(a^3 / mu)`. + pub fn period(self, parameter: GravityParameter) -> f64 { + 2.0 * core::f64::consts::PI * (self.semi_major_axis.powi(3) / parameter.value()).sqrt() + } + + /// Returns the eccentric anomaly corresponding to the true anomaly. + pub fn eccentric_anomaly(self) -> f64 { + crate::anomaly::eccentric_from_true(self.true_anomaly, self.eccentricity) + } + + /// Returns the mean anomaly corresponding to the true anomaly. + pub fn mean_anomaly(self) -> f64 { + crate::anomaly::mean_from_true(self.true_anomaly, self.eccentricity) + } + + /// Converts these elements to a position and velocity. + /// + /// Returns `None` when the elements are not a valid elliptical orbit. + /// + /// The conversion places the satellite in the perifocal frame, where the + /// first axis points to perigee and the third along the orbit normal, + /// + /// ```text + /// r = p / (1 + e cos nu) + /// x = r cos nu, y = r sin nu, z = 0 + /// vx = -sqrt(mu / p) sin nu + /// vy = sqrt(mu / p) (e + cos nu) + /// ``` + /// + /// and then rotates by the argument of perigee, the inclination, and the + /// right ascension of the ascending node. + pub fn to_state(self, parameter: GravityParameter) -> Option { + if !self.is_valid() { + return None; + } + let p = self.semi_latus_rectum(); + let mu = parameter.value(); + let cos_nu = self.true_anomaly.cos(); + let sin_nu = self.true_anomaly.sin(); + let radius = p / (1.0 + self.eccentricity * cos_nu); + let scale = (mu / p).sqrt(); + + let perifocal_position = Vec3::new(radius * cos_nu, radius * sin_nu, 0.0); + let perifocal_velocity = + Vec3::new(-scale * sin_nu, scale * (self.eccentricity + cos_nu), 0.0); + + // Rotate from the perifocal frame to the inertial frame by the + // argument of perigee about the third axis, the inclination about + // the first, and the right ascension about the third. + let rotation = perifocal_to_inertial( + self.argument_of_perigee, + self.inclination, + self.right_ascension, + ); + Some(State { + position: rotation * perifocal_position, + velocity: rotation * perifocal_velocity, + }) + } + + /// Returns the elements for a position and velocity. + /// + /// Returns `None` when the state is not finite, when the radius is zero, + /// or when the orbit is not elliptical. This conversion assumes the + /// orbit is bound, so a parabolic or hyperbolic state is rejected rather + /// than returned with an eccentricity at or above one. + pub fn from_state(state: &State, parameter: GravityParameter) -> Option { + let position = state.position; + let velocity = state.velocity; + if !position.is_finite() || !velocity.is_finite() { + return None; + } + let mu = parameter.value(); + let radius = position.norm(); + if radius == 0.0 { + return None; + } + + let velocity_squared = velocity.norm_squared(); + let radial_velocity = position.dot(velocity) / radius; + + // The specific angular momentum and the node vector, which together + // give the orientation. + let angular_momentum = position.cross(velocity); + let angular_momentum_norm = angular_momentum.norm(); + if angular_momentum_norm == 0.0 { + // A purely radial trajectory has no defined orbit plane. + return None; + } + + // The eccentricity vector points toward perigee and has magnitude + // equal to the eccentricity. + let eccentricity_vector = + position * (velocity_squared - mu / radius) - velocity * (radius * radial_velocity); + let eccentricity = eccentricity_vector.norm() / mu; + if !(0.0..1.0).contains(&eccentricity) { + return None; + } + + // The specific mechanical energy gives the semi-major axis. + let energy = velocity_squared / 2.0 - mu / radius; + if energy == 0.0 { + return None; + } + let semi_major_axis = -mu / (2.0 * energy); + if semi_major_axis <= 0.0 || !semi_major_axis.is_finite() { + return None; + } + + let node = Vec3::Z.cross(angular_momentum); + let node_norm = node.norm(); + let inclination = (angular_momentum.z / angular_momentum_norm) + .clamp(-1.0, 1.0) + .acos(); + + // The node and the argument of perigee are undefined for a zero + // inclination and for a circular orbit respectively; in those cases + // the standard convention sets them to zero and measures the angle + // from the reference direction instead. + let right_ascension = if node_norm == 0.0 { + 0.0 + } else { + wrap_to_pi(node.y.atan2(node.x)) + }; + + let argument_of_perigee = if eccentricity < 1e-12 { + 0.0 + } else if node_norm == 0.0 { + // Equatorial orbit: measure the argument from the first axis. + wrap_to_pi(eccentricity_vector.y.atan2(eccentricity_vector.x)) + } else { + let value = (node.dot(eccentricity_vector) / (node_norm * eccentricity_vector.norm())) + .clamp(-1.0, 1.0); + let angle = value.acos(); + if eccentricity_vector.z < 0.0 { + wrap_to_pi(-angle) + } else { + wrap_to_pi(angle) + } + }; + + // The true anomaly follows from the eccentric anomaly recovered + // from the radial distance and the radial velocity, which is more + // accurate near a circular orbit than the angle between vectors. + let true_anomaly = true_anomaly_from_state(position, velocity, eccentricity, mu); + + let elements = Self { + semi_major_axis, + eccentricity, + inclination, + right_ascension, + argument_of_perigee, + true_anomaly, + }; + elements.is_valid().then_some(elements) + } + + /// Returns a copy of these elements with a different true anomaly. + pub fn with_true_anomaly(self, true_anomaly: f64) -> Self { + Self { + true_anomaly, + ..self + } + } + + /// Returns a copy of these elements at a mean anomaly. + /// + /// The true anomaly is recovered from the mean anomaly through Kepler's + /// equation. + pub fn with_mean_anomaly(self, mean_anomaly: f64) -> Self { + let eccentric = eccentric_from_mean(mean_anomaly, self.eccentricity); + Self { + true_anomaly: true_from_eccentric(eccentric, self.eccentricity), + ..self + } + } + + /// Returns the mean anomaly, which is the linear measure of time along + /// the orbit. + /// + /// This is the same value as [`KeplerianElements::mean_anomaly`], named + /// for symmetry with [`KeplerianElements::with_mean_anomaly`]. + pub fn to_mean_anomaly(self) -> f64 { + mean_from_eccentric(self.eccentric_anomaly(), self.eccentricity) + } +} + +/// Returns the rotation from the perifocal frame to the inertial frame. +/// +/// The rotation is a product of three elementary rotations: by the argument +/// of perigee about the third axis, then the inclination about the first, +/// then the right ascension about the third. +pub(crate) fn perifocal_to_inertial( + argument_of_perigee: f64, + inclination: f64, + right_ascension: f64, +) -> apsis_math::Mat3 { + use apsis_math::Mat3; + let (sin_w, cos_w) = argument_of_perigee.sin_cos(); + let (sin_i, cos_i) = inclination.sin_cos(); + let (sin_o, cos_o) = right_ascension.sin_cos(); + + // Expanded product R3(O) R1(i) R3(w). Writing it out avoids three matrix + // products and keeps the result well conditioned. + Mat3::from_rows( + [ + cos_o * cos_w - sin_o * sin_w * cos_i, + -cos_o * sin_w - sin_o * cos_w * cos_i, + sin_o * sin_i, + ], + [ + sin_o * cos_w + cos_o * sin_w * cos_i, + -sin_o * sin_w + cos_o * cos_w * cos_i, + -cos_o * sin_i, + ], + [sin_w * sin_i, cos_w * sin_i, cos_i], + ) +} + +/// Recovers the true anomaly from a state vector. +/// +/// Using the eccentric anomaly as an intermediate keeps the result accurate +/// for near circular orbits, where the eccentricity vector is short and its +/// direction is poorly determined. The eccentric anomaly follows from the +/// radial distance and the radial velocity through +/// +/// ```text +/// cos E = (1 - r / a) / e +/// sin E = r rdot / (e sqrt(mu a)) +/// ``` +/// +/// and the true anomaly follows from the eccentric anomaly. A circular +/// orbit, where the eccentricity is too small for those expressions, is +/// handled by measuring the angle from the node instead, which is the same +/// convention [`KeplerianElements::from_state`] uses for the argument of +/// perigee. +fn true_anomaly_from_state(position: Vec3, velocity: Vec3, eccentricity: f64, mu: f64) -> f64 { + let radius = position.norm(); + let radial_velocity = position.dot(velocity) / radius; + let semi_major_axis = 1.0 / (2.0 / radius - velocity.norm_squared() / mu); + if !semi_major_axis.is_finite() || semi_major_axis <= 0.0 { + return 0.0; + } + + // Below this eccentricity the angle is recovered from the node instead, + // because the division by the eccentricity would amplify the rounding + // in the radial terms. + const CIRCULAR_THRESHOLD: f64 = 1e-8; + if eccentricity < CIRCULAR_THRESHOLD { + let angular_momentum = position.cross(velocity); + let node = Vec3::Z.cross(angular_momentum); + if node.norm() == 0.0 { + // Equatorial circular orbit: measure from the first axis. + return wrap_to_pi(position.y.atan2(position.x)); + } + // The argument of latitude is the angle from the node to the + // position, measured in the orbit plane. + let node_unit = node.normalize().expect("node is non-zero"); + let in_plane = position.normalize().expect("radius is non-zero"); + let cross = node_unit.cross(in_plane); + let along = angular_momentum + .normalize() + .expect("angular momentum is non-zero"); + return wrap_to_pi(cross.dot(along).atan2(node_unit.dot(in_plane))); + } + + let sin_e = radius * radial_velocity / (eccentricity * (mu * semi_major_axis).sqrt()); + let cos_e = (1.0 - radius / semi_major_axis) / eccentricity; + let eccentric_anomaly = sin_e.atan2(cos_e); + true_from_eccentric(eccentric_anomaly, eccentricity) +} diff --git a/crates/orbit/apsis-kepler/src/lib.rs b/crates/orbit/apsis-kepler/src/lib.rs index b55745f..3648162 100644 --- a/crates/orbit/apsis-kepler/src/lib.rs +++ b/crates/orbit/apsis-kepler/src/lib.rs @@ -1 +1,49 @@ //! Keplerian orbital elements and two-body propagation. +//! +//! A Keplerian element set describes an orbit as an ellipse, a parabola, or +//! a hyperbola with a fixed orientation in space. From the six elements and +//! the gravitational parameter, this crate computes the position and +//! velocity at any time under the two-body assumption, and converts in the +//! other direction. +//! +//! The two-body model includes only the point mass gravity of the central +//! body. It omits the oblateness of the Earth, drag, and third body +//! perturbations, so it drifts from reality over time. It is the right model +//! for analysing an orbit whose elements are already known, for example to +//! find the radius at a true anomaly, and the wrong model for propagating a +//! two-line element set, which is the job of `apsis-sgp4`. +//! +//! Angles are in radians. Lengths are kilometers and the gravitational +//! parameter is in cubic kilometers per second squared, matching the rest of +//! the orbit layer. +//! +//! ``` +//! use apsis_kepler::{GravityParameter, KeplerianElements}; +//! +//! // A circular orbit at the Earth's equatorial radius has the speed +//! // sqrt(mu / r), about 7.9 km/s. +//! let elements = KeplerianElements { +//! semi_major_axis: 6_378.137, +//! eccentricity: 0.0, +//! inclination: 0.0, +//! right_ascension: 0.0, +//! argument_of_perigee: 0.0, +//! true_anomaly: 0.0, +//! }; +//! let state = elements.to_state(GravityParameter::WGS84).expect("valid elements"); +//! assert!((state.velocity.norm() - 7.909).abs() < 0.01); +//! ``` + +#![forbid(unsafe_code)] +#![deny(missing_docs)] + +mod anomaly; +mod elements; +mod parameter; + +pub use anomaly::{ + eccentric_from_mean, eccentric_from_true, mean_from_eccentric, mean_from_true, + true_from_eccentric, true_from_mean, wrap_to_pi, +}; +pub use elements::{KeplerianElements, State}; +pub use parameter::GravityParameter; diff --git a/crates/orbit/apsis-kepler/src/parameter.rs b/crates/orbit/apsis-kepler/src/parameter.rs new file mode 100644 index 0000000..2d2403a --- /dev/null +++ b/crates/orbit/apsis-kepler/src/parameter.rs @@ -0,0 +1,30 @@ +//! The Earth gravitational parameter. + +/// The gravitational parameter of the central body, in cubic kilometers per +/// second squared. +/// +/// Two values are offered because the workspace supports both gravity +/// models, and the correct choice is the one that matches the source of the +/// orbital elements. For elements derived from a two-line element set, the +/// WGS 72 value is the one the SGP4 theory was fitted to. +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct GravityParameter(pub f64); + +impl GravityParameter { + /// The WGS 72 gravitational parameter, `398 600.8 km^3/s^2`. + pub const WGS72: Self = Self(398_600.8); + + /// The WGS 84 gravitational parameter, `398 600.5 km^3/s^2`. + pub const WGS84: Self = Self(398_600.5); + + /// Returns the value in cubic kilometers per second squared. + pub const fn value(self) -> f64 { + self.0 + } + + /// Returns the square root of the parameter, in kilometers to the three + /// halves per second. + pub fn sqrt(self) -> f64 { + self.0.sqrt() + } +} diff --git a/crates/orbit/apsis-kepler/tests/anomaly.rs b/crates/orbit/apsis-kepler/tests/anomaly.rs new file mode 100644 index 0000000..643835e --- /dev/null +++ b/crates/orbit/apsis-kepler/tests/anomaly.rs @@ -0,0 +1,104 @@ +//! Tests for the anomaly conversions. + +use apsis_kepler::{ + eccentric_from_mean, eccentric_from_true, mean_from_eccentric, mean_from_true, + true_from_eccentric, true_from_mean, wrap_to_pi, +}; +use core::f64::consts::PI; + +#[test] +fn wrap_places_angles_in_the_half_open_interval() { + for turns in -3..=3 { + for offset in [0.0, 0.25, 0.5, 0.75] { + let angle = PI + (f64::from(turns) + offset) * 2.0 * PI; + let wrapped = wrap_to_pi(angle); + assert!( + (-PI..PI).contains(&wrapped), + "angle {angle} wrapped to {wrapped}" + ); + // The result differs from the input by a whole number of turns. + let turns_removed = (angle - wrapped) / (2.0 * PI); + assert!((turns_removed - turns_removed.round()).abs() < 1e-9); + } + } +} + +#[test] +fn wrap_keeps_values_already_in_range() { + for angle in [-3.0, -1.0, 0.0, 1.0, 3.0] { + assert!((wrap_to_pi(angle) - angle).abs() < 1e-12); + } +} + +#[test] +fn keplers_equation_holds_for_the_solution() { + // M = E - e sin E must be satisfied by the returned eccentric anomaly. + for eccentricity in [0.0, 0.01, 0.1, 0.5, 0.9, 0.99] { + for mean in [-3.0, -1.0, 0.0, 0.5, 1.5, 3.0] { + let eccentric = eccentric_from_mean(mean, eccentricity); + let recovered = mean_from_eccentric(eccentric, eccentricity); + assert!( + (recovered - mean).abs() < 1e-9, + "e={eccentricity} M={mean} recovered={recovered}" + ); + } + } +} + +#[test] +fn eccentric_and_true_anomaly_round_trip() { + for eccentricity in [0.0, 0.05, 0.3, 0.7, 0.95] { + for true_anomaly in [-3.0, -1.0, 0.0, 0.25, 1.0, 3.0] { + let eccentric = eccentric_from_true(true_anomaly, eccentricity); + let back = true_from_eccentric(eccentric, eccentricity); + assert!( + (back - true_anomaly).abs() < 1e-9, + "e={eccentricity} nu={true_anomaly} back={back}" + ); + } + } +} + +#[test] +fn mean_and_true_anomaly_round_trip() { + for eccentricity in [0.0, 0.05, 0.3, 0.7, 0.95] { + for true_anomaly in [-2.5, -0.5, 0.0, 0.75, 2.0] { + let mean = mean_from_true(true_anomaly, eccentricity); + let back = true_from_mean(mean, eccentricity); + assert!( + (back - true_anomaly).abs() < 1e-8, + "e={eccentricity} nu={true_anomaly} back={back}" + ); + } + } +} + +#[test] +fn a_circular_orbit_has_equal_anomalies() { + // With zero eccentricity every anomaly is the same angle. + for angle in [-2.0, -0.5, 0.0, 1.0, 2.5] { + assert!((eccentric_from_mean(angle, 0.0) - angle).abs() < 1e-12); + assert!((true_from_eccentric(angle, 0.0) - angle).abs() < 1e-12); + assert!((true_from_mean(angle, 0.0) - angle).abs() < 1e-12); + } +} + +#[test] +fn anomalies_at_perigee_are_zero() { + for eccentricity in [0.0, 0.3, 0.8] { + assert!(eccentric_from_mean(0.0, eccentricity).abs() < 1e-12); + assert!(true_from_mean(0.0, eccentricity).abs() < 1e-12); + assert!(mean_from_true(0.0, eccentricity).abs() < 1e-12); + } +} + +#[test] +fn true_anomaly_exceeds_mean_anomaly_before_apogee() { + // For an eccentric orbit the satellite spends less time near perigee, + // so the true anomaly runs ahead of the mean anomaly in the first half + // of the orbit. + let eccentricity = 0.5; + let mean = 1.0; + let true_anomaly = true_from_mean(mean, eccentricity); + assert!(true_anomaly > mean, "nu={true_anomaly} M={mean}"); +} diff --git a/crates/orbit/apsis-kepler/tests/elements.rs b/crates/orbit/apsis-kepler/tests/elements.rs new file mode 100644 index 0000000..aef70d7 --- /dev/null +++ b/crates/orbit/apsis-kepler/tests/elements.rs @@ -0,0 +1,335 @@ +//! Tests for the Keplerian elements and their state vector conversions. +//! +//! The expected values were computed independently from the standard +//! perifocal frame expressions in extended precision. Only the point mass +//! gravity of the central body is involved, so the two should agree to +//! floating point rounding. + +use apsis_kepler::{GravityParameter, KeplerianElements, State, wrap_to_pi}; +use apsis_math::Vec3; +use core::f64::consts::{PI, TAU}; + +const POSITION_TOLERANCE: f64 = 1e-6; +const VELOCITY_TOLERANCE: f64 = 1e-9; + +fn assert_state_close(state: State, position: [f64; 3], velocity: [f64; 3]) { + let position_error = (state.position - Vec3::new(position[0], position[1], position[2])).norm(); + let velocity_error = (state.velocity - Vec3::new(velocity[0], velocity[1], velocity[2])).norm(); + assert!( + position_error < POSITION_TOLERANCE, + "position error {position_error}, got {}", + state.position + ); + assert!( + velocity_error < VELOCITY_TOLERANCE, + "velocity error {velocity_error}, got {}", + state.velocity + ); +} + +fn low_orbit() -> KeplerianElements { + KeplerianElements { + semi_major_axis: 7_000.0, + eccentricity: 0.01, + inclination: 0.9, + right_ascension: 1.2, + argument_of_perigee: 0.4, + true_anomaly: 0.7, + } +} + +fn elliptical_orbit() -> KeplerianElements { + KeplerianElements { + semi_major_axis: 26_600.0, + eccentricity: 0.74, + inclination: 0.1, + right_ascension: 2.0, + argument_of_perigee: 5.0, + true_anomaly: 1.1, + } +} + +#[test] +fn conversion_matches_an_independent_computation() { + let state = low_orbit() + .to_state(GravityParameter::WGS84) + .expect("elements are valid"); + assert_state_close( + state, + [-2444.845_044_819, 4331.004_606_023, 4849.169_779_458], + [-4.471_116_396_765, -5.509_080_648_689, 2.735_799_894_571], + ); +} + +#[test] +fn conversion_matches_for_a_highly_elliptical_orbit() { + let state = elliptical_orbit() + .to_state(GravityParameter::WGS84) + .expect("elements are valid"); + assert_state_close( + state, + [-2201.703_284_413, 8734.956_144_841, -163.848_650_213], + [-8.348_843_969_446, 1.823_413_050_670, 0.685_564_215_543], + ); +} + +#[test] +fn a_circular_equatorial_orbit_has_the_circular_speed() { + // At zero eccentricity, zero inclination, and zero orientation, the + // satellite sits on the first axis and moves along the second, with + // speed sqrt(mu / a). + let elements = KeplerianElements { + semi_major_axis: 6_378.137, + eccentricity: 0.0, + inclination: 0.0, + right_ascension: 0.0, + argument_of_perigee: 0.0, + true_anomaly: 0.0, + }; + let state = elements + .to_state(GravityParameter::WGS84) + .expect("elements are valid"); + assert!((state.position.x - 6_378.137).abs() < 1e-9); + assert!(state.position.y.abs() < 1e-9); + assert!(state.position.z.abs() < 1e-9); + + let expected_speed = (GravityParameter::WGS84.value() / 6_378.137).sqrt(); + assert!((state.velocity.norm() - expected_speed).abs() < 1e-9); + assert!(state.velocity.y > 0.0); +} + +#[test] +fn the_state_vector_lies_in_the_orbit_plane() { + let elements = low_orbit(); + let state = elements + .to_state(GravityParameter::WGS84) + .expect("elements are valid"); + // The position and velocity span the orbit plane, so the normal to that + // plane is perpendicular to both. + let normal = state + .position + .cross(state.velocity) + .normalize() + .expect("non-zero"); + assert!(normal.dot(state.position).abs() < 1e-6); + assert!(normal.dot(state.velocity).abs() < 1e-9); +} + +#[test] +fn radius_follows_the_conic_equation() { + let elements = low_orbit(); + for true_anomaly in [-2.0, -1.0, 0.0, 1.0, 2.0, 3.0] { + let state = elements + .with_true_anomaly(true_anomaly) + .to_state(GravityParameter::WGS84) + .expect("elements are valid"); + let p = elements.semi_latus_rectum(); + let expected = p / (1.0 + elements.eccentricity * true_anomaly.cos()); + assert!((state.position.norm() - expected).abs() < 1e-6); + } +} + +#[test] +fn perigee_and_apogee_are_the_extremes_of_the_radius() { + let elements = elliptical_orbit(); + let perigee = elements + .with_true_anomaly(0.0) + .to_state(GravityParameter::WGS84) + .expect("valid"); + let apogee = elements + .with_true_anomaly(PI) + .to_state(GravityParameter::WGS84) + .expect("valid"); + assert!((perigee.position.norm() - elements.perigee_radius()).abs() < 1e-6); + assert!((apogee.position.norm() - elements.apogee_radius()).abs() < 1e-6); + assert!(perigee.position.norm() < apogee.position.norm()); +} + +#[test] +fn the_speed_is_highest_at_perigee() { + let elements = elliptical_orbit(); + let perigee = elements + .with_true_anomaly(0.0) + .to_state(GravityParameter::WGS84) + .expect("valid"); + let apogee = elements + .with_true_anomaly(PI) + .to_state(GravityParameter::WGS84) + .expect("valid"); + assert!(perigee.velocity.norm() > apogee.velocity.norm()); +} + +#[test] +fn orbital_geometry_is_consistent() { + let elements = elliptical_orbit(); + let p = elements.semi_latus_rectum(); + assert!((p - elements.semi_major_axis * (1.0 - elements.eccentricity.powi(2))).abs() < 1e-9); + assert!((elements.semi_minor_axis().powi(2) - elements.semi_major_axis * p).abs() < 1e-6); + assert!( + (elements.apogee_radius() + - elements.perigee_radius() + - 2.0 * elements.semi_major_axis * elements.eccentricity) + .abs() + < 1e-9 + ); +} + +#[test] +fn the_period_follows_keplers_third_law() { + let elements = low_orbit(); + let period = elements.period(GravityParameter::WGS84); + let expected = + 2.0 * PI * (elements.semi_major_axis.powi(3) / GravityParameter::WGS84.value()).sqrt(); + assert!((period - expected).abs() < 1e-9); + // A 7000 km orbit has a period of about 97 minutes. + assert!((period / 60.0 - 97.0).abs() < 2.0); +} + +#[test] +fn state_round_trips_through_the_elements() { + for elements in [low_orbit(), elliptical_orbit()] { + let state = elements + .to_state(GravityParameter::WGS84) + .expect("elements are valid"); + let recovered = KeplerianElements::from_state(&state, GravityParameter::WGS84) + .expect("state describes an ellipse"); + + assert!( + (recovered.semi_major_axis - elements.semi_major_axis).abs() < 1e-6, + "semi-major axis {} vs {}", + recovered.semi_major_axis, + elements.semi_major_axis + ); + assert!( + (recovered.eccentricity - elements.eccentricity).abs() < 1e-9, + "eccentricity {} vs {}", + recovered.eccentricity, + elements.eccentricity + ); + assert!((recovered.inclination - elements.inclination).abs() < 1e-9); + // The angles are compared modulo a full turn, because a value such + // as 5.0 radians and one reduced to -1.28 radians describe the same + // orientation. + assert!((wrap_to_pi(recovered.right_ascension - elements.right_ascension)).abs() < 1e-9); + assert!( + (wrap_to_pi(recovered.argument_of_perigee - elements.argument_of_perigee)).abs() < 1e-9 + ); + assert!((wrap_to_pi(recovered.true_anomaly - elements.true_anomaly)).abs() < 1e-7); + + // The round trip must reproduce the state as well as the elements, + // which is the stronger check. + let again = recovered.to_state(GravityParameter::WGS84).expect("valid"); + assert!((again.position - state.position).norm() < 1e-6); + assert!((again.velocity - state.velocity).norm() < 1e-9); + } +} + +#[test] +fn a_circular_orbit_round_trips() { + let elements = KeplerianElements { + semi_major_axis: 7_000.0, + eccentricity: 0.0, + inclination: 1.0, + right_ascension: 0.5, + argument_of_perigee: 0.0, + true_anomaly: 0.0, + }; + let state = elements + .to_state(GravityParameter::WGS72) + .expect("elements are valid"); + let recovered = KeplerianElements::from_state(&state, GravityParameter::WGS72) + .expect("state describes an ellipse"); + assert!((recovered.semi_major_axis - 7_000.0).abs() < 1e-6); + assert!(recovered.eccentricity < 1e-9); + assert!((recovered.inclination - 1.0).abs() < 1e-9); + assert!((recovered.right_ascension - 0.5).abs() < 1e-9); +} + +#[test] +fn mean_anomaly_round_trips_through_the_elements() { + let elements = elliptical_orbit(); + let mean = elements.to_mean_anomaly(); + let restored = elements.with_mean_anomaly(mean); + assert!((restored.true_anomaly - elements.true_anomaly).abs() < 1e-8); +} + +#[test] +fn invalid_elements_are_rejected() { + let mut elements = low_orbit(); + elements.eccentricity = 1.0; + assert!(!elements.is_valid()); + assert!(elements.to_state(GravityParameter::WGS84).is_none()); + + let mut elements = low_orbit(); + elements.semi_major_axis = -1.0; + assert!(!elements.is_valid()); + assert!(elements.to_state(GravityParameter::WGS84).is_none()); + + let mut elements = low_orbit(); + elements.inclination = PI + 0.1; + assert!(!elements.is_valid()); + + let mut elements = low_orbit(); + elements.true_anomaly = f64::NAN; + assert!(!elements.is_valid()); +} + +#[test] +fn degenerate_states_are_rejected() { + assert_eq!( + KeplerianElements::from_state( + &State { + position: Vec3::ZERO, + velocity: Vec3::ZERO + }, + GravityParameter::WGS84 + ), + None + ); + // A purely radial state has no defined orbit plane. + assert_eq!( + KeplerianElements::from_state( + &State { + position: Vec3::new(7_000.0, 0.0, 0.0), + velocity: Vec3::new(1.0, 0.0, 0.0) + }, + GravityParameter::WGS84 + ), + None + ); + // An unbound state has a non-positive semi-major axis. + assert_eq!( + KeplerianElements::from_state( + &State { + position: Vec3::new(7_000.0, 0.0, 0.0), + velocity: Vec3::new(0.0, 20.0, 0.0) + }, + GravityParameter::WGS84 + ), + None + ); +} + +#[test] +fn gravity_parameter_values_match_the_standards() { + assert_eq!(GravityParameter::WGS72.value(), 398_600.8); + assert_eq!(GravityParameter::WGS84.value(), 398_600.5); + assert!((GravityParameter::WGS84.sqrt() - 398_600.5_f64.sqrt()).abs() < 1e-9); +} + +#[test] +fn a_full_revolution_returns_to_the_same_point() { + // Advancing the mean anomaly by one full turn must return the same + // position, which verifies the anomaly conversions agree. + let elements = elliptical_orbit(); + let start = elements + .with_mean_anomaly(1.0) + .to_state(GravityParameter::WGS84) + .expect("valid"); + let end = elements + .with_mean_anomaly(1.0 + TAU) + .to_state(GravityParameter::WGS84) + .expect("valid"); + assert!((start.position - end.position).norm() < 1e-6); + assert!((start.velocity - end.velocity).norm() < 1e-9); +}