Implement position retrieval

crates/lox_core/src/ephemeris/daf_spk.rs

@@ -1 +1,2 @@
+pub mod api;
 pub mod parser;

crates/lox_core/src/ephemeris/daf_spk/api.rs

@@ -0,0 +1,124 @@
+use super::parser::{DafSpkError, Spk, SpkSegment, SpkType2Array, SpkType2Record};
+
+type Position = (f64, f64, f64);
+type Epoch = f64;
+type Body = i32;
+
+impl Spk {
+    fn find_segment(&self, origin: Body, target: Body) -> Result<(&SpkSegment, i32), DafSpkError> {
+        let mut sign = 1;
+
+        let mut target = target;
+        let mut origin = origin;
+        if target < origin {
+            (origin, target) = (target, origin);
+            sign = -1;
+        }
+
+        let segment = self
+            .segments
+            .get(&origin)
+            .ok_or(DafSpkError::UnableToFindMatchingSegment)?
+            .get(&target)
+            .ok_or(DafSpkError::UnableToFindMatchingSegment)?;
+
+        Ok((&segment, sign))
+    }
+
+    fn find_record<'a>(
+        &'a self,
+        array: &'a SpkType2Array,
+        initial_epoch: Epoch,
+        epoch: Epoch,
+    ) -> Result<(&SpkType2Record, f64), DafSpkError> {
+        let seconds_from_record_start = epoch - initial_epoch;
+
+        let intlen = array.intlen as f64;
+        let mut record_number = (seconds_from_record_start / intlen).floor() as usize;
+        let mut fraction = seconds_from_record_start % intlen;
+
+        // @TODO clarify this
+        if record_number == array.n as usize {
+            record_number -= 1;
+            fraction = array.intlen as f64;
+        }
+
+        let record = array
+            .records
+            .get(record_number)
+            .ok_or(DafSpkError::UnableToFindMatchingRecord)?;
+
+        Ok((record, fraction))
+    }
+
+    pub fn position(
+        &self,
+        epoch: Epoch,
+        origin: Body,
+        target: Body,
+    ) -> Result<Position, DafSpkError> {
+        let (segment, sign) = self.find_segment(origin, target)?;
+        let sign = sign as f64;
+
+        if epoch < segment.initial_epoch || epoch > segment.final_epoch {
+            return Err(DafSpkError::UnableToFindMatchingSegment);
+        }
+
+        // @TODO use a more flexible way
+        let mut x = 0f64;
+        let mut y = 0f64;
+        let mut z = 0f64;
+
+        match &segment.data {
+            super::parser::SpkArray::Type2(array) => {
+                let (record, fraction) = self.find_record(array, segment.initial_epoch, epoch)?;
+
+                let degree_of_polynomial = array.degree_of_polynomial() as usize;
+                // @TODO figure name
+                let mut figure_out_name_for_this_var =
+                    Vec::<f64>::with_capacity(degree_of_polynomial);
+
+                figure_out_name_for_this_var.push(1f64);
+                figure_out_name_for_this_var.push(2f64 * fraction / array.intlen as f64 - 1f64);
+
+                for i in 2..degree_of_polynomial {
+                    figure_out_name_for_this_var.push(
+                        2f64 * figure_out_name_for_this_var[1]
+                            * figure_out_name_for_this_var[i - 1]
+                            - figure_out_name_for_this_var[i - 2],
+                    );
+                }
+
+                for i in 1..degree_of_polynomial {
+                    x += sign * record.x[i] * figure_out_name_for_this_var[i];
+                    y += sign * record.y[i] * figure_out_name_for_this_var[i];
+                    z += sign * record.z[i] * figure_out_name_for_this_var[i];
+                }
+            }
+        }
+
+        Ok((x, y, z))
+    }
+}
+
+#[cfg(test)]
+mod test {
+    use crate::ephemeris::daf_spk::parser::parse_daf_spk;
+    use crate::ephemeris::daf_spk::parser::test::FILE_CONTENTS;
+
+    use super::*;
+
+    #[test]
+    fn test_position() {
+        let spk = parse_daf_spk(&FILE_CONTENTS).expect("Unable to parse DAF/SPK");
+
+        assert_eq!(
+            Err(DafSpkError::UnableToFindMatchingSegment),
+            spk.position(2457388.5000000 as Epoch, 1, 2)
+        );
+        assert_eq!(
+            Ok((12634339.035429325, 12000602.073373662, 5074109.0604534615)),
+            spk.position(-14200747200.0 as Epoch, 0, 1)
+        ); //@TODO needs validation
+    }
+}