Add support for SKP Type 9 files

CHANGELOG.md

@@ -7,6 +7,10 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ## [Unreleased]
 
+### Added
+
+- Added support for SPICE kernels of type 9, this allows reading SOHO spice files.
+
 ### Changed
 
 - Comet Magnitude estimates now accepts two phase correction values instead of 1.

src/kete_core/src/spice/interpolation.rs

@@ -80,21 +80,22 @@ pub fn chebyshev3_evaluate_both(
 ///
 /// # Arguments
 ///
-/// * `times` - Times where `x` and `d`x are evaluated at.
-/// * `x` - The values of the function `f` evaluated at the specified times.
-/// * `dx` - The values of the derivative of the function `f`.
+/// * `times` - Times where the function `f` is evaluated at.
+/// * `y_vals` - The values of the function `f` at the specified times.
+/// * `dy` - The values of the derivative of the function `f`.
 /// * `eval_time` - Time at which to evaluate the interpolation function.
-pub fn hermite_interpolation(times: &[f64], x: &[f64], dx: &[f64], eval_time: f64) -> (f64, f64) {
-    assert_eq!(times.len(), x.len());
-    assert_eq!(times.len(), dx.len());
+#[inline(always)]
+pub fn hermite_interpolation(times: &[f64], y: &[f64], dy: &[f64], eval_time: f64) -> (f64, f64) {
+    debug_assert_eq!(times.len(), y.len());
+    debug_assert_eq!(times.len(), dy.len());
 
-    let n = x.len();
+    let n = y.len();
 
-    let mut work = DVector::<f64>::zeros(2 * x.len());
-    let mut d_work = DVector::<f64>::zeros(2 * x.len());
-    for (idx, (x0, dx0)) in x.iter().zip(dx).enumerate() {
-        work[2 * idx] = *x0;
-        work[2 * idx + 1] = *dx0;
+    let mut work = DVector::<f64>::zeros(2 * y.len());
+    let mut d_work = DVector::<f64>::zeros(2 * y.len());
+    for (idx, (y0, dy0)) in y.iter().zip(dy).enumerate() {
+        work[2 * idx] = *y0;
+        work[2 * idx + 1] = *dy0;
     }
 
     for idx in 1..n {
@@ -132,3 +133,63 @@ pub fn hermite_interpolation(times: &[f64], x: &[f64], dx: &[f64], eval_time: f6
     }
     (work[0], d_work[0])
 }
+
+/// Interpolate using lagrange interpolation.
+///
+/// # Arguments
+///
+/// * `times` - Times where the function `f` is evaluated at.
+/// * `y_vals` - The values of the function `f` at the specified times.
+/// * `eval_time` - Time at which to evaluate the interpolation function.
+pub fn lagrange_interpolation(x: &[f64], y: &mut [f64], eval_time: f64) -> f64 {
+    debug_assert_eq!(x.len(), y.len());
+
+    // implementation of newton interpolation
+    for idx in 1..x.len() {
+        for idy in idx..x.len() {
+            y[idy] = (y[idy] - y[idx - 1]) / (x[idy] - x[idx - 1]);
+        }
+    }
+    let deg = x.len() - 1;
+    let mut val = y[deg];
+    for k in 1..deg + 1 {
+        val = y[deg - k] + (eval_time - x[deg - k]) * val;
+    }
+    val
+}
+
+#[cfg(test)]
+mod tests {
+    use super::lagrange_interpolation;
+
+    #[test]
+    fn test_lagrange_interpolation() {
+        let times = vec![0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0];
+        let y = times.clone();
+
+        for v in 0..100 {
+            let eval_time = (v as f64) / 100. * 9.0;
+            let interp = lagrange_interpolation(&times, &mut y.clone(), eval_time);
+            assert!((interp - eval_time).abs() < 1e-12);
+        }
+
+        let y: Vec<_> = times
+            .iter()
+            .map(|x| x + 1.75 * x.powi(2) - 3.0 * x.powi(3) - 11.0 * x.powi(4))
+            .collect();
+
+        for v in 0..100 {
+            let x = (v as f64) / 100. * 9.0;
+            let expected = x + 1.75 * x.powi(2) - 3.0 * x.powi(3) - 11.0 * x.powi(4);
+            let interp = lagrange_interpolation(&times, &mut y.clone(), x);
+            assert!(
+                (interp - expected).abs() < 1e-10,
+                "x={} interp={} expected={} diff={}",
+                x,
+                interp,
+                expected,
+                interp - expected
+            );
+        }
+    }
+}

src/kete_core/src/spice/spk_segments.rs

@@ -29,6 +29,7 @@ use std::fmt::Debug;
 pub enum SpkSegmentType {
     Type1(SpkSegmentType1),
     Type2(SpkSegmentType2),
+    Type9(SpkSegmentType9),
     Type10(SpkSegmentType10),
     Type13(SpkSegmentType13),
     Type21(SpkSegmentType21),
@@ -40,6 +41,7 @@ impl SpkSegmentType {
         match segment_type {
             1 => Ok(SpkSegmentType::Type1(array.into())),
             2 => Ok(SpkSegmentType::Type2(array.into())),
+            9 => Ok(SpkSegmentType::Type9(array.into())),
             13 => Ok(SpkSegmentType::Type13(array.into())),
             10 => Ok(SpkSegmentType::Type10(array.into())),
             21 => Ok(SpkSegmentType::Type21(array.into())),
@@ -56,6 +58,7 @@ impl From<SpkSegmentType> for DafArray {
         match value {
             SpkSegmentType::Type1(seg) => seg.array,
             SpkSegmentType::Type2(seg) => seg.array,
+            SpkSegmentType::Type9(seg) => seg.array,
             SpkSegmentType::Type10(seg) => seg.array.array,
             SpkSegmentType::Type13(seg) => seg.array,
             SpkSegmentType::Type21(seg) => seg.array,
@@ -149,6 +152,7 @@ impl SpkSegment {
         let (pos, vel) = match &self.segment {
             SpkSegmentType::Type1(v) => v.try_get_pos_vel(self, jd)?,
             SpkSegmentType::Type2(v) => v.try_get_pos_vel(self, jd)?,
+            SpkSegmentType::Type9(v) => v.try_get_pos_vel(self, jd)?,
             SpkSegmentType::Type10(v) => v.try_get_pos_vel(self, jd)?,
             SpkSegmentType::Type13(v) => v.try_get_pos_vel(self, jd)?,
             SpkSegmentType::Type21(v) => v.try_get_pos_vel(self, jd)?,
@@ -384,6 +388,99 @@ impl From<DafArray> for SpkSegmentType2 {
     }
 }
 
+// TODO: SPK Segment type 8 should be a minor variation on type 9. This was not
+// implemented here due to missing a valid SPK file to test against.
+
+/// Lagrange Interpolation (Uneven Time Steps)
+///
+/// This uses a collection of individual positions/velocities and interpolates between
+/// them using Lagrange interpolation.
+/// <https://naif.jpl.nasa.gov/pub/naif/toolkit_docs/FORTRAN/req/spk.html#Type%209:%20Lagrange%20Interpolation%20---%20Unequal%20Time%20Steps>
+#[derive(Debug)]
+pub struct SpkSegmentType9 {
+    array: DafArray,
+    poly_degree: usize,
+    n_records: usize,
+}
+
+impl From<DafArray> for SpkSegmentType9 {
+    fn from(array: DafArray) -> Self {
+        let n_records = array[array.len() - 1] as usize;
+        let poly_degree = array[array.len() - 2] as usize;
+
+        Self {
+            array,
+            poly_degree,
+            n_records,
+        }
+    }
+}
+
+/// Type 9 Record View
+/// A view into a record of type 9, provided mainly for clarity to the underlying
+/// data structure.
+struct Type9RecordView<'a> {
+    pos: &'a [f64; 3],
+    vel: &'a [f64; 3],
+}
+
+impl SpkSegmentType9 {
+    #[inline(always)]
+    fn get_record(&self, idx: usize) -> Type9RecordView {
+        unsafe {
+            let rec = self.array.data.get_unchecked(idx * 6..(idx + 1) * 6);
+            Type9RecordView {
+                pos: rec[0..3].try_into().unwrap(),
+                vel: rec[3..6].try_into().unwrap(),
+            }
+        }
+    }
+
+    #[inline(always)]
+    fn get_times(&self) -> &[f64] {
+        unsafe {
+            self.array
+                .data
+                .get_unchecked(self.n_records * 6..self.n_records * 7)
+        }
+    }
+
+    #[inline(always)]
+    fn try_get_pos_vel(&self, _: &SpkSegment, jd: f64) -> KeteResult<([f64; 3], [f64; 3])> {
+        let jd = jd_to_spice_jd(jd);
+        let times = self.get_times();
+        let window_size = self.poly_degree + 1;
+        let start_idx: isize = match times.binary_search_by(|probe| probe.total_cmp(&jd)) {
+            Ok(c) => c as isize - (window_size as isize) / 2,
+            Err(c) => {
+                if (jd - times[c - 1]).abs() < (jd - times[c]).abs() {
+                    c as isize - 1 - window_size as isize / 2
+                } else {
+                    c as isize - window_size as isize / 2
+                }
+            }
+        };
+        let start_idx = start_idx.clamp(0, (self.n_records - window_size) as isize) as usize;
+
+        let mut pos = [0.0; 3];
+        let mut vel = [0.0; 3];
+        for idx in 0..3 {
+            let mut p: Box<[f64]> = (0..window_size)
+                .map(|i| self.get_record(i + start_idx).pos[idx])
+                .collect();
+            let mut dp: Box<[f64]> = (0..window_size)
+                .map(|i| self.get_record(i + start_idx).vel[idx])
+                .collect();
+            let p = lagrange_interpolation(&times[start_idx..start_idx + window_size], &mut p, jd);
+            let v = lagrange_interpolation(&times[start_idx..start_idx + window_size], &mut dp, jd);
+            pos[idx] = p / AU_KM;
+            vel[idx] = v / AU_KM * 86400.;
+        }
+
+        Ok((pos, vel))
+    }
+}
+
 /// Space Command two-line elements
 ///
 /// <https://naif.jpl.nasa.gov/pub/naif/toolkit_docs/C/req/spk.html#Type%2010:%20Space%20Command%20Two-Line%20Elements>