#![allow(
clippy::cast_possible_truncation,
clippy::cast_sign_loss,
clippy::cast_precision_loss,
clippy::cast_possible_wrap,
clippy::many_single_char_names
)]
use spacelab_core::{Angle, Epoch, Tt, Ut1, Vec3};
use spacelab_fa::{cio_locator_s, cip_xy, era};
use crate::cio::{cirs_to_tirs, gcrf_to_cirs, tirs_to_itrf};
use crate::polar_motion::tio_locator;
use crate::rotation::{Gcrf, Itrf, Rotation, Transform};
use crate::teme::EARTH_ROTATION_RATE;
const STENCIL: usize = 8;
const GRID_DT: f64 = 600.0;
#[derive(Clone, Copy)]
struct Node {
x: f64,
y: f64,
s: f64,
sp: f64,
}
pub struct CioInterpolator {
nodes: Vec<Node>,
t_start: f64,
epoch_ut1: Epoch<Ut1>,
epoch_tt: Epoch<Tt>,
xp: Angle,
yp: Angle,
dx: Angle,
dy: Angle,
lod_seconds: f64,
}
impl CioInterpolator {
#[must_use]
#[allow(clippy::too_many_arguments)] pub fn new(
epoch_ut1: Epoch<Ut1>,
epoch_tt: Epoch<Tt>,
span: f64,
xp: Angle,
yp: Angle,
dx: Angle,
dy: Angle,
lod_seconds: f64,
) -> Self {
let margin = STENCIL as f64 * GRID_DT;
let t_start = span.min(0.0) - margin;
let reach = span.abs() + 2.0 * margin;
let count = (reach / GRID_DT).ceil() as usize + 1;
let mut nodes = Vec::with_capacity(count);
let epoch_tt_ns = epoch_tt.ns_since_j2000();
for i in 0..count {
let ti = t_start + i as f64 * GRID_DT;
let tt = Epoch::<Tt>::from_ns_since_j2000(epoch_tt_ns + (ti * 1e9).round() as i128);
let (x, y) = cip_xy(tt);
let s = cio_locator_s(tt, x, y);
let sp = tio_locator(tt).radians();
nodes.push(Node { x, y, s, sp });
}
Self {
nodes,
t_start,
epoch_ut1,
epoch_tt,
xp,
yp,
dx,
dy,
lod_seconds,
}
}
#[must_use]
pub fn rotation(&self, t: f64) -> Rotation<Gcrf, Itrf> {
let node = self.interpolate(t);
let ut1 = Epoch::<Ut1>::from_ns_since_j2000(
self.epoch_ut1.ns_since_j2000() + (t * 1e9).round() as i128,
);
let q = gcrf_to_cirs(
node.x + self.dx.radians(),
node.y + self.dy.radians(),
node.s,
);
tirs_to_itrf(self.xp, self.yp, Angle::from_radians(node.sp)) * cirs_to_tirs(era(ut1)) * q
}
#[must_use]
pub fn rotation_rows(&self, t: f64) -> [[f64; 3]; 3] {
self.rotation(t).matrix().rows
}
#[must_use]
pub fn transform(&self, t: f64) -> Transform<Gcrf, Itrf> {
let node = self.interpolate(t);
let ut1 = Epoch::<Ut1>::from_ns_since_j2000(
self.epoch_ut1.ns_since_j2000() + (t * 1e9).round() as i128,
);
let q = gcrf_to_cirs(
node.x + self.dx.radians(),
node.y + self.dy.radians(),
node.s,
);
let rotation = tirs_to_itrf(self.xp, self.yp, Angle::from_radians(node.sp))
* cirs_to_tirs(era(ut1))
* q;
let rate = EARTH_ROTATION_RATE * (1.0 - self.lod_seconds / 86_400.0);
let omega = q.inverse().matrix() * Vec3::new(0.0, 0.0, rate);
Transform::new(rotation, omega)
}
#[must_use]
pub fn covers(&self, t: f64) -> bool {
self.node_index(t).is_some()
}
fn node_index(&self, t: f64) -> Option<f64> {
let u = (t - self.t_start) / GRID_DT;
(0.0..=(self.nodes.len() - 1) as f64)
.contains(&u)
.then_some(u)
}
fn exact_node(&self, t: f64) -> Node {
let tt = Epoch::<Tt>::from_ns_since_j2000(
self.epoch_tt.ns_since_j2000() + (t * 1e9).round() as i128,
);
let (x, y) = cip_xy(tt);
Node {
x,
y,
s: cio_locator_s(tt, x, y),
sp: tio_locator(tt).radians(),
}
}
fn interpolate(&self, t: f64) -> Node {
let Some(u) = self.node_index(t) else {
return self.exact_node(t);
};
let last = self.nodes.len() - STENCIL;
let i0 = (u.floor() as isize - STENCIL as isize / 2 + 1).clamp(0, last as isize) as usize;
debug_assert!(
i0 as f64 <= u && u <= (i0 + STENCIL - 1) as f64,
"stencil [{i0}, {}] does not contain the query at u = {u}: this is extrapolation",
i0 + STENCIL - 1
);
let xq = u - i0 as f64;
let (mut x, mut y, mut s, mut sp) = (0.0, 0.0, 0.0, 0.0);
for j in 0..STENCIL {
let mut l = 1.0;
for k in 0..STENCIL {
if k != j {
l *= (xq - k as f64) / (j as f64 - k as f64);
}
}
let n = self.nodes[i0 + j];
x += l * n.x;
y += l * n.y;
s += l * n.s;
sp += l * n.sp;
}
Node { x, y, s, sp }
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::gcrf_to_itrf;
use spacelab_testkit::nan_max;
#[test]
fn cio_interpolation_matches_the_exact_transform() {
let epoch_tt = Epoch::<Tt>::from_ns_since_j2000(788_400_000_000_000_000);
let epoch_ut1 = Epoch::<Ut1>::from_ns_since_j2000(788_400_000_000_000_000 - 69_000_000_000);
let xp = Angle::from_arcseconds(0.16);
let yp = Angle::from_arcseconds(0.35);
let zero = Angle::from_radians(0.0);
let span = 6.0 * 3600.0;
let cio = CioInterpolator::new(epoch_ut1, epoch_tt, span, xp, yp, zero, zero, 0.0);
let r = Vec3::new(6_800e3, 1_200e3, 900e3);
let mut worst = 0.0_f64;
for k in 0..=240 {
let t = span * f64::from(k) / 240.0;
let ut1 = Epoch::<Ut1>::from_ns_since_j2000(
epoch_ut1.ns_since_j2000() + (t * 1e9).round() as i128,
);
let tt = Epoch::<Tt>::from_ns_since_j2000(
epoch_tt.ns_since_j2000() + (t * 1e9).round() as i128,
);
let exact = gcrf_to_itrf(ut1, tt, xp, yp, zero, zero, 0.0)
.rotation
.matrix()
* r;
let interp = cio.rotation(t).matrix() * r;
worst = nan_max(worst, (exact - interp).norm());
}
assert!(
worst < 1e-6,
"CIO interpolation error {worst:.3e} m exceeds 1 µm"
);
}
#[test]
fn a_query_outside_the_grid_is_exact_rather_than_extrapolated() {
let epoch_tt = Epoch::<Tt>::from_ns_since_j2000(788_400_000_000_000_000);
let epoch_ut1 = Epoch::<Ut1>::from_ns_since_j2000(788_400_000_000_000_000 - 69_000_000_000);
let xp = Angle::from_arcseconds(0.16);
let yp = Angle::from_arcseconds(0.35);
let zero = Angle::from_radians(0.0);
let span = 6.0 * 3600.0;
let cio = CioInterpolator::new(epoch_ut1, epoch_tt, span, xp, yp, zero, zero, 0.0);
let r = Vec3::new(6_800e3, 1_200e3, 900e3);
assert!(cio.covers(0.0) && cio.covers(span), "the arc is covered");
let mut worst = 0.0_f64;
for days in [-365.0, -30.0, -1.0, 1.0, 30.0, 365.0] {
let t = span + days * 86_400.0;
assert!(!cio.covers(t), "t = {t} should be outside the grid");
let ut1 = Epoch::<Ut1>::from_ns_since_j2000(
epoch_ut1.ns_since_j2000() + (t * 1e9).round() as i128,
);
let tt = Epoch::<Tt>::from_ns_since_j2000(
epoch_tt.ns_since_j2000() + (t * 1e9).round() as i128,
);
let exact = gcrf_to_itrf(ut1, tt, xp, yp, zero, zero, 0.0)
.rotation
.matrix()
* r;
worst = nan_max(worst, (exact - cio.rotation(t).matrix() * r).norm());
}
assert!(
worst < 1e-9,
"an uncovered query differs from the exact chain by {worst:.3e} m — it extrapolated"
);
}
#[test]
#[allow(clippy::float_cmp)] fn rows_and_transform_match_rotation_and_carry_the_earth_rate() {
let epoch_tt = Epoch::<Tt>::from_ns_since_j2000(788_400_000_000_000_000);
let epoch_ut1 = Epoch::<Ut1>::from_ns_since_j2000(788_400_000_000_000_000 - 69_000_000_000);
let xp = Angle::from_arcseconds(0.16);
let yp = Angle::from_arcseconds(0.35);
let zero = Angle::from_radians(0.0);
let span = 6.0 * 3600.0;
let cio = CioInterpolator::new(epoch_ut1, epoch_tt, span, xp, yp, zero, zero, 0.0);
let t = span * 0.37;
let rows = cio.rotation_rows(t);
assert_eq!(rows, cio.rotation(t).matrix().rows);
let tf = cio.transform(t);
assert_eq!(tf.rotation.matrix().rows, rows);
assert!((tf.omega.norm() - EARTH_ROTATION_RATE).abs() <= 1e-12 * EARTH_ROTATION_RATE);
let r = Vec3::new(6_800e3, 1_200e3, 900e3);
let ut1 = Epoch::<Ut1>::from_ns_since_j2000(
epoch_ut1.ns_since_j2000() + (t * 1e9).round() as i128,
);
let tt =
Epoch::<Tt>::from_ns_since_j2000(epoch_tt.ns_since_j2000() + (t * 1e9).round() as i128);
let exact = gcrf_to_itrf(ut1, tt, xp, yp, zero, zero, 0.0)
.rotation
.matrix()
* r;
let got = tf.rotation.matrix() * r;
assert!((exact - got).norm() < 1e-6);
}
#[test]
fn the_celestial_pole_offsets_reach_the_interpolated_stencil() {
let epoch_tt = Epoch::<Tt>::from_ns_since_j2000(788_400_000_000_000_000);
let epoch_ut1 = Epoch::<Ut1>::from_ns_since_j2000(788_400_000_000_000_000 - 69_000_000_000);
let xp = Angle::from_arcseconds(0.16);
let yp = Angle::from_arcseconds(0.35);
let zero = Angle::from_radians(0.0);
let dx = Angle::from_arcseconds(12e-3);
let dy = Angle::from_arcseconds(-7e-3);
let span = 6.0 * 3600.0;
let r = Vec3::new(6_800e3, 1_200e3, 900e3);
let cio = CioInterpolator::new(epoch_ut1, epoch_tt, span, xp, yp, dx, dy, 0.0);
let mut worst = 0.0_f64;
let mut offsets_are_worth = 0.0_f64;
for k in 0..=48 {
let t = span * f64::from(k) / 48.0;
let ns = (t * 1e9).round() as i128;
let ut1 = Epoch::<Ut1>::from_ns_since_j2000(epoch_ut1.ns_since_j2000() + ns);
let tt = Epoch::<Tt>::from_ns_since_j2000(epoch_tt.ns_since_j2000() + ns);
let exact = gcrf_to_itrf(ut1, tt, xp, yp, dx, dy, 0.0).rotation.matrix() * r;
let zeroed = gcrf_to_itrf(ut1, tt, xp, yp, zero, zero, 0.0)
.rotation
.matrix()
* r;
worst = nan_max(worst, (exact - cio.rotation(t).matrix() * r).norm());
worst = nan_max(
worst,
(exact - cio.transform(t).rotation.matrix() * r).norm(),
);
offsets_are_worth = nan_max(offsets_are_worth, (exact - zeroed).norm());
}
assert!(
worst < 1e-6,
"interpolated rotation differs from the exact chain by {worst:.3e} m with offsets"
);
assert!(
offsets_are_worth > 0.01,
"the offsets moved the position by {offsets_are_worth:.3e} m — too little for the \
agreement above to show they were applied"
);
}
#[test]
#[allow(clippy::float_cmp)] fn the_grid_boundary_is_exact_and_every_node_reproduces_the_series() {
let epoch_tt = Epoch::<Tt>::from_ns_since_j2000(788_400_000_000_000_000);
let epoch_ut1 = Epoch::<Ut1>::from_ns_since_j2000(788_400_000_000_000_000 - 69_000_000_000);
let zero = Angle::from_radians(0.0);
let span = 6.0 * 3600.0;
let cio = CioInterpolator::new(epoch_ut1, epoch_tt, span, zero, zero, zero, zero, 0.0);
let t_first = cio.t_start;
let t_last = cio.t_start + (cio.nodes.len() - 1) as f64 * GRID_DT;
assert!(cio.covers(t_first), "the first node must be covered");
assert!(cio.covers(t_last), "the last node must be covered");
assert!(
!cio.covers(t_last + GRID_DT * 1e-6),
"a query past the last node must not be covered — that is extrapolation"
);
assert!(
!cio.covers(t_first - GRID_DT * 1e-6),
"a query before the first node must not be covered"
);
assert!(cio.covers(0.0) && cio.covers(span), "the arc itself");
let r = Vec3::new(6_800e3, 1_200e3, 900e3);
for i in 0..cio.nodes.len() {
let t = cio.t_start + i as f64 * GRID_DT;
let ns = (t * 1e9).round() as i128;
let ut1 = Epoch::<Ut1>::from_ns_since_j2000(epoch_ut1.ns_since_j2000() + ns);
let tt = Epoch::<Tt>::from_ns_since_j2000(epoch_tt.ns_since_j2000() + ns);
let exact = gcrf_to_itrf(ut1, tt, zero, zero, zero, zero, 0.0)
.rotation
.matrix()
* r;
let gap = (exact - cio.rotation(t).matrix() * r).norm();
assert!(
gap == 0.0,
"node {i} at t = {t} is off the exact chain by {gap:.3e} m — the stencil that \
interpolated it does not contain it"
);
}
}
#[test]
fn the_interpolated_rate_carries_lod_exactly_as_the_scalar_chain_does() {
let epoch_tt = Epoch::<Tt>::from_ns_since_j2000(788_400_000_000_000_000);
let epoch_ut1 = Epoch::<Ut1>::from_ns_since_j2000(788_400_000_000_000_000 - 69_000_000_000);
let zero = Angle::from_radians(0.0);
let span = 6.0 * 3600.0;
for lod in [0.0, 0.0018, -0.0005, 0.0035] {
let cio = CioInterpolator::new(epoch_ut1, epoch_tt, span, zero, zero, zero, zero, lod);
let scalar = gcrf_to_itrf(epoch_ut1, epoch_tt, zero, zero, zero, zero, lod)
.omega
.norm();
let interpolated = cio.transform(span * 0.37).omega.norm();
assert!(
(interpolated - scalar).abs() < 1e-20,
"LOD {lod} s: interpolator |ω| = {interpolated:.20e}, scalar = {scalar:.20e}"
);
if lod > 0.0 {
assert!(
EARTH_ROTATION_RATE - interpolated > 0.5e-12,
"a positive LOD must slow the interpolated rate"
);
}
}
}
}