use std::collections::HashMap;
use std::path::Path;
use thiserror::Error;
use crate::daf::{DafError, DafFile};
use crate::frame::{rotate_state, NaifFrame};
#[derive(Debug, Error)]
pub enum SpkError {
#[error(transparent)]
Daf(#[from] DafError),
#[error("no segment covers target {target} wrt center {center} at et {et}")]
NoCoverage { target: i32, center: i32, et: f64 },
#[error("unsupported SPK data type {0}")]
UnsupportedType(i32),
#[error("malformed Type 2 segment: {0}")]
BadType2(&'static str),
}
#[derive(Clone)]
pub struct SpkSegment {
pub target: i32,
pub center: i32,
pub frame: i32,
pub data_type: i32,
pub start_et: f64,
pub end_et: f64,
pub start_addr: u32,
pub end_addr: u32,
pub name: String,
payload: SpkPayload,
}
#[derive(Clone)]
enum SpkPayload {
Type2(SpkType2),
Type3(SpkType3),
Type9(SpkType9),
Type13(SpkType13),
Unsupported,
}
#[derive(Clone)]
struct SpkType2 {
file: DafFile,
init: f64,
intlen: f64,
rsize: usize,
n_records: usize,
n_coef: usize,
start_addr: u32,
}
impl SpkType2 {
fn from_segment(file: &DafFile, start_addr: u32, end_addr: u32) -> Result<Self, SpkError> {
let trailer = file.read_doubles(end_addr - 3, end_addr)?;
let init = trailer[0];
let intlen = trailer[1];
let rsize = trailer[2] as usize;
let n_records = trailer[3] as usize;
if rsize < 2 || (rsize - 2) % 3 != 0 {
return Err(SpkError::BadType2("RSIZE not 2 + 3N"));
}
let n_coef = (rsize - 2) / 3;
if n_coef == 0 {
return Err(SpkError::BadType2("degree < 0"));
}
if intlen <= 0.0 {
return Err(SpkError::BadType2("INTLEN <= 0"));
}
Ok(SpkType2 {
file: file.clone(),
init,
intlen,
rsize,
n_records,
n_coef,
start_addr,
})
}
fn evaluate(&self, et: f64) -> Result<[f64; 6], SpkError> {
let raw_idx = ((et - self.init) / self.intlen).floor() as isize;
let idx = raw_idx.clamp(0, self.n_records as isize - 1) as usize;
let rec_start = self.start_addr + (idx * self.rsize) as u32;
let rec_end = rec_start + self.rsize as u32 - 1;
let rec = self.file.read_doubles(rec_start, rec_end)?;
let mid = rec[0];
let radius = rec[1];
if radius == 0.0 {
return Err(SpkError::BadType2("RADIUS == 0"));
}
let s = (et - mid) / radius;
let n = self.n_coef;
let xc = &rec[2..2 + n];
let yc = &rec[2 + n..2 + 2 * n];
let zc = &rec[2 + 2 * n..2 + 3 * n];
let (x, vx) = cheby_val_and_deriv(xc, s);
let (y, vy) = cheby_val_and_deriv(yc, s);
let (z, vz) = cheby_val_and_deriv(zc, s);
let inv_r = 1.0 / radius;
Ok([x, y, z, vx * inv_r, vy * inv_r, vz * inv_r])
}
}
#[derive(Clone)]
struct SpkType3 {
file: DafFile,
init: f64,
intlen: f64,
rsize: usize,
n_records: usize,
n_coef: usize,
start_addr: u32,
}
impl SpkType3 {
fn from_segment(file: &DafFile, start_addr: u32, end_addr: u32) -> Result<Self, SpkError> {
let trailer = file.read_doubles(end_addr - 3, end_addr)?;
let init = trailer[0];
let intlen = trailer[1];
let rsize = trailer[2] as usize;
let n_records = trailer[3] as usize;
if rsize < 2 || (rsize - 2) % 6 != 0 {
return Err(SpkError::BadType2("RSIZE not 2 + 6N"));
}
let n_coef = (rsize - 2) / 6;
if n_coef == 0 || intlen <= 0.0 {
return Err(SpkError::BadType2("degree<0 or INTLEN<=0"));
}
Ok(SpkType3 {
file: file.clone(),
init,
intlen,
rsize,
n_records,
n_coef,
start_addr,
})
}
fn evaluate(&self, et: f64) -> Result<[f64; 6], SpkError> {
let raw_idx = ((et - self.init) / self.intlen).floor() as isize;
let idx = raw_idx.clamp(0, self.n_records as isize - 1) as usize;
let rec_start = self.start_addr + (idx * self.rsize) as u32;
let rec_end = rec_start + self.rsize as u32 - 1;
let rec = self.file.read_doubles(rec_start, rec_end)?;
let mid = rec[0];
let radius = rec[1];
let s = (et - mid) / radius;
let n = self.n_coef;
let xc = &rec[2..2 + n];
let yc = &rec[2 + n..2 + 2 * n];
let zc = &rec[2 + 2 * n..2 + 3 * n];
let vxc = &rec[2 + 3 * n..2 + 4 * n];
let vyc = &rec[2 + 4 * n..2 + 5 * n];
let vzc = &rec[2 + 5 * n..2 + 6 * n];
let (x, _) = cheby_val_and_deriv(xc, s);
let (y, _) = cheby_val_and_deriv(yc, s);
let (z, _) = cheby_val_and_deriv(zc, s);
let (vx, _) = cheby_val_and_deriv(vxc, s);
let (vy, _) = cheby_val_and_deriv(vyc, s);
let (vz, _) = cheby_val_and_deriv(vzc, s);
Ok([x, y, z, vx, vy, vz])
}
}
struct DiscreteMeta {
window_or_degree: usize,
n_states: usize,
states_start: u32,
epochs_start: u32,
}
impl DiscreteMeta {
fn from_segment(file: &DafFile, start_addr: u32, end_addr: u32) -> Result<Self, SpkError> {
let tail = file.read_doubles(end_addr - 1, end_addr)?;
let window_or_degree = tail[0] as usize;
let n_states = tail[1] as usize;
if n_states == 0 {
return Err(SpkError::BadType2("empty discrete-state segment"));
}
let n_dir = n_states / 100;
let states_start = start_addr;
let epochs_start = start_addr + (6 * n_states) as u32;
let expected_end = epochs_start + n_states as u32 + n_dir as u32 + 2 - 1;
if expected_end != end_addr {
return Err(SpkError::BadType2("segment size does not match trailer"));
}
Ok(DiscreteMeta {
window_or_degree,
n_states,
states_start,
epochs_start,
})
}
}
#[derive(Clone)]
struct SpkType9 {
file: DafFile,
meta_degree: usize,
n_states: usize,
states_start: u32,
epochs_start: u32,
}
impl SpkType9 {
fn from_segment(file: &DafFile, start_addr: u32, end_addr: u32) -> Result<Self, SpkError> {
let meta = DiscreteMeta::from_segment(file, start_addr, end_addr)?;
Ok(SpkType9 {
file: file.clone(),
meta_degree: meta.window_or_degree,
n_states: meta.n_states,
states_start: meta.states_start,
epochs_start: meta.epochs_start,
})
}
fn evaluate(&self, et: f64) -> Result<[f64; 6], SpkError> {
let window = self.meta_degree + 1;
let (i0, count) = pick_window(&self.file, self.epochs_start, self.n_states, window, et)?;
let epochs = self.file.read_doubles(
self.epochs_start + i0 as u32,
self.epochs_start + (i0 + count - 1) as u32,
)?;
let states = self.file.read_doubles(
self.states_start + (6 * i0) as u32,
self.states_start + (6 * (i0 + count) - 1) as u32,
)?;
let mut out = [0.0_f64; 6];
let mut comp = vec![0.0_f64; count];
for k in 0..6 {
for j in 0..count {
comp[j] = states[6 * j + k];
}
out[k] = lagrange_eval(&epochs, &comp, et);
}
Ok(out)
}
}
#[derive(Clone)]
struct SpkType13 {
file: DafFile,
window_size: usize,
n_states: usize,
states_start: u32,
epochs_start: u32,
}
impl SpkType13 {
fn from_segment(file: &DafFile, start_addr: u32, end_addr: u32) -> Result<Self, SpkError> {
let meta = DiscreteMeta::from_segment(file, start_addr, end_addr)?;
let window_size = meta.window_or_degree + 1;
if window_size < 2 {
return Err(SpkError::BadType2("Type 13 window size < 2"));
}
Ok(SpkType13 {
file: file.clone(),
window_size,
n_states: meta.n_states,
states_start: meta.states_start,
epochs_start: meta.epochs_start,
})
}
fn evaluate(&self, et: f64) -> Result<[f64; 6], SpkError> {
let (i0, count) = pick_window(
&self.file,
self.epochs_start,
self.n_states,
self.window_size,
et,
)?;
let epochs = self.file.read_doubles(
self.epochs_start + i0 as u32,
self.epochs_start + (i0 + count - 1) as u32,
)?;
let states = self.file.read_doubles(
self.states_start + (6 * i0) as u32,
self.states_start + (6 * (i0 + count) - 1) as u32,
)?;
let mut out = [0.0_f64; 6];
let mut pos_vals = vec![0.0_f64; count];
let mut vel_vals = vec![0.0_f64; count];
for axis in 0..3 {
for j in 0..count {
pos_vals[j] = states[6 * j + axis];
vel_vals[j] = states[6 * j + 3 + axis];
}
let (p, v) = hermite_eval(&epochs, &pos_vals, &vel_vals, et);
out[axis] = p;
out[3 + axis] = v;
}
Ok(out)
}
}
fn pick_window(
file: &DafFile,
epochs_start: u32,
n_states: usize,
window: usize,
et: f64,
) -> Result<(usize, usize), SpkError> {
if window == 0 {
return Err(SpkError::BadType2("window size 0"));
}
let count = window.min(n_states);
let epochs = file.read_doubles(epochs_start, epochs_start + n_states as u32 - 1)?;
let mut lo = 0usize;
let mut hi = n_states;
while lo < hi {
let mid = (lo + hi) / 2;
if epochs[mid] <= et {
lo = mid + 1;
} else {
hi = mid;
}
}
let left_idx = lo.saturating_sub(1);
let half = (count - 1) / 2;
let start = left_idx.saturating_sub(half);
let start = start.min(n_states - count);
Ok((start, count))
}
fn lagrange_eval(xs: &[f64], ys: &[f64], x: f64) -> f64 {
let n = xs.len();
for i in 0..n {
if xs[i] == x {
return ys[i];
}
}
let mut w = vec![1.0_f64; n];
for i in 0..n {
for j in 0..n {
if i != j {
w[i] *= xs[i] - xs[j];
}
}
w[i] = 1.0 / w[i];
}
let mut num = 0.0_f64;
let mut den = 0.0_f64;
for i in 0..n {
let term = w[i] / (x - xs[i]);
num += term * ys[i];
den += term;
}
num / den
}
fn hermite_eval(xs: &[f64], ys: &[f64], dys: &[f64], x: f64) -> (f64, f64) {
let n = xs.len();
debug_assert_eq!(ys.len(), n);
debug_assert_eq!(dys.len(), n);
let m = 2 * n;
let mut z = vec![0.0_f64; m];
let mut f = vec![0.0_f64; m];
for i in 0..n {
z[2 * i] = xs[i];
z[2 * i + 1] = xs[i];
f[2 * i] = ys[i];
f[2 * i + 1] = ys[i];
}
let prev = f.clone();
let mut col = vec![0.0_f64; m];
for i in 0..m - 1 {
if (i % 2) == 0 {
col[i] = dys[i / 2];
} else {
col[i] = (prev[i + 1] - prev[i]) / (z[i + 1] - z[i]);
}
}
let mut coeffs = vec![0.0_f64; m];
coeffs[0] = f[0];
coeffs[1] = col[0];
let mut cur = col.clone();
for k in 2..m {
let mut next = vec![0.0_f64; m - k];
for i in 0..m - k {
next[i] = (cur[i + 1] - cur[i]) / (z[i + k] - z[i]);
}
coeffs[k] = next[0];
cur = next;
}
let mut val = coeffs[0];
let mut der = 0.0_f64;
let mut prod = 1.0_f64; let mut dprod = 0.0_f64; for k in 1..m {
let dprod_new = prod + (x - z[k - 1]) * dprod;
let prod_new = (x - z[k - 1]) * prod;
val += coeffs[k] * prod_new;
der += coeffs[k] * dprod_new;
prod = prod_new;
dprod = dprod_new;
}
(val, der)
}
pub(crate) fn cheby_val_and_deriv(c: &[f64], s: f64) -> (f64, f64) {
let n = c.len();
if n == 0 {
return (0.0, 0.0);
}
if n == 1 {
return (c[0], 0.0);
}
let mut t_prev = 1.0;
let mut t_curr = s;
let mut dt_prev = 0.0;
let mut dt_curr = 1.0;
let mut val = c[0] * t_prev + c[1] * t_curr;
let mut der = c[1] * dt_curr;
for &ck in &c[2..] {
let t_next = 2.0 * s * t_curr - t_prev;
let dt_next = 2.0 * t_curr + 2.0 * s * dt_curr - dt_prev;
val += ck * t_next;
der += ck * dt_next;
t_prev = t_curr;
t_curr = t_next;
dt_prev = dt_curr;
dt_curr = dt_next;
}
(val, der)
}
pub struct SpkFile {
segments: Vec<SpkSegment>,
index: HashMap<(i32, i32), Vec<usize>>,
}
impl SpkFile {
pub fn open<P: AsRef<Path>>(path: P) -> Result<Self, SpkError> {
let daf = DafFile::open(path)?;
Self::from_daf(daf)
}
pub fn from_daf(daf: DafFile) -> Result<Self, SpkError> {
let mut segments = Vec::new();
let mut index: HashMap<(i32, i32), Vec<usize>> = HashMap::new();
for summary in daf.summaries()? {
if summary.doubles.len() < 2 || summary.integers.len() < 6 {
continue;
}
let start_et = summary.doubles[0];
let end_et = summary.doubles[1];
let target = summary.integers[0];
let center = summary.integers[1];
let frame = summary.integers[2];
let data_type = summary.integers[3];
let start_addr = summary.integers[4] as u32;
let end_addr = summary.integers[5] as u32;
let payload = match data_type {
2 => SpkPayload::Type2(SpkType2::from_segment(&daf, start_addr, end_addr)?),
3 => SpkPayload::Type3(SpkType3::from_segment(&daf, start_addr, end_addr)?),
9 => SpkPayload::Type9(SpkType9::from_segment(&daf, start_addr, end_addr)?),
13 => SpkPayload::Type13(SpkType13::from_segment(&daf, start_addr, end_addr)?),
_ => SpkPayload::Unsupported,
};
index
.entry((target, center))
.or_default()
.push(segments.len());
segments.push(SpkSegment {
target,
center,
frame,
data_type,
start_et,
end_et,
start_addr,
end_addr,
name: summary.name,
payload,
});
}
Ok(SpkFile { segments, index })
}
pub fn segments(&self) -> &[SpkSegment] {
&self.segments
}
pub fn state_in_frame(
&self,
target: i32,
center: i32,
et: f64,
out_frame: NaifFrame,
) -> Result<[f64; 6], SpkError> {
let s = self.state(target, center, et)?;
Ok(rotate_state(NaifFrame::J2000, out_frame, &s))
}
pub fn state(&self, target: i32, center: i32, et: f64) -> Result<[f64; 6], SpkError> {
if target == center {
return Ok([0.0; 6]);
}
if let Some(s) = self.try_direct(target, center, et)? {
return Ok(s);
}
let t_ssb = self.state_wrt_ssb(target, et)?;
let c_ssb = self.state_wrt_ssb(center, et)?;
let mut out = [0.0_f64; 6];
for i in 0..6 {
out[i] = t_ssb[i] - c_ssb[i];
}
Ok(out)
}
fn try_direct(&self, target: i32, center: i32, et: f64) -> Result<Option<[f64; 6]>, SpkError> {
if let Some(indices) = self.index.get(&(target, center)) {
for &i in indices {
let seg = &self.segments[i];
if et >= seg.start_et && et <= seg.end_et {
return Ok(Some(Self::eval_segment(seg, et)?));
}
}
}
if let Some(indices) = self.index.get(&(center, target)) {
for &i in indices {
let seg = &self.segments[i];
if et >= seg.start_et && et <= seg.end_et {
let mut s = Self::eval_segment(seg, et)?;
for v in s.iter_mut() {
*v = -*v;
}
return Ok(Some(s));
}
}
}
Ok(None)
}
fn eval_segment(seg: &SpkSegment, et: f64) -> Result<[f64; 6], SpkError> {
match &seg.payload {
SpkPayload::Type2(t) => t.evaluate(et),
SpkPayload::Type3(t) => t.evaluate(et),
SpkPayload::Type9(t) => t.evaluate(et),
SpkPayload::Type13(t) => t.evaluate(et),
SpkPayload::Unsupported => Err(SpkError::UnsupportedType(seg.data_type)),
}
}
fn state_wrt_ssb(&self, body: i32, et: f64) -> Result<[f64; 6], SpkError> {
if body == 0 {
return Ok([0.0; 6]);
}
let mut total = [0.0_f64; 6];
let mut cur = body;
for _ in 0..32 {
let (delta, next_center) = self.step_toward_ssb(cur, et)?;
for i in 0..6 {
total[i] += delta[i];
}
if next_center == 0 {
return Ok(total);
}
if next_center == body {
return Err(SpkError::NoCoverage {
target: body,
center: 0,
et,
});
}
cur = next_center;
}
Err(SpkError::NoCoverage {
target: body,
center: 0,
et,
})
}
fn step_toward_ssb(&self, body: i32, et: f64) -> Result<([f64; 6], i32), SpkError> {
let mut preferred: Option<(&SpkSegment, [f64; 6])> = None;
for seg in &self.segments {
if seg.target != body {
continue;
}
if et < seg.start_et || et > seg.end_et {
continue;
}
let s = Self::eval_segment(seg, et)?;
if seg.center == 0 {
return Ok((s, 0));
}
if preferred.is_none() {
preferred = Some((seg, s));
}
}
match preferred {
Some((seg, s)) => Ok((s, seg.center)),
None => Err(SpkError::NoCoverage {
target: body,
center: 0,
et,
}),
}
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn hermite_reproduces_cubic() {
let xs = [0.0, 1.0];
let ys = [0.0, 1.0];
let dys = [0.0, 3.0];
for x in [0.0, 0.25, 0.5, 0.75, 1.0] {
let (v, d) = hermite_eval(&xs, &ys, &dys, x);
assert!((v - x * x * x).abs() < 1e-14, "v={v}");
assert!((d - 3.0 * x * x).abs() < 1e-14, "d={d}");
}
}
#[test]
fn hermite_reproduces_quartic_with_three_knots() {
let xs = [-1.0, 0.0, 1.0];
let f = |x: f64| x.powi(4) - 2.0 * x * x + 3.0;
let df = |x: f64| 4.0 * x.powi(3) - 4.0 * x;
let ys: Vec<_> = xs.iter().map(|&x| f(x)).collect();
let dys: Vec<_> = xs.iter().map(|&x| df(x)).collect();
for x in [-0.9, -0.5, 0.1, 0.7] {
let (v, d) = hermite_eval(&xs, &ys, &dys, x);
assert!((v - f(x)).abs() < 1e-12, "v={v} expected {}", f(x));
assert!((d - df(x)).abs() < 1e-12, "d={d} expected {}", df(x));
}
}
#[test]
fn chebyshev_value_matches_reference() {
let c = [1.0, 2.0, 3.0];
let (v, d) = cheby_val_and_deriv(&c, 0.5);
assert!((v - (6.0 * 0.25 + 1.0 - 2.0)).abs() < 1e-14);
assert!((d - (12.0 * 0.5 + 2.0)).abs() < 1e-14);
}
#[test]
fn chebyshev_derivative_matches_finite_difference() {
let c = [0.3, -1.2, 0.7, 0.4, -0.15, 0.02];
let h = 1e-6;
for &s in &[-0.9_f64, -0.3, 0.0, 0.25, 0.7] {
let (_, d_analytic) = cheby_val_and_deriv(&c, s);
let (vp, _) = cheby_val_and_deriv(&c, s + h);
let (vm, _) = cheby_val_and_deriv(&c, s - h);
let d_fd = (vp - vm) / (2.0 * h);
assert!(
(d_analytic - d_fd).abs() < 1e-8,
"analytic={d_analytic} fd={d_fd}"
);
}
}
#[test]
fn chebyshev_degree_zero_is_constant() {
let (v, d) = cheby_val_and_deriv(&[2.5], 0.42);
assert_eq!(v, 2.5);
assert_eq!(d, 0.0);
}
#[test]
fn lagrange_reproduces_quartic_exactly() {
let xs = [-2.0_f64, -0.5, 0.1, 1.3, 2.7];
let f = |x: f64| 1.0 + 2.0 * x - 3.0 * x * x + 0.5 * x.powi(3) - 0.1 * x.powi(4);
let ys: Vec<_> = xs.iter().map(|&x| f(x)).collect();
for x in [-1.7_f64, -0.2, 0.0, 0.8, 2.5] {
let v = lagrange_eval(&xs, &ys, x);
assert!((v - f(x)).abs() < 1e-12, "lagrange={v} expected={}", f(x));
}
}
#[test]
fn hermite_derivative_matches_finite_difference() {
let xs: Vec<f64> = (0..9).map(|i| -2.0 + 0.5 * (i as f64)).collect();
let ys: Vec<f64> = xs.iter().map(|&x| x.sin()).collect();
let dys: Vec<f64> = xs.iter().map(|&x| x.cos()).collect();
let x0 = 0.37;
let h = 1e-5;
let (v0, d_analytic) = hermite_eval(&xs, &ys, &dys, x0);
let (vp, _) = hermite_eval(&xs, &ys, &dys, x0 + h);
let (vm, _) = hermite_eval(&xs, &ys, &dys, x0 - h);
let d_fd = (vp - vm) / (2.0 * h);
assert!((v0 - x0.sin()).abs() < 1e-8);
assert!(
(d_analytic - d_fd).abs() < 1e-6,
"analytic={d_analytic} fd={d_fd}"
);
}
}
#[cfg(test)]
mod synthetic_spk_tests {
use super::*;
use crate::daf::{DOUBLE_BYTES, RECORD_BYTES};
use std::io::Write;
use tempfile::NamedTempFile;
struct SegSpec {
target: i32,
center: i32,
frame: i32,
data_type: i32, start_et: f64,
end_et: f64,
mid: f64,
radius: f64,
coefs: Vec<Vec<f64>>,
name: String,
}
fn build_spk(segs: &[SegSpec]) -> NamedTempFile {
let nd = 2u32;
let ni = 6u32;
let ss_doubles = nd as usize + (ni as usize).div_ceil(2); let summary_bytes = ss_doubles * DOUBLE_BYTES;
let doubles_per_record = (RECORD_BYTES / DOUBLE_BYTES) as u32;
let mut per_segment_payload: Vec<(u32, u32, Vec<f64>)> = Vec::new();
let mut cursor_doubles: u32 = 1 + 3 * doubles_per_record;
for s in segs {
let n_coef_each = s.coefs[0].len();
let expected_blocks = if s.data_type == 2 { 3 } else { 6 };
assert_eq!(s.coefs.len(), expected_blocks, "wrong coef block count");
for c in &s.coefs {
assert_eq!(c.len(), n_coef_each, "unequal coef block lengths");
}
let rsize = 2 + expected_blocks * n_coef_each;
let mut data: Vec<f64> = Vec::with_capacity(rsize + 4);
data.push(s.mid);
data.push(s.radius);
for block in &s.coefs {
data.extend_from_slice(block);
}
data.push(s.start_et);
data.push(s.end_et - s.start_et);
data.push(rsize as f64);
data.push(1.0);
let start_addr = cursor_doubles;
let end_addr = start_addr + data.len() as u32 - 1;
cursor_doubles = end_addr + 1;
per_segment_payload.push((start_addr, end_addr, data));
}
let mut record1 = vec![0u8; RECORD_BYTES];
record1[0..8].copy_from_slice(b"DAF/SPK ");
record1[8..12].copy_from_slice(&nd.to_le_bytes());
record1[12..16].copy_from_slice(&ni.to_le_bytes());
for b in &mut record1[16..76] {
*b = b' ';
}
record1[76..80].copy_from_slice(&2u32.to_le_bytes()); record1[80..84].copy_from_slice(&2u32.to_le_bytes()); record1[84..88].copy_from_slice(&0u32.to_le_bytes()); record1[88..96].copy_from_slice(b"LTL-IEEE");
let mut record2 = vec![0u8; RECORD_BYTES];
record2[0..8].copy_from_slice(&0.0f64.to_le_bytes()); record2[8..16].copy_from_slice(&0.0f64.to_le_bytes()); record2[16..24].copy_from_slice(&(segs.len() as f64).to_le_bytes()); for (i, s) in segs.iter().enumerate() {
let (start_addr, end_addr, _) = &per_segment_payload[i];
let soff = 24 + i * summary_bytes;
record2[soff..soff + 8].copy_from_slice(&s.start_et.to_le_bytes());
record2[soff + 8..soff + 16].copy_from_slice(&s.end_et.to_le_bytes());
let int_start = soff + 2 * DOUBLE_BYTES;
record2[int_start..int_start + 4].copy_from_slice(&s.target.to_le_bytes());
record2[int_start + 4..int_start + 8].copy_from_slice(&s.center.to_le_bytes());
record2[int_start + 8..int_start + 12].copy_from_slice(&s.frame.to_le_bytes());
record2[int_start + 12..int_start + 16].copy_from_slice(&s.data_type.to_le_bytes());
record2[int_start + 16..int_start + 20]
.copy_from_slice(&(*start_addr as i32).to_le_bytes());
record2[int_start + 20..int_start + 24]
.copy_from_slice(&(*end_addr as i32).to_le_bytes());
}
let mut record3 = vec![b' '; RECORD_BYTES];
for (i, s) in segs.iter().enumerate() {
let noff = i * summary_bytes;
let nbytes = s.name.as_bytes();
let n = nbytes.len().min(summary_bytes);
record3[noff..noff + n].copy_from_slice(&nbytes[..n]);
}
let mut data_bytes = Vec::new();
for (_, _, data) in &per_segment_payload {
for d in data {
data_bytes.extend_from_slice(&d.to_le_bytes());
}
}
while data_bytes.len() % RECORD_BYTES != 0 {
data_bytes.push(0);
}
let mut all = Vec::new();
all.extend_from_slice(&record1);
all.extend_from_slice(&record2);
all.extend_from_slice(&record3);
all.extend_from_slice(&data_bytes);
let mut tmp = NamedTempFile::new().unwrap();
tmp.write_all(&all).unwrap();
tmp.flush().unwrap();
tmp
}
fn one_record_type2(
target: i32,
center: i32,
start_et: f64,
end_et: f64,
coefs_x: &[f64],
coefs_y: &[f64],
coefs_z: &[f64],
) -> SegSpec {
SegSpec {
target,
center,
frame: 1, data_type: 2,
start_et,
end_et,
mid: 0.5 * (start_et + end_et),
radius: 0.5 * (end_et - start_et),
coefs: vec![coefs_x.to_vec(), coefs_y.to_vec(), coefs_z.to_vec()],
name: format!("SYN {target} FROM {center}"),
}
}
#[test]
fn type2_state_at_midpoint_returns_degree_zero_position() {
let seg = one_record_type2(3, 0, -100.0, 100.0, &[1.0, 2.0], &[3.0, 0.0], &[4.0, -1.0]);
let tmp = build_spk(&[seg]);
let spk = SpkFile::open(tmp.path()).expect("open");
let s = spk.state(3, 0, 0.0).expect("state");
assert!((s[0] - 1.0).abs() < 1e-14); assert!((s[1] - 3.0).abs() < 1e-14); assert!((s[2] - 4.0).abs() < 1e-14); assert!((s[3] - 0.02).abs() < 1e-14);
assert!((s[4] - 0.0).abs() < 1e-14);
assert!((s[5] - (-0.01)).abs() < 1e-14);
}
#[test]
fn type2_hand_computed_position_and_velocity() {
let seg = one_record_type2(
5,
0,
-50.0,
50.0,
&[1.0, 2.0, 3.0],
&[0.0, 0.0, 0.0],
&[0.0, 0.0, 0.0],
);
let tmp = build_spk(&[seg]);
let spk = SpkFile::open(tmp.path()).expect("open");
let s = spk.state(5, 0, 25.0).expect("state");
assert!((s[0] - 0.5).abs() < 1e-14, "x={}", s[0]);
assert!((s[3] - 0.16).abs() < 1e-14, "vx={}", s[3]);
}
#[test]
fn reverse_direction_negates_returned_state() {
let seg = one_record_type2(3, 10, -10.0, 10.0, &[7.0, 0.5], &[-2.0, 1.5], &[3.0, -0.25]);
let tmp = build_spk(&[seg]);
let spk = SpkFile::open(tmp.path()).expect("open");
let forward = spk.state(3, 10, 0.0).expect("forward");
let reverse = spk.state(10, 3, 0.0).expect("reverse");
for i in 0..6 {
assert!((forward[i] + reverse[i]).abs() < 1e-14, "axis {i}");
}
}
#[test]
fn chain_walk_composes_through_intermediate_center() {
let seg_earth_from_emb =
one_record_type2(399, 3, -100.0, 100.0, &[1.0, 0.0], &[2.0, 0.0], &[3.0, 0.0]);
let seg_emb_from_ssb = one_record_type2(
3,
0,
-100.0,
100.0,
&[100.0, 0.0],
&[200.0, 0.0],
&[300.0, 0.0],
);
let seg_sun_from_ssb =
one_record_type2(10, 0, -100.0, 100.0, &[0.1, 0.0], &[0.2, 0.0], &[0.3, 0.0]);
let tmp = build_spk(&[seg_earth_from_emb, seg_emb_from_ssb, seg_sun_from_ssb]);
let spk = SpkFile::open(tmp.path()).expect("open");
let earth_ssb = spk.state(399, 0, 0.0).expect("earth ssb");
assert!((earth_ssb[0] - 101.0).abs() < 1e-14);
assert!((earth_ssb[1] - 202.0).abs() < 1e-14);
assert!((earth_ssb[2] - 303.0).abs() < 1e-14);
let earth_sun = spk.state(399, 10, 0.0).expect("earth sun");
assert!((earth_sun[0] - (101.0 - 0.1)).abs() < 1e-14);
assert!((earth_sun[1] - (202.0 - 0.2)).abs() < 1e-14);
assert!((earth_sun[2] - (303.0 - 0.3)).abs() < 1e-14);
}
#[test]
fn no_coverage_when_et_outside_segment() {
let seg = one_record_type2(5, 0, -10.0, 10.0, &[1.0, 0.0], &[2.0, 0.0], &[3.0, 0.0]);
let tmp = build_spk(&[seg]);
let spk = SpkFile::open(tmp.path()).expect("open");
let err = spk.state(5, 0, 1000.0).unwrap_err();
assert!(matches!(err, SpkError::NoCoverage { .. }));
}
#[test]
fn no_coverage_when_target_missing() {
let seg = one_record_type2(5, 0, -10.0, 10.0, &[1.0, 0.0], &[2.0, 0.0], &[3.0, 0.0]);
let tmp = build_spk(&[seg]);
let spk = SpkFile::open(tmp.path()).expect("open");
let err = spk.state(9999, 0, 0.0).unwrap_err();
assert!(matches!(err, SpkError::NoCoverage { .. }));
}
#[test]
fn state_target_equals_center_is_zero_vector() {
let seg = one_record_type2(5, 0, -10.0, 10.0, &[1.0, 2.0], &[3.0, 4.0], &[5.0, 6.0]);
let tmp = build_spk(&[seg]);
let spk = SpkFile::open(tmp.path()).expect("open");
let s = spk.state(7, 7, 0.0).expect("identity");
assert_eq!(s, [0.0; 6]);
}
#[test]
fn type3_uses_separate_velocity_coefficients() {
let spec = SegSpec {
target: 7,
center: 0,
frame: 1,
data_type: 3,
start_et: -100.0,
end_et: 100.0,
mid: 0.0,
radius: 100.0,
coefs: vec![
vec![1.0, 0.0], vec![2.0, 0.0], vec![3.0, 0.0], vec![0.5, 0.0], vec![0.25, 0.0],
vec![-0.125, 0.0],
],
name: "SYN TYPE 3".to_string(),
};
let tmp = build_spk(&[spec]);
let spk = SpkFile::open(tmp.path()).expect("open");
let s = spk.state(7, 0, 0.0).expect("state");
assert!((s[0] - 1.0).abs() < 1e-14);
assert!((s[1] - 2.0).abs() < 1e-14);
assert!((s[2] - 3.0).abs() < 1e-14);
assert!((s[3] - 0.5).abs() < 1e-14, "vx={}", s[3]);
assert!((s[4] - 0.25).abs() < 1e-14, "vy={}", s[4]);
assert!((s[5] - (-0.125)).abs() < 1e-14, "vz={}", s[5]);
}
#[test]
fn state_selects_segment_by_epoch_range() {
let early = one_record_type2(5, 0, -100.0, 0.0, &[1.0, 0.0], &[0.0, 0.0], &[0.0, 0.0]);
let late = one_record_type2(5, 0, 0.0, 100.0, &[2.0, 0.0], &[0.0, 0.0], &[0.0, 0.0]);
let tmp = build_spk(&[early, late]);
let spk = SpkFile::open(tmp.path()).expect("open");
let s_early = spk.state(5, 0, -50.0).expect("early");
assert!((s_early[0] - 1.0).abs() < 1e-14);
let s_late = spk.state(5, 0, 50.0).expect("late");
assert!((s_late[0] - 2.0).abs() < 1e-14);
}
#[test]
fn derivative_matches_central_finite_difference() {
let seg = one_record_type2(
5,
0,
-1000.0,
1000.0,
&[0.1, -0.05, 0.02, -0.003],
&[0.2, 0.04, -0.01, 0.005],
&[0.3, 0.08, 0.015, -0.004],
);
let tmp = build_spk(&[seg]);
let spk = SpkFile::open(tmp.path()).expect("open");
for &et in &[-500.0_f64, -100.0, 0.0, 250.0, 750.0] {
let h = 1e-3_f64;
let s = spk.state(5, 0, et).unwrap();
let sp = spk.state(5, 0, et + h).unwrap();
let sm = spk.state(5, 0, et - h).unwrap();
for axis in 0..3 {
let fd = (sp[axis] - sm[axis]) / (2.0 * h);
assert!(
(s[3 + axis] - fd).abs() < 1e-9,
"axis {axis} et={et}: analytic={} fd={}",
s[3 + axis],
fd
);
}
}
}
type Type2Record = (f64, f64, Vec<f64>, Vec<f64>, Vec<f64>);
fn build_spk_multi_record_type2(
target: i32,
center: i32,
init: f64,
intlen: f64,
records: &[Type2Record],
) -> NamedTempFile {
let n_records = records.len();
assert!(n_records > 0);
let n_coef = records[0].2.len();
for (_, _, xc, yc, zc) in records {
assert_eq!(xc.len(), n_coef);
assert_eq!(yc.len(), n_coef);
assert_eq!(zc.len(), n_coef);
}
let rsize = 2 + 3 * n_coef;
let mut data: Vec<f64> = Vec::with_capacity(rsize * n_records + 4);
for (mid, radius, xc, yc, zc) in records {
data.push(*mid);
data.push(*radius);
data.extend_from_slice(xc);
data.extend_from_slice(yc);
data.extend_from_slice(zc);
}
data.push(init);
data.push(intlen);
data.push(rsize as f64);
data.push(n_records as f64);
let doubles_per_record = (RECORD_BYTES / DOUBLE_BYTES) as u32;
let start_addr = 1 + 3 * doubles_per_record;
let end_addr = start_addr + data.len() as u32 - 1;
let mut record1 = vec![0u8; RECORD_BYTES];
record1[0..8].copy_from_slice(b"DAF/SPK ");
record1[8..12].copy_from_slice(&2u32.to_le_bytes());
record1[12..16].copy_from_slice(&6u32.to_le_bytes());
for b in &mut record1[16..76] {
*b = b' ';
}
record1[76..80].copy_from_slice(&2u32.to_le_bytes());
record1[80..84].copy_from_slice(&2u32.to_le_bytes());
record1[88..96].copy_from_slice(b"LTL-IEEE");
let mut record2 = vec![0u8; RECORD_BYTES];
record2[0..8].copy_from_slice(&0.0f64.to_le_bytes());
record2[8..16].copy_from_slice(&0.0f64.to_le_bytes());
record2[16..24].copy_from_slice(&1.0f64.to_le_bytes());
let start_et = init;
let end_et = init + intlen * n_records as f64;
record2[24..32].copy_from_slice(&start_et.to_le_bytes());
record2[32..40].copy_from_slice(&end_et.to_le_bytes());
let int_start = 40;
record2[int_start..int_start + 4].copy_from_slice(&target.to_le_bytes());
record2[int_start + 4..int_start + 8].copy_from_slice(¢er.to_le_bytes());
record2[int_start + 8..int_start + 12].copy_from_slice(&1i32.to_le_bytes());
record2[int_start + 12..int_start + 16].copy_from_slice(&2i32.to_le_bytes());
record2[int_start + 16..int_start + 20].copy_from_slice(&(start_addr as i32).to_le_bytes());
record2[int_start + 20..int_start + 24].copy_from_slice(&(end_addr as i32).to_le_bytes());
let mut record3 = vec![b' '; RECORD_BYTES];
let name = b"MULTI REC";
record3[..name.len()].copy_from_slice(name);
let mut data_bytes = Vec::new();
for d in &data {
data_bytes.extend_from_slice(&d.to_le_bytes());
}
while data_bytes.len() % RECORD_BYTES != 0 {
data_bytes.push(0);
}
let mut all = Vec::new();
all.extend_from_slice(&record1);
all.extend_from_slice(&record2);
all.extend_from_slice(&record3);
all.extend_from_slice(&data_bytes);
let mut tmp = NamedTempFile::new().unwrap();
tmp.write_all(&all).unwrap();
tmp.flush().unwrap();
tmp
}
#[test]
fn multi_record_segment_selects_correct_record_by_et() {
let tmp = build_spk_multi_record_type2(
5,
0,
0.0, 100.0, &[
(50.0, 50.0, vec![10.0, 0.0], vec![0.0, 0.0], vec![0.0, 0.0]),
(150.0, 50.0, vec![20.0, 0.0], vec![0.0, 0.0], vec![0.0, 0.0]),
(250.0, 50.0, vec![30.0, 0.0], vec![0.0, 0.0], vec![0.0, 0.0]),
],
);
let spk = SpkFile::open(tmp.path()).expect("open");
let s0 = spk.state(5, 0, 25.0).expect("rec 0");
assert!((s0[0] - 10.0).abs() < 1e-14, "record 0 x={}", s0[0]);
let s1 = spk.state(5, 0, 125.0).expect("rec 1");
assert!((s1[0] - 20.0).abs() < 1e-14, "record 1 x={}", s1[0]);
let s2 = spk.state(5, 0, 250.0).expect("rec 2");
assert!((s2[0] - 30.0).abs() < 1e-14, "record 2 x={}", s2[0]);
let s3 = spk.state(5, 0, 300.0).expect("right edge");
assert!((s3[0] - 30.0).abs() < 1e-14, "edge x={}", s3[0]);
}
}