298 lines
10 KiB
Rust
298 lines
10 KiB
Rust
//! 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<Sample>,
|
|
}
|
|
|
|
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<Sample>) -> Result<Self, EphemerisError> {
|
|
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<f64> {
|
|
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<usize> {
|
|
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<Sample> {
|
|
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<Sample> {
|
|
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<Sample> {
|
|
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<f64> {
|
|
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<F>(&self, predicate: F) -> bool
|
|
where
|
|
F: Fn(&Sample) -> bool,
|
|
{
|
|
self.samples.iter().all(predicate)
|
|
}
|
|
}
|