From fc23571d4a917ee6c68b7794869fccb39f6479cd Mon Sep 17 00:00:00 2001 From: ApsisRS contributors Date: Sat, 26 Sep 2026 23:11:00 +0800 Subject: [PATCH] orbit: add ephemeris sampling and Hermite interpolation --- Cargo.lock | 5 + crates/orbit/apsis-ephemeris/Cargo.toml | 5 + crates/orbit/apsis-ephemeris/src/ephemeris.rs | 297 ++++++++++++ crates/orbit/apsis-ephemeris/src/error.rs | 76 +++ crates/orbit/apsis-ephemeris/src/lib.rs | 74 +++ crates/orbit/apsis-ephemeris/src/sampler.rs | 179 +++++++ .../orbit/apsis-ephemeris/tests/ephemeris.rs | 441 ++++++++++++++++++ 7 files changed, 1077 insertions(+) create mode 100644 crates/orbit/apsis-ephemeris/src/ephemeris.rs create mode 100644 crates/orbit/apsis-ephemeris/src/error.rs create mode 100644 crates/orbit/apsis-ephemeris/src/sampler.rs create mode 100644 crates/orbit/apsis-ephemeris/tests/ephemeris.rs diff --git a/Cargo.lock b/Cargo.lock index 61e291a..ec0eac4 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -82,6 +82,11 @@ dependencies = [ [[package]] name = "apsis-ephemeris" version = "0.1.0" +dependencies = [ + "apsis-core", + "apsis-kepler", + "apsis-math", +] [[package]] name = "apsis-event" diff --git a/crates/orbit/apsis-ephemeris/Cargo.toml b/crates/orbit/apsis-ephemeris/Cargo.toml index 0881ff6..595e0e2 100644 --- a/crates/orbit/apsis-ephemeris/Cargo.toml +++ b/crates/orbit/apsis-ephemeris/Cargo.toml @@ -11,3 +11,8 @@ publish = false workspace = true [dependencies] +apsis-core.workspace = true +apsis-math.workspace = true + +[dev-dependencies] +apsis-kepler.workspace = true diff --git a/crates/orbit/apsis-ephemeris/src/ephemeris.rs b/crates/orbit/apsis-ephemeris/src/ephemeris.rs new file mode 100644 index 0000000..7e6eda8 --- /dev/null +++ b/crates/orbit/apsis-ephemeris/src/ephemeris.rs @@ -0,0 +1,297 @@ +//! The ephemeris table and its Hermite interpolation. + +use crate::error::{EphemerisError, EphemerisErrorKind}; +use apsis_math::Vec3; + +/// One sampled state. +/// +/// A sample is a position and velocity at a time, which is the form every +/// propagator in the workspace produces and the form the interpolation +/// needs. +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct Sample { + /// The time of the sample, in seconds from the caller's origin. + pub seconds: f64, + /// Position, in kilometers. + pub position: Vec3, + /// Velocity, in kilometers per second. + pub velocity: Vec3, +} + +impl Sample { + /// Creates a sample. + pub const fn new(seconds: f64, position: Vec3, velocity: Vec3) -> Self { + Self { + seconds, + position, + velocity, + } + } + + /// Returns `true` when the time, position, and velocity are finite. + pub fn is_valid(&self) -> bool { + self.seconds.is_finite() && self.position.is_finite() && self.velocity.is_finite() + } + + /// Returns the distance from the origin, in kilometers. + pub fn radius(&self) -> f64 { + self.position.norm() + } + + /// Returns the speed, in kilometers per second. + pub fn speed(&self) -> f64 { + self.velocity.norm() + } +} + +/// Evaluates the Hermite cubic for one component. +/// +/// `p0` and `p1` are the endpoint values, `v0` and `v1` the endpoint +/// derivatives with respect to time, `dt` the interval length, and `s` the +/// fraction along the interval in `[0, 1]`. +/// +/// The four terms are the standard cubic Hermite basis: the first and third +/// carry the endpoints, the second and fourth the slopes. The derivative +/// terms are scaled by `dt` because the slopes are per unit time while `s` +/// is per unit interval. +pub fn hermite_position(p0: Vec3, v0: Vec3, p1: Vec3, v1: Vec3, dt: f64, s: f64) -> Vec3 { + let s2 = s * s; + let s3 = s2 * s; + let h00 = 2.0 * s3 - 3.0 * s2 + 1.0; + let h10 = s3 - 2.0 * s2 + s; + let h01 = -2.0 * s3 + 3.0 * s2; + let h11 = s3 - s2; + p0 * h00 + v0 * (h10 * dt) + p1 * h01 + v1 * (h11 * dt) +} + +/// Evaluates the derivative of the Hermite cubic for one component. +/// +/// This is the velocity that goes with [`hermite_position`]. It is the exact +/// derivative of that cubic, so the reconstructed state is internally +/// consistent: a caller that differentiates the position gets this velocity. +pub fn hermite_velocity(p0: Vec3, v0: Vec3, p1: Vec3, v1: Vec3, dt: f64, s: f64) -> Vec3 { + let s2 = s * s; + let dh00 = 6.0 * s2 - 6.0 * s; + let dh10 = 3.0 * s2 - 4.0 * s + 1.0; + let dh01 = -6.0 * s2 + 6.0 * s; + let dh11 = 3.0 * s2 - 2.0 * s; + // The chain rule turns the per interval derivative into a per second + // one, which divides the position terms by `dt` and leaves the velocity + // terms, already per second, untouched. + p0 * (dh00 / dt) + v0 * dh10 + p1 * (dh01 / dt) + v1 * dh11 +} + +/// Evaluates both the Hermite position and velocity. +pub fn hermite_state(p0: Vec3, v0: Vec3, p1: Vec3, v1: Vec3, dt: f64, s: f64) -> (Vec3, Vec3) { + ( + hermite_position(p0, v0, p1, v1, dt, s), + hermite_velocity(p0, v0, p1, v1, dt, s), + ) +} + +/// A table of sampled states that answers queries by interpolation. +/// +/// The table holds at least two samples in strictly increasing time order. +/// A query at a sample time returns that sample exactly; a query between two +/// samples returns the Hermite cubic through the four endpoint values. A +/// query outside the covered interval is refused rather than extrapolated, +/// because the interpolation has no information there and an extrapolated +/// orbit can diverge quickly. +#[derive(Debug, Clone, PartialEq)] +pub struct Ephemeris { + samples: Vec, +} + +impl Ephemeris { + /// Builds a table from samples in increasing time order. + /// + /// Returns an error when there are fewer than two samples, when a sample + /// is not finite, or when the times do not strictly increase. + pub fn from_samples(samples: Vec) -> Result { + if samples.len() < 2 { + return Err(EphemerisError::new(EphemerisErrorKind::TooFewSamples) + .with_detail(format!("{} sample(s)", samples.len()))); + } + for sample in &samples { + if !sample.is_valid() { + return Err(EphemerisError::new(EphemerisErrorKind::InvalidState) + .with_detail(format!("at {} s", sample.seconds))); + } + } + for pair in samples.windows(2) { + if pair[1].seconds <= pair[0].seconds { + return Err( + EphemerisError::new(EphemerisErrorKind::NotMonotonic).with_detail(format!( + "{} s follows {} s", + pair[1].seconds, pair[0].seconds + )), + ); + } + } + Ok(Self { samples }) + } + + /// Returns the samples in time order. + pub fn samples(&self) -> &[Sample] { + &self.samples + } + + /// Returns the number of samples. + pub fn len(&self) -> usize { + self.samples.len() + } + + /// Returns `true` when the table is empty. + /// + /// A table built through [`Ephemeris::from_samples`] is never empty, so + /// this is always `false` for one that exists; it is provided so that + /// generic code that accepts a slice can use the usual pair of methods. + pub fn is_empty(&self) -> bool { + self.samples.is_empty() + } + + /// Returns the first sample time, in seconds. + pub fn start_time(&self) -> f64 { + self.samples.first().map_or(0.0, |sample| sample.seconds) + } + + /// Returns the last sample time, in seconds. + pub fn end_time(&self) -> f64 { + self.samples.last().map_or(0.0, |sample| sample.seconds) + } + + /// Returns the time the table covers, in seconds. + pub fn duration(&self) -> f64 { + self.end_time() - self.start_time() + } + + /// Returns `true` when a time falls within the covered interval. + /// + /// The interval is closed, so its endpoints are covered. + pub fn covers(&self, seconds: f64) -> bool { + seconds.is_finite() && seconds >= self.start_time() && seconds <= self.end_time() + } + + /// Returns the nominal step, when the samples are evenly spaced. + /// + /// Returns `None` when the spacing varies, which a caller checks before + /// indexing samples by arithmetic. + pub fn uniform_step(&self) -> Option { + let first = self.samples.windows(2).next()?[1].seconds - self.samples[0].seconds; + let uniform = self + .samples + .windows(2) + .all(|pair| (pair[1].seconds - pair[0].seconds - first).abs() <= 1e-9); + uniform.then_some(first) + } + + /// Returns `true` when the samples are evenly spaced. + pub fn is_uniform(&self) -> bool { + self.uniform_step().is_some() + } + + /// Returns the index of the interval that contains a time. + /// + /// The index `i` is such that `samples[i].seconds <= seconds <= + /// samples[i + 1].seconds`. A time outside the interval returns `None`. + fn interval_of(&self, seconds: f64) -> Option { + if !self.covers(seconds) { + return None; + } + // A binary search for the last sample at or before the time. The + // partition point is the first sample strictly after it, so the + // interval starts one before, clamped to the last interval. + let after = self + .samples + .partition_point(|sample| sample.seconds <= seconds); + let index = after.saturating_sub(1); + Some(index.min(self.samples.len() - 2)) + } + + /// Returns the state at a time, interpolating between samples. + /// + /// A time equal to a sample time returns that sample. Returns `None` + /// when the time is outside the covered interval. + pub fn state_at(&self, seconds: f64) -> Option { + let index = self.interval_of(seconds)?; + let first = self.samples[index]; + let second = self.samples[index + 1]; + + // At a sample the interpolation must reproduce the sample exactly, + // which avoids the round off of evaluating the cubic at its end. + if seconds == first.seconds { + return Some(first); + } + if seconds == second.seconds { + return Some(second); + } + + let dt = second.seconds - first.seconds; + let s = (seconds - first.seconds) / dt; + let (position, velocity) = hermite_state( + first.position, + first.velocity, + second.position, + second.velocity, + dt, + s, + ); + Some(Sample::new(seconds, position, velocity)) + } + + /// Returns the state at a time, or the nearest endpoint when the time is + /// outside the covered interval. + /// + /// This is for a caller that must produce a value for any time and is + /// content to hold the last known state rather than fail. + pub fn clamped_state_at(&self, seconds: f64) -> Option { + if self.covers(seconds) { + return self.state_at(seconds); + } + if !seconds.is_finite() { + return None; + } + if seconds < self.start_time() { + self.samples.first().copied() + } else { + self.samples.last().copied() + } + } + + /// Returns the sample nearest to a time, without interpolating. + pub fn nearest_sample(&self, seconds: f64) -> Option { + if !seconds.is_finite() { + return None; + } + let index = match self.interval_of(seconds) { + Some(index) => { + let first = self.samples[index]; + let second = self.samples[index + 1]; + if (seconds - first.seconds).abs() <= (second.seconds - seconds).abs() { + index + } else { + index + 1 + } + } + None if seconds < self.start_time() => 0, + None => self.samples.len() - 1, + }; + Some(self.samples[index]) + } + + /// Returns the times of every sample, in order. + pub fn times(&self) -> Vec { + self.samples.iter().map(|sample| sample.seconds).collect() + } + + /// Returns `true` when every state satisfies a predicate. + /// + /// This is a convenience for a caller that wants to check a property of + /// the whole table, such as every radius being above the surface, once. + pub fn all_samples(&self, predicate: F) -> bool + where + F: Fn(&Sample) -> bool, + { + self.samples.iter().all(predicate) + } +} diff --git a/crates/orbit/apsis-ephemeris/src/error.rs b/crates/orbit/apsis-ephemeris/src/error.rs new file mode 100644 index 0000000..07441b0 --- /dev/null +++ b/crates/orbit/apsis-ephemeris/src/error.rs @@ -0,0 +1,76 @@ +//! Errors reported while building or querying an ephemeris. + +use core::fmt; + +/// The category of an ephemeris error. +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] +#[non_exhaustive] +pub enum EphemerisErrorKind { + /// The sampling grid has fewer than two samples. + TooFewSamples, + /// A time or a step is not finite. + NotFinite, + /// A step is zero or negative. + NonPositiveStep, + /// The samples are not in increasing time order. + NotMonotonic, + /// A state produced by the sampler is not finite. + InvalidState, + /// A query time falls outside the covered interval. + OutOfRange, +} + +impl fmt::Display for EphemerisErrorKind { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + let text = match self { + Self::TooFewSamples => "fewer than two samples", + Self::NotFinite => "a time is not finite", + Self::NonPositiveStep => "the step is not positive", + Self::NotMonotonic => "the samples are not in increasing time order", + Self::InvalidState => "the sampled state is not finite", + Self::OutOfRange => "the time is outside the covered interval", + }; + f.write_str(text) + } +} + +/// A failure while building or querying an ephemeris. +#[derive(Debug, Clone, PartialEq, Eq)] +pub struct EphemerisError { + kind: EphemerisErrorKind, + detail: Option, +} + +impl EphemerisError { + /// Creates an error with the given category. + pub fn new(kind: EphemerisErrorKind) -> Self { + Self { kind, detail: None } + } + + /// Attaches a description of the value that caused the failure. + pub fn with_detail(mut self, detail: impl Into) -> Self { + self.detail = Some(detail.into()); + self + } + + /// Returns the category of this failure. + pub const fn kind(&self) -> EphemerisErrorKind { + self.kind + } + + /// Returns the detail text attached to this error, if any. + pub fn detail(&self) -> Option<&str> { + self.detail.as_deref() + } +} + +impl fmt::Display for EphemerisError { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + match &self.detail { + Some(detail) => write!(f, "{}: {detail}", self.kind), + None => write!(f, "{}", self.kind), + } + } +} + +impl core::error::Error for EphemerisError {} diff --git a/crates/orbit/apsis-ephemeris/src/lib.rs b/crates/orbit/apsis-ephemeris/src/lib.rs index cd6adf7..da944a0 100644 --- a/crates/orbit/apsis-ephemeris/src/lib.rs +++ b/crates/orbit/apsis-ephemeris/src/lib.rs @@ -1 +1,75 @@ //! Ephemeris sampling and interpolation. +//! +//! A propagator gives the state of a satellite at any instant, but calling +//! it millions of times is wasteful when the result is only needed to draw a +//! track or to search for a crossing. This crate records the state at a +//! grid of times, called samples, and answers queries between them by +//! interpolation. +//! +//! The two halves are deliberately independent: +//! +//! - [`Ephemeris`] is a table of times and states. It knows nothing about +//! where the states came from, so a table propagated by SGP4, computed by +//! two-body theory, or read from a file all work the same way. +//! - [`Sampler`] fills a table by calling a function that produces a state +//! from a time. In this workspace that function is normally +//! [`apsis_sgp4::Satellite::propagate_at`], but the sampler only requires +//! a closure, so a test can use an analytic function. +//! +//! Interpolation is Hermite cubic, which uses both the position and the +//! velocity at each end of an interval. Using the velocity is what makes it +//! accurate: a cubic through two positions alone has no information about +//! how fast the satellite is moving, while Hermite reconstruction matches +//! the slope at both ends and so reproduces a smooth orbit far better for +//! the same sample spacing. On a circular orbit the error is of order the +//! fourth power of the step, so halving the step cuts the error about +//! sixteen fold. +//! +//! Times are in seconds from an origin chosen by the caller, matching the +//! rest of the orbit layer. Positions are in kilometers and velocities in +//! kilometers per second. +//! +//! ``` +//! use apsis_ephemeris::{Sample, Sampler}; +//! use apsis_kepler::{GravityParameter, KeplerianElements}; +//! +//! // A circular orbit at the Earth's equatorial radius. +//! let elements = KeplerianElements { +//! semi_major_axis: 7_000.0, +//! eccentricity: 0.0, +//! inclination: 0.9, +//! right_ascension: 0.0, +//! argument_of_perigee: 0.0, +//! true_anomaly: 0.0, +//! }; +//! let state_at = |seconds: f64| { +//! let mean = 2.0 * core::f64::consts::PI * seconds +//! / elements.period(GravityParameter::WGS84); +//! let state = elements +//! .with_mean_anomaly(mean) +//! .to_state(GravityParameter::WGS84) +//! .expect("the orbit is valid"); +//! Sample::new(seconds, state.position, state.velocity) +//! }; +//! +//! let sampler = Sampler::new(0.0, 600.0, 60.0); +//! let ephemeris = sampler.sample(state_at).expect("the grid is usable"); +//! assert_eq!(ephemeris.len(), 11); +//! +//! // A query between two samples is interpolated. +//! let state = ephemeris.state_at(30.0).expect("the time is covered"); +//! assert!(state.position.norm() > 6_900.0); +//! ``` + +#![forbid(unsafe_code)] +#![deny(missing_docs)] + +mod ephemeris; +mod error; +mod sampler; + +pub use ephemeris::{Ephemeris, Sample, hermite_position, hermite_state, hermite_velocity}; +pub use error::{EphemerisError, EphemerisErrorKind}; +pub use sampler::Sampler; + +pub use apsis_math::Vec3; diff --git a/crates/orbit/apsis-ephemeris/src/sampler.rs b/crates/orbit/apsis-ephemeris/src/sampler.rs new file mode 100644 index 0000000..b3f1c0a --- /dev/null +++ b/crates/orbit/apsis-ephemeris/src/sampler.rs @@ -0,0 +1,179 @@ +//! Filling an ephemeris by calling a state function on a grid. + +use crate::ephemeris::{Ephemeris, Sample}; +use crate::error::{EphemerisError, EphemerisErrorKind}; +use apsis_math::Vec3; + +/// A uniform time grid that a state function is evaluated on. +/// +/// A sampler describes when to sample and how finely. The grid is defined by +/// a start time, an end time, and a step, and the number of samples follows +/// from them: the samples are placed at the start and at every step up to +/// the end, with the end itself included whether or not it falls on the +/// grid. Including the end matters, because a caller that asks for coverage +/// up to a time expects the table to reach it. +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct Sampler { + /// The time of the first sample, in seconds. + pub start_seconds: f64, + /// The time of the last sample, in seconds. + pub end_seconds: f64, + /// The spacing between samples, in seconds. + pub step_seconds: f64, +} + +impl Sampler { + /// Creates a sampler over an interval with a step. + pub const fn new(start_seconds: f64, end_seconds: f64, step_seconds: f64) -> Self { + Self { + start_seconds, + end_seconds, + step_seconds, + } + } + + /// Returns `true` when the sampler describes a usable grid. + /// + /// The times must be finite, the end must be strictly after the start, + /// and the step must be finite and positive. A zero length interval is + /// refused because a table needs at least two distinct times and the + /// interpolation has no interval to work over. + pub fn is_valid(&self) -> bool { + self.start_seconds.is_finite() + && self.end_seconds.is_finite() + && self.step_seconds.is_finite() + && self.step_seconds > 0.0 + && self.end_seconds > self.start_seconds + } + + /// Returns the number of samples the grid produces. + /// + /// This is the number of whole steps that fit in the interval plus one + /// for the start and one for the end when the end is off the grid. + /// Returns `None` when the sampler is not usable. + pub fn sample_count(&self) -> Option { + if !self.is_valid() { + return None; + } + let span = self.end_seconds - self.start_seconds; + let whole_steps = (span / self.step_seconds).floor(); + // The end sample is added unless it already coincides with the last + // whole step within a rounding tolerance. + let remainder = span - whole_steps * self.step_seconds; + let extra = if remainder > self.step_seconds * 1e-9 { + 1 + } else { + 0 + }; + Some(whole_steps as usize + 1 + extra) + } + + /// Returns the times of the grid, in order. + /// + /// Returns `None` when the sampler is not usable. The last time is the + /// end when the end does not fall on the grid, and the last grid point + /// otherwise. + pub fn times(&self) -> Option> { + let count = self.sample_count()?; + let mut times = Vec::with_capacity(count); + for index in 0..count { + let time = self.start_seconds + index as f64 * self.step_seconds; + if time >= self.end_seconds { + break; + } + times.push(time); + } + // The end is always included, whether or not it landed on the grid. + if times.last().copied() != Some(self.end_seconds) { + times.push(self.end_seconds); + } + Some(times) + } + + /// Fills an ephemeris by evaluating a state function on the grid. + /// + /// The function maps a time in seconds to a position and velocity. In + /// this workspace it is normally a closure over + /// [`apsis_sgp4::Satellite::propagate_at`], but any function works, so + /// the sampler can be tested against an analytic orbit. + /// + /// Returns an error when the sampler is not usable or when the function + /// produces a state that is not finite. + pub fn sample(&self, mut state_at: F) -> Result + where + F: FnMut(f64) -> Sample, + { + if !self.is_valid() { + return Err( + EphemerisError::new(EphemerisErrorKind::NonPositiveStep).with_detail(format!( + "start {}, end {}, step {}", + self.start_seconds, self.end_seconds, self.step_seconds + )), + ); + } + let times = self + .times() + .ok_or_else(|| EphemerisError::new(EphemerisErrorKind::NotFinite))?; + let mut samples = Vec::with_capacity(times.len()); + for time in times { + let sample = state_at(time); + if !sample.is_valid() { + return Err(EphemerisError::new(EphemerisErrorKind::InvalidState) + .with_detail(format!("at {time} s"))); + } + // The time is set from the grid rather than trusted from the + // function, so the table stays strictly ordered even if the + // function returns a slightly different time. + samples.push(Sample::new(time, sample.position, sample.velocity)); + } + Ephemeris::from_samples(samples) + } + + /// Fills an ephemeris from a function that returns only a position. + /// + /// The velocity is reconstructed from neighbouring positions by a + /// central difference. This is for a position-only source, such as a + /// tabulated file; where a velocity is available it should be used, so + /// prefer [`Sampler::sample`]. + pub fn sample_positions(&self, mut position_at: F) -> Result + where + F: FnMut(f64) -> Vec3, + { + if !self.is_valid() { + return Err(EphemerisError::new(EphemerisErrorKind::NonPositiveStep)); + } + let times = self + .times() + .ok_or_else(|| EphemerisError::new(EphemerisErrorKind::NotFinite))?; + let mut positions = Vec::with_capacity(times.len()); + for &time in × { + let position = position_at(time); + if !position.is_finite() { + return Err(EphemerisError::new(EphemerisErrorKind::InvalidState) + .with_detail(format!("at {time} s"))); + } + positions.push(position); + } + + // A central difference inside the interval and a one sided one at + // each end. The finite difference is second order, which matches the + // accuracy of the Hermite interpolation it feeds. + let count = positions.len(); + let mut samples = Vec::with_capacity(count); + for (index, &position) in positions.iter().enumerate() { + let velocity = if count == 1 { + Vec3::new(0.0, 0.0, 0.0) + } else if index == 0 { + (positions[1] - positions[0]) / (times[1] - times[0]) + } else if index == count - 1 { + (positions[count - 1] - positions[count - 2]) + / (times[count - 1] - times[count - 2]) + } else { + (positions[index + 1] - positions[index - 1]) + / (times[index + 1] - times[index - 1]) + }; + samples.push(Sample::new(times[index], position, velocity)); + } + Ephemeris::from_samples(samples) + } +} diff --git a/crates/orbit/apsis-ephemeris/tests/ephemeris.rs b/crates/orbit/apsis-ephemeris/tests/ephemeris.rs new file mode 100644 index 0000000..b254166 --- /dev/null +++ b/crates/orbit/apsis-ephemeris/tests/ephemeris.rs @@ -0,0 +1,441 @@ +//! Tests for the ephemeris table and its interpolation. + +use apsis_ephemeris::{ + Ephemeris, EphemerisErrorKind, Sample, Sampler, hermite_position, hermite_state, + hermite_velocity, +}; +use apsis_kepler::{GravityParameter, KeplerianElements}; +use apsis_math::Vec3; + +/// The circular orbit the interpolation is checked against. +/// +/// A circular orbit is the useful case: the state is analytic, so the error +/// of the interpolation is exactly the difference from a known value, and +/// the radius is constant, so a bad reconstruction shows up immediately. +fn circular() -> KeplerianElements { + KeplerianElements { + semi_major_axis: 7_000.0, + eccentricity: 0.0, + inclination: 0.9, + right_ascension: 0.5, + argument_of_perigee: 0.0, + true_anomaly: 0.0, + } +} + +/// Returns the exact state of the circular orbit at a time. +fn exact_state(seconds: f64) -> Sample { + let elements = circular(); + let mean = 2.0 * core::f64::consts::PI * seconds / elements.period(GravityParameter::WGS84); + let state = elements + .with_mean_anomaly(mean) + .to_state(GravityParameter::WGS84) + .expect("the circular orbit is valid"); + Sample::new(seconds, state.position, state.velocity) +} + +fn sample_table(step: f64, span: f64) -> Ephemeris { + Sampler::new(0.0, span, step) + .sample(exact_state) + .expect("the grid is usable") +} + +#[test] +fn a_table_can_be_built_from_samples() { + let samples = vec![ + Sample::new(0.0, Vec3::new(7_000.0, 0.0, 0.0), Vec3::new(0.0, 7.5, 0.0)), + Sample::new( + 60.0, + Vec3::new(0.0, 7_000.0, 0.0), + Vec3::new(-7.5, 0.0, 0.0), + ), + ]; + let ephemeris = Ephemeris::from_samples(samples).expect("two ordered samples"); + assert_eq!(ephemeris.len(), 2); + assert!(!ephemeris.is_empty()); + assert_eq!(ephemeris.start_time(), 0.0); + assert_eq!(ephemeris.end_time(), 60.0); + assert_eq!(ephemeris.duration(), 60.0); +} + +#[test] +fn a_table_needs_two_samples() { + let error = Ephemeris::from_samples(Vec::new()).expect_err("an empty table is refused"); + assert_eq!(error.kind(), EphemerisErrorKind::TooFewSamples); + let one = vec![Sample::new(0.0, Vec3::ZERO, Vec3::ZERO)]; + let error = Ephemeris::from_samples(one).expect_err("one sample is refused"); + assert_eq!(error.kind(), EphemerisErrorKind::TooFewSamples); +} + +#[test] +fn samples_must_be_in_increasing_time_order() { + let samples = vec![ + Sample::new(10.0, Vec3::ZERO, Vec3::ZERO), + Sample::new(5.0, Vec3::ZERO, Vec3::ZERO), + ]; + let error = Ephemeris::from_samples(samples).expect_err("the order is wrong"); + assert_eq!(error.kind(), EphemerisErrorKind::NotMonotonic); + + // Equal times are also refused, because a zero width interval has no + // interpolation to do. + let samples = vec![ + Sample::new(5.0, Vec3::ZERO, Vec3::ZERO), + Sample::new(5.0, Vec3::ZERO, Vec3::ZERO), + ]; + let error = Ephemeris::from_samples(samples).expect_err("equal times are refused"); + assert_eq!(error.kind(), EphemerisErrorKind::NotMonotonic); +} + +#[test] +fn a_non_finite_sample_is_refused() { + let samples = vec![ + Sample::new(0.0, Vec3::ZERO, Vec3::ZERO), + Sample::new(60.0, Vec3::new(f64::NAN, 0.0, 0.0), Vec3::ZERO), + ]; + let error = Ephemeris::from_samples(samples).expect_err("the state is not finite"); + assert_eq!(error.kind(), EphemerisErrorKind::InvalidState); +} + +#[test] +fn a_query_at_a_sample_returns_it_exactly() { + let ephemeris = sample_table(60.0, 600.0); + for sample in ephemeris.samples() { + let queried = ephemeris + .state_at(sample.seconds) + .expect("the sample time is covered"); + // The interpolation must not perturb a sample, so the returned + // state is bit for bit the stored one. + assert_eq!(queried, *sample, "at {} s", sample.seconds); + } +} + +#[test] +fn a_query_between_samples_is_interpolated() { + let ephemeris = sample_table(60.0, 600.0); + let midpoint = ephemeris.state_at(30.0).expect("the time is covered"); + assert_eq!(midpoint.seconds, 30.0); + // The interpolated position differs from both endpoints. + assert_ne!(midpoint.position, ephemeris.samples()[0].position); + assert_ne!(midpoint.position, ephemeris.samples()[1].position); + // And it is a plausible radius for the orbit. + assert!((midpoint.radius() - 7_000.0).abs() < 1.0); +} + +#[test] +fn the_interpolation_is_accurate_on_a_circular_orbit() { + // Hermite cubic on a smooth orbit: the position error over a step of + // sixty seconds should be far below a kilometer. + let ephemeris = sample_table(60.0, 600.0); + let mut worst = 0.0_f64; + for index in 0..600 { + let time = f64::from(index) + 0.5; + let interpolated = ephemeris.state_at(time).expect("covered"); + let truth = exact_state(time); + worst = worst.max(interpolated.position.distance(truth.position)); + } + assert!( + worst < 1e-3, + "the worst position error is {worst} km, which is too large" + ); +} + +#[test] +fn halving_the_step_cuts_the_error_by_about_sixteen() { + // A quartic method: the error scales as the fourth power of the step. + // This is the property that makes the interpolation worth using, and it + // fails if the velocity is dropped or mis-scaled. + let error_for = |step: f64| { + let ephemeris = sample_table(step, 600.0); + let mut worst = 0.0_f64; + // Query at the midpoints between samples, where the error is + // largest. + let mut time = step / 2.0; + while time < 600.0 { + let interpolated = ephemeris.state_at(time).expect("covered"); + let truth = exact_state(time); + worst = worst.max(interpolated.position.distance(truth.position)); + time += step; + } + worst + }; + let coarse = error_for(120.0); + let fine = error_for(60.0); + assert!(coarse > 0.0, "the coarse error should be non-zero"); + let ratio = coarse / fine; + assert!( + (8.0..32.0).contains(&ratio), + "the error ratio is {ratio}, expected about sixteen" + ); +} + +#[test] +fn the_interpolated_velocity_matches_the_position_derivative() { + // The velocity is the analytic derivative of the position cubic, so a + // numerical derivative of the position must agree with it. + let ephemeris = sample_table(60.0, 600.0); + let time = 137.0; + let h = 1e-4; + let before = ephemeris.state_at(time - h).expect("covered").position; + let after = ephemeris.state_at(time + h).expect("covered").position; + let numeric = (after - before) / (2.0 * h); + let analytic = ephemeris.state_at(time).expect("covered").velocity; + assert!( + numeric.distance(analytic) < 1e-6, + "numeric {} against analytic {}", + numeric, + analytic + ); +} + +#[test] +fn a_query_outside_the_interval_is_refused() { + let ephemeris = sample_table(60.0, 600.0); + assert!(ephemeris.state_at(-1.0).is_none()); + assert!(ephemeris.state_at(600.1).is_none()); + assert!(ephemeris.state_at(f64::NAN).is_none()); + assert!(ephemeris.state_at(f64::INFINITY).is_none()); + // The endpoints are covered. + assert!(ephemeris.covers(0.0)); + assert!(ephemeris.covers(600.0)); + assert!(!ephemeris.covers(-1e-9)); +} + +#[test] +fn a_clamped_query_returns_the_nearest_endpoint() { + let ephemeris = sample_table(60.0, 600.0); + let before = ephemeris.clamped_state_at(-100.0).expect("clamped"); + assert_eq!(before, ephemeris.samples()[0]); + let after = ephemeris.clamped_state_at(10_000.0).expect("clamped"); + assert_eq!(after, ephemeris.samples()[ephemeris.len() - 1]); + // Inside the interval the clamped query is the ordinary one. + let inside = ephemeris.clamped_state_at(30.0).expect("covered"); + assert_eq!(inside, ephemeris.state_at(30.0).expect("covered")); + assert!(ephemeris.clamped_state_at(f64::NAN).is_none()); +} + +#[test] +fn the_nearest_sample_is_found() { + let ephemeris = sample_table(60.0, 600.0); + assert_eq!(ephemeris.nearest_sample(0.0).expect("found").seconds, 0.0); + assert_eq!(ephemeris.nearest_sample(29.0).expect("found").seconds, 0.0); + assert_eq!(ephemeris.nearest_sample(31.0).expect("found").seconds, 60.0); + // A midpoint ties toward the earlier sample. + assert_eq!(ephemeris.nearest_sample(30.0).expect("found").seconds, 0.0); + // Outside the interval the nearest end is returned. + assert_eq!( + ephemeris.nearest_sample(-100.0).expect("found").seconds, + 0.0 + ); + assert_eq!( + ephemeris.nearest_sample(10_000.0).expect("found").seconds, + 600.0 + ); + assert!(ephemeris.nearest_sample(f64::NAN).is_none()); +} + +#[test] +fn a_uniform_grid_is_recognized() { + let uniform = sample_table(60.0, 600.0); + assert!(uniform.is_uniform()); + assert_eq!(uniform.uniform_step(), Some(60.0)); + + let uneven = Ephemeris::from_samples(vec![ + Sample::new(0.0, Vec3::ZERO, Vec3::ZERO), + Sample::new(60.0, Vec3::ZERO, Vec3::ZERO), + Sample::new(150.0, Vec3::ZERO, Vec3::ZERO), + ]) + .expect("ordered"); + assert!(!uneven.is_uniform()); + assert_eq!(uneven.uniform_step(), None); +} + +#[test] +fn the_times_of_the_samples_are_listed() { + let ephemeris = sample_table(60.0, 300.0); + assert_eq!( + ephemeris.times(), + vec![0.0, 60.0, 120.0, 180.0, 240.0, 300.0] + ); +} + +#[test] +fn a_property_of_every_sample_can_be_checked() { + let ephemeris = sample_table(60.0, 600.0); + assert!(ephemeris.all_samples(|sample| sample.radius() > 6_000.0)); + assert!(!ephemeris.all_samples(|sample| sample.radius() > 8_000.0)); +} + +// The sampler's grid. + +#[test] +fn the_grid_includes_both_ends() { + // The end falls on the grid. + let sampler = Sampler::new(0.0, 300.0, 60.0); + assert_eq!(sampler.sample_count(), Some(6)); + assert_eq!( + sampler.times().expect("usable"), + vec![0.0, 60.0, 120.0, 180.0, 240.0, 300.0] + ); + + // The end does not fall on the grid, so it is appended. + let sampler = Sampler::new(0.0, 250.0, 60.0); + assert_eq!(sampler.sample_count(), Some(6)); + assert_eq!( + sampler.times().expect("usable"), + vec![0.0, 60.0, 120.0, 180.0, 240.0, 250.0] + ); +} + +#[test] +fn a_single_step_grid_has_two_samples() { + let sampler = Sampler::new(10.0, 70.0, 60.0); + assert_eq!(sampler.sample_count(), Some(2)); + assert_eq!(sampler.times().expect("usable"), vec![10.0, 70.0]); +} + +#[test] +fn a_zero_length_interval_is_refused() { + // A table needs at least two distinct times, so a zero length grid has + // no interval to interpolate over and is rejected outright rather than + // producing a table that cannot answer a query. + let sampler = Sampler::new(5.0, 5.0, 60.0); + assert!(!sampler.is_valid()); + assert_eq!(sampler.sample_count(), None); + assert!(sampler.times().is_none()); + assert!(sampler.sample(exact_state).is_err()); +} + +#[test] +fn an_unusable_sampler_is_refused() { + for sampler in [ + Sampler::new(0.0, 600.0, 0.0), + Sampler::new(0.0, 600.0, -60.0), + Sampler::new(0.0, 600.0, f64::NAN), + Sampler::new(f64::NAN, 600.0, 60.0), + Sampler::new(0.0, f64::INFINITY, 60.0), + Sampler::new(600.0, 0.0, 60.0), + ] { + assert!(!sampler.is_valid(), "{sampler:?}"); + assert_eq!(sampler.sample_count(), None); + assert!(sampler.times().is_none()); + assert!(sampler.sample(exact_state).is_err()); + } +} + +#[test] +fn a_bad_state_from_the_function_is_reported() { + let sampler = Sampler::new(0.0, 120.0, 60.0); + let error = sampler + .sample(|seconds| { + if seconds > 0.0 { + Sample::new(seconds, Vec3::new(f64::NAN, 0.0, 0.0), Vec3::ZERO) + } else { + Sample::new(seconds, Vec3::ZERO, Vec3::ZERO) + } + }) + .expect_err("the state is not finite"); + assert_eq!(error.kind(), EphemerisErrorKind::InvalidState); +} + +#[test] +fn the_sampler_overrides_a_bad_time_from_the_function() { + // A function that returns a wrong time must not corrupt the table: the + // grid time is used instead. + let sampler = Sampler::new(0.0, 120.0, 60.0); + let ephemeris = sampler + .sample(|_| Sample::new(999.0, Vec3::new(7_000.0, 0.0, 0.0), Vec3::ZERO)) + .expect("the states are finite"); + assert_eq!(ephemeris.times(), vec![0.0, 60.0, 120.0]); +} + +#[test] +fn positions_can_be_sampled_without_velocities() { + // The velocity is reconstructed by finite differences, so a + // position-only source still produces a usable table. + let ephemeris = Sampler::new(0.0, 600.0, 60.0) + .sample_positions(|seconds| exact_state(seconds).position) + .expect("the positions are finite"); + assert_eq!(ephemeris.len(), 11); + + // The reconstructed velocity is close to the true one. + let sample = ephemeris.state_at(300.0).expect("covered"); + let truth = exact_state(300.0); + assert!( + sample.velocity.distance(truth.velocity) < 1e-2, + "the reconstructed velocity {} is too far from {}", + sample.velocity, + truth.velocity + ); + + // The interpolation is still usable, though far less accurate than with + // exact velocities: the finite difference velocity is only second order + // accurate, so its error feeds straight into the Hermite reconstruction. + // The error is a few kilometers over a one minute step, against + // millimeters when the velocity is known. + let mut worst = 0.0_f64; + for index in 0..600 { + let time = f64::from(index) + 0.5; + let interpolated = ephemeris.state_at(time).expect("covered"); + worst = worst.max(interpolated.position.distance(exact_state(time).position)); + } + assert!(worst < 5.0, "the worst position error is {worst} km"); +} + +#[test] +fn position_sampling_reports_a_bad_position() { + let sampler = Sampler::new(0.0, 120.0, 60.0); + let error = sampler + .sample_positions(|seconds| { + if seconds > 0.0 { + Vec3::new(f64::NAN, 0.0, 0.0) + } else { + Vec3::ZERO + } + }) + .expect_err("the position is not finite"); + assert_eq!(error.kind(), EphemerisErrorKind::InvalidState); +} + +// The Hermite basis functions directly. + +#[test] +fn the_hermite_basis_reproduces_its_endpoints() { + let p0 = Vec3::new(1.0, 2.0, 3.0); + let p1 = Vec3::new(4.0, 5.0, 6.0); + let v0 = Vec3::new(0.1, 0.2, 0.3); + let v1 = Vec3::new(0.4, 0.5, 0.6); + let dt = 60.0; + + // At s = 0 the result is p0 and the velocity is v0. + assert_eq!(hermite_position(p0, v0, p1, v1, dt, 0.0), p0); + assert_eq!(hermite_velocity(p0, v0, p1, v1, dt, 0.0), v0); + // At s = 1 the result is p1 and the velocity is v1. + assert_eq!(hermite_position(p0, v0, p1, v1, dt, 1.0), p1); + assert_eq!(hermite_velocity(p0, v0, p1, v1, dt, 1.0), v1); +} + +#[test] +fn the_hermite_state_returns_both_parts() { + let p0 = Vec3::new(1.0, 0.0, 0.0); + let p1 = Vec3::new(2.0, 0.0, 0.0); + let (position, velocity) = hermite_state(p0, Vec3::ZERO, p1, Vec3::ZERO, 10.0, 0.5); + // Both endpoints are stationary, so the cubic is the S-curve that + // passes through the midpoint with the slope the endpoints imply. + assert_eq!(position, Vec3::new(1.5, 0.0, 0.0)); + // The slope at the midpoint is 1.5 / dt, not zero: the curve rises from + // zero slope at one end to zero slope at the other. + assert_eq!(velocity, Vec3::new(0.15, 0.0, 0.0)); +} + +#[test] +fn a_sample_reports_its_radius_and_speed() { + let sample = Sample::new(0.0, Vec3::new(3.0, 4.0, 0.0), Vec3::new(0.0, 0.0, 12.0)); + assert!((sample.radius() - 5.0).abs() < 1e-12); + assert!((sample.speed() - 12.0).abs() < 1e-12); +} + +#[test] +fn an_error_displays_its_kind() { + let error = Ephemeris::from_samples(Vec::new()).expect_err("empty"); + assert!(error.to_string().contains("two samples"), "{error}"); +}