diff --git a/CHANGELOG.md b/CHANGELOG.md index 6567f70..9199107 100644 --- a/CHANGELOG.md +++ b/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. diff --git a/src/kete/shape.py b/src/kete/shape.py index d67bbed..dc9be25 100644 --- a/src/kete/shape.py +++ b/src/kete/shape.py @@ -168,7 +168,7 @@ def __init__( for j in range(0, 4 * i): points.append([np.pi - float(i) * dth, j * dphi]) points.append([np.pi, 0.0]) - points = np.degrees(points).tolist() + points = np.degrees(points).tolist() # type: ignore vecs = [] for point in points: diff --git a/src/kete_core/src/spice/interpolation.rs b/src/kete_core/src/spice/interpolation.rs index 416e4e4..54292a4 100644 --- a/src/kete_core/src/spice/interpolation.rs +++ b/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::::zeros(2 * x.len()); - let mut d_work = DVector::::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::::zeros(2 * y.len()); + let mut d_work = DVector::::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(×, &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(×, &mut y.clone(), x); + assert!( + (interp - expected).abs() < 1e-10, + "x={} interp={} expected={} diff={}", + x, + interp, + expected, + interp - expected + ); + } + } +} diff --git a/src/kete_core/src/spice/spk_segments.rs b/src/kete_core/src/spice/spk_segments.rs index e642421..7579935 100644 --- a/src/kete_core/src/spice/spk_segments.rs +++ b/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 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 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. +/// +#[derive(Debug)] +pub struct SpkSegmentType9 { + array: DafArray, + poly_degree: usize, + n_records: usize, +} + +impl From 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(×[start_idx..start_idx + window_size], &mut p, jd); + let v = lagrange_interpolation(×[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 /// ///