orbit: add Keplerian elements and two-body propagation

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"

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" }

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

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,
+    )
+}

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<State> {
+        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<Self> {
+        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)
+}

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;

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()
+    }
+}

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}");
+}

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);
+}