Merge pull request #316 from nyx-space/gh-315-differencing

Add state differencing functionality + VNC and RCN frames include time derivative of the rotation matrix

anise/Cargo.toml

@@ -45,8 +45,8 @@ regex = { version = "1.10.5", optional = true }
 
 [dev-dependencies]
 rust-spice = "0.7.6"
-parquet = "52.0.0"
-arrow = "52.0.0"
+parquet = "53.0.0"
+arrow = "53.0.0"
 criterion = "0.5"
 iai = "0.1"
 polars = { version = "0.42.0", features = ["lazy", "parquet"] }

anise/src/almanac/aer.rs

@@ -15,6 +15,7 @@ use crate::{
     frames::Frame,
     math::angles::{between_0_360, between_pm_180},
     prelude::Orbit,
+    time::uuid_from_epoch,
 };
 
 use super::Almanac;
@@ -70,7 +71,7 @@ impl Almanac {
         }
 
         // Compute the SEZ DCM
-        let from = tx.frame.orientation_id * 1_000 + 1;
+        let from = uuid_from_epoch(tx.frame.orientation_id, rx.epoch);
         // SEZ DCM is topo to fixed
         let sez_dcm = tx
             .dcm_from_topocentric_to_body_fixed(from)

anise/src/astro/orbit.rs

@@ -425,7 +425,7 @@ impl Orbit {
     /// 2. Compute the cross product of these
     /// 3. Build the DCM with these unit vectors
     /// 4. Return the DCM structure
-    pub fn dcm_from_rcn_to_inertial(&self) -> PhysicsResult<DCM> {
+    pub fn dcm3x3_from_rcn_to_inertial(&self) -> PhysicsResult<DCM> {
         let r = self.r_hat();
         let n = self.hvec()? / self.hmag()?;
         let c = n.cross(&r);
@@ -440,6 +440,41 @@ impl Orbit {
         })
     }
 
+    /// Builds the rotation matrix that rotates from this state's inertial frame to this state's RCN frame (radial, cross, normal)
+    ///
+    /// # Frame warning
+    /// If the stattion is NOT in an inertial frame, then this computation is INVALID.
+    ///
+    /// # Algorithm
+    /// 1. Compute \hat{r}, \hat{h}, the unit vectors of the radius and orbital momentum.
+    /// 2. Compute the cross product of these
+    /// 3. Build the DCM with these unit vectors
+    /// 4. Return the DCM structure with a 6x6 DCM with the time derivative of the VNC frame set.
+    ///
+    /// # Note on the time derivative
+    /// If the pre or post states cannot be computed, then the time derivative of the DCM will _not_ be set.
+    /// Further note that most astrodynamics tools do *not* account for the time derivative in the RIC frame.
+    pub fn dcm_from_rcn_to_inertial(&self) -> PhysicsResult<DCM> {
+        let rot_mat_dt = if let Ok(pre) = self.at_epoch(self.epoch - Unit::Millisecond * 1) {
+            if let Ok(post) = self.at_epoch(self.epoch + Unit::Millisecond * 1) {
+                let dcm_pre = pre.dcm3x3_from_rcn_to_inertial()?;
+                let dcm_post = post.dcm3x3_from_rcn_to_inertial()?;
+                Some(0.5 * dcm_post.rot_mat - 0.5 * dcm_pre.rot_mat)
+            } else {
+                None
+            }
+        } else {
+            None
+        };
+
+        Ok(DCM {
+            rot_mat: self.dcm3x3_from_rcn_to_inertial()?.rot_mat,
+            rot_mat_dt,
+            from: uuid_from_epoch(self.frame.orientation_id, self.epoch),
+            to: self.frame.orientation_id,
+        })
+    }
+
     /// Builds the rotation matrix that rotates from this state's inertial frame to this state's VNC frame (velocity, normal, cross)
     ///
     /// # Frame warning
@@ -449,8 +484,9 @@ impl Orbit {
     /// 1. Compute \hat{v}, \hat{h}, the unit vectors of the radius and orbital momentum.
     /// 2. Compute the cross product of these
     /// 3. Build the DCM with these unit vectors
-    /// 4. Return the DCM structure
-    pub fn dcm_from_vnc_to_inertial(&self) -> PhysicsResult<DCM> {
+    /// 4. Return the DCM structure.
+    ///
+    pub fn dcm3x3_from_vnc_to_inertial(&self) -> PhysicsResult<DCM> {
         let v = self.velocity_km_s / self.vmag_km_s();
         let n = self.hvec()? / self.hmag()?;
         let c = v.cross(&n);
@@ -464,6 +500,42 @@ impl Orbit {
             to: self.frame.orientation_id,
         })
     }
+
+    /// Builds the rotation matrix that rotates from this state's inertial frame to this state's VNC frame (velocity, normal, cross)
+    ///
+    /// # Frame warning
+    /// If the stattion is NOT in an inertial frame, then this computation is INVALID.
+    ///
+    /// # Algorithm
+    /// 1. Compute \hat{v}, \hat{h}, the unit vectors of the radius and orbital momentum.
+    /// 2. Compute the cross product of these
+    /// 3. Build the DCM with these unit vectors
+    /// 4. Compute the difference between the DCMs of the pre and post states (+/- 1 ms), to build the DCM time derivative
+    /// 4. Return the DCM structure with a 6x6 DCM with the time derivative of the VNC frame set.
+    ///
+    /// # Note on the time derivative
+    /// If the pre or post states cannot be computed, then the time derivative of the DCM will _not_ be set.
+    /// Further note that most astrodynamics tools do *not* account for the time derivative in the RIC frame.
+    pub fn dcm_from_vnc_to_inertial(&self) -> PhysicsResult<DCM> {
+        let rot_mat_dt = if let Ok(pre) = self.at_epoch(self.epoch - Unit::Millisecond * 1) {
+            if let Ok(post) = self.at_epoch(self.epoch + Unit::Millisecond * 1) {
+                let dcm_pre = pre.dcm3x3_from_vnc_to_inertial()?;
+                let dcm_post = post.dcm3x3_from_vnc_to_inertial()?;
+                Some(0.5 * dcm_post.rot_mat - 0.5 * dcm_pre.rot_mat)
+            } else {
+                None
+            }
+        } else {
+            None
+        };
+
+        Ok(DCM {
+            rot_mat: self.dcm3x3_from_vnc_to_inertial()?.rot_mat,
+            rot_mat_dt,
+            from: uuid_from_epoch(self.frame.orientation_id, self.epoch),
+            to: self.frame.orientation_id,
+        })
+    }
 }
 
 #[allow(clippy::too_many_arguments)]
@@ -1110,6 +1182,23 @@ impl Orbit {
         rslt.frame.strip();
         Ok(rslt)
     }
+
+    /// Returns a Cartesian state representing the VNC difference between self and other, in position and velocity (with transport theorem).
+    /// Refer to dcm_from_vnc_to_inertial for details on the VNC frame.
+    ///
+    /// # Algorithm
+    /// 1. Compute the VNC DCM of self
+    /// 2. Rotate self into the VNC frame
+    /// 3. Rotation other into the VNC frame
+    /// 4. Compute the difference between these two states
+    /// 5. Strip the astrodynamical information from the frame, enabling only computations from `CartesianState`
+    pub fn vnc_difference(&self, other: &Self) -> PhysicsResult<Self> {
+        let self_in_vnc = (self.dcm_from_vnc_to_inertial()?.transpose() * self)?;
+        let other_in_vnc = (self.dcm_from_vnc_to_inertial()?.transpose() * other)?;
+        let mut rslt = (self_in_vnc - other_in_vnc)?;
+        rslt.frame.strip();
+        Ok(rslt)
+    }
 }
 
 #[allow(clippy::format_in_format_args)]

anise/src/math/cartesian.rs

@@ -8,11 +8,11 @@
  * Documentation: https://nyxspace.com/
  */
 
-use super::{perp_vector, root_mean_squared, Vector3};
+use super::{perp_vector, root_mean_squared, root_sum_squared, Vector3};
 use crate::{
     astro::PhysicsResult,
     constants::SPEED_OF_LIGHT_KM_S,
-    errors::{EpochMismatchSnafu, FrameMismatchSnafu, PhysicsError},
+    errors::{EpochMismatchSnafu, FrameMismatchSnafu, MathError, PhysicsError},
     prelude::Frame,
 };
 
@@ -253,11 +253,39 @@ impl CartesianState {
                 frame2: other.frame
             }
         );
-        Ok(root_mean_squared(&self.radius_km, &other.radius_km))
+        Ok(root_sum_squared(&self.radius_km, &other.radius_km))
     }
 
     /// Returns the root mean squared (RSS) velocity difference between this state and another state, if both frames match (epoch does not need to match)
     pub fn rss_velocity_km_s(&self, other: &Self) -> PhysicsResult<f64> {
+        ensure!(
+            self.frame.ephem_origin_match(other.frame)
+                && self.frame.orient_origin_match(other.frame),
+            FrameMismatchSnafu {
+                action: "computing velocity RSS",
+                frame1: self.frame,
+                frame2: other.frame
+            }
+        );
+        Ok(root_sum_squared(&self.velocity_km_s, &other.velocity_km_s))
+    }
+
+    /// Returns the root sum squared (RMS) radius difference between this state and another state, if both frames match (epoch does not need to match)
+    pub fn rms_radius_km(&self, other: &Self) -> PhysicsResult<f64> {
+        ensure!(
+            self.frame.ephem_origin_match(other.frame)
+                && self.frame.orient_origin_match(other.frame),
+            FrameMismatchSnafu {
+                action: "computing radius RSS",
+                frame1: self.frame,
+                frame2: other.frame
+            }
+        );
+        Ok(root_mean_squared(&self.radius_km, &other.radius_km))
+    }
+
+    /// Returns the root sum squared (RMS) velocity difference between this state and another state, if both frames match (epoch does not need to match)
+    pub fn rms_velocity_km_s(&self, other: &Self) -> PhysicsResult<f64> {
         ensure!(
             self.frame.ephem_origin_match(other.frame)
                 && self.frame.orient_origin_match(other.frame),
@@ -287,6 +315,81 @@ impl CartesianState {
     pub fn light_time(&self) -> Duration {
         (self.radius_km.norm() / SPEED_OF_LIGHT_KM_S).seconds()
     }
+
+    /// Returns the absolute position difference in kilometer between this orbit and another.
+    /// Raises an error if the frames do not match (epochs do not need to match).
+    pub fn abs_pos_diff_km(&self, other: &Self) -> PhysicsResult<f64> {
+        ensure!(
+            self.frame.ephem_origin_match(other.frame)
+                && self.frame.orient_origin_match(other.frame),
+            FrameMismatchSnafu {
+                action: "computing velocity RSS",
+                frame1: self.frame,
+                frame2: other.frame
+            }
+        );
+
+        Ok((self.radius_km - other.radius_km).norm())
+    }
+
+    /// Returns the absolute velocity difference in kilometer per second between this orbit and another.
+    /// Raises an error if the frames do not match (epochs do not need to match).
+    pub fn abs_vel_diff_km_s(&self, other: &Self) -> PhysicsResult<f64> {
+        ensure!(
+            self.frame.ephem_origin_match(other.frame)
+                && self.frame.orient_origin_match(other.frame),
+            FrameMismatchSnafu {
+                action: "computing velocity RSS",
+                frame1: self.frame,
+                frame2: other.frame
+            }
+        );
+
+        Ok((self.velocity_km_s - other.velocity_km_s).norm())
+    }
+
+    /// Returns the absolute position and velocity differences in km and km/s between this orbit and another.
+    /// Raises an error if the frames do not match (epochs do not need to match).
+    pub fn abs_difference(&self, other: &Self) -> PhysicsResult<(f64, f64)> {
+        Ok((self.abs_pos_diff_km(other)?, self.abs_vel_diff_km_s(other)?))
+    }
+
+    /// Returns the relative position difference (unitless) between this orbit and another.
+    /// This is computed by dividing the absolute difference by the norm of this object's radius vector.
+    /// If the radius is zero, this function raises a math error.
+    /// Raises an error if the frames do not match or  (epochs do not need to match).
+    pub fn rel_pos_diff(&self, other: &Self) -> PhysicsResult<f64> {
+        if self.rmag_km() <= f64::EPSILON {
+            return Err(PhysicsError::AppliedMath {
+                source: MathError::DivisionByZero {
+                    action: "computing relative position difference",
+                },
+            });
+        }
+
+        Ok(self.abs_pos_diff_km(other)? / self.rmag_km())
+    }
+
+    /// Returns the absolute velocity difference in kilometer per second between this orbit and another.
+    /// Raises an error if the frames do not match (epochs do not need to match).
+    pub fn rel_vel_diff(&self, other: &Self) -> PhysicsResult<f64> {
+        if self.vmag_km_s() <= f64::EPSILON {
+            return Err(PhysicsError::AppliedMath {
+                source: MathError::DivisionByZero {
+                    action: "computing relative velocity difference",
+                },
+            });
+        }
+
+        Ok(self.abs_vel_diff_km_s(other)? / self.vmag_km_s())
+    }
+
+    /// Returns the relative difference between this orbit and another for the position and velocity, respectively the first and second return values.
+    /// Both return values are UNITLESS because the relative difference is computed as the absolute difference divided by the rmag and vmag of this object.
+    /// Raises an error if the frames do not match, if the position is zero or the velocity is zero.
+    pub fn rel_difference(&self, other: &Self) -> PhysicsResult<(f64, f64)> {
+        Ok((self.rel_pos_diff(other)?, self.rel_vel_diff(other)?))
+    }
 }
 
 impl Add for CartesianState {

anise/src/math/mod.rs

@@ -26,8 +26,8 @@ pub mod units;
 use nalgebra::allocator::Allocator;
 use nalgebra::{DefaultAllocator, DimName, OVector};
 
-/// Returns the root mean squared (RSS) between two vectors of any dimension N.
-pub fn root_mean_squared<N: DimName>(vec_a: &OVector<f64, N>, vec_b: &OVector<f64, N>) -> f64
+/// Returns the root sum squared (RSS) between two vectors of any dimension N.
+pub fn root_sum_squared<N: DimName>(vec_a: &OVector<f64, N>, vec_b: &OVector<f64, N>) -> f64
 where
     DefaultAllocator: Allocator<N>,
 {
@@ -39,6 +39,21 @@ where
         .sqrt()
 }
 
+/// Returns the root mean squared (RSS) between two vectors of any dimension N.
+pub fn root_mean_squared<N: DimName>(vec_a: &OVector<f64, N>, vec_b: &OVector<f64, N>) -> f64
+where
+    DefaultAllocator: Allocator<N>,
+{
+    let sum_of_squares = vec_a
+        .iter()
+        .zip(vec_b.iter())
+        .map(|(&x, &y)| (x - y).powi(2))
+        .sum::<f64>();
+
+    let mean_of_squares = sum_of_squares / vec_a.len() as f64;
+    mean_of_squares.sqrt()
+}
+
 /// Returns the projection of a onto b
 /// Converted from NAIF SPICE's `projv`
 pub fn project_vector(a: &Vector3, b: &Vector3) -> Vector3 {