Reference frame basics

swiftnav/Cargo.toml

@@ -13,3 +13,7 @@ rust-version = "1.62.1"
 rustversion = "1.0"
 chrono = { version = "0.4", optional = true }
 swiftnav-sys = { version = "^0.8.1", path = "../swiftnav-sys/" }
+strum = { version = "0.26", features = ["derive"] }
+
+[dev-dependencies]
+float_eq = "1.0.1"

swiftnav/src/coords.rs

@@ -47,6 +47,13 @@
 //!   * "Transformation from Cartesian to Geodetic Coordinates Accelerated by
 //!      Halley’s Method", T. Fukushima (2006), Journal of Geodesy.
 
+use std::ops::{Add, AddAssign, Mul, MulAssign, Sub, SubAssign};
+
+use crate::{
+    reference_frame::{get_transformation, ReferenceFrame, TransformationNotFound},
+    time::GpsTime,
+};
+
 /// WGS84 geodetic coordinates (Latitude, Longitude, Height)
 ///
 /// Internally stored as an array of 3 [f64](std::f64) values: latitude, longitude (both in the given angular units) and height above the geoid in meters
@@ -327,6 +334,104 @@ impl AsMut<[f64; 3]> for ECEF {
     }
 }
 
+impl Add for ECEF {
+    type Output = ECEF;
+    fn add(self, rhs: ECEF) -> ECEF {
+        ECEF([self.x() + rhs.x(), self.y() + rhs.y(), self.z() + rhs.z()])
+    }
+}
+
+impl Add<&ECEF> for ECEF {
+    type Output = ECEF;
+    fn add(self, rhs: &ECEF) -> ECEF {
+        self + *rhs
+    }
+}
+
+impl Add<&ECEF> for &ECEF {
+    type Output = ECEF;
+    fn add(self, rhs: &ECEF) -> ECEF {
+        *self + *rhs
+    }
+}
+
+impl AddAssign for ECEF {
+    fn add_assign(&mut self, rhs: ECEF) {
+        *self += &rhs;
+    }
+}
+
+impl AddAssign<&ECEF> for ECEF {
+    fn add_assign(&mut self, rhs: &ECEF) {
+        self.0[0] += rhs.x();
+        self.0[1] += rhs.y();
+        self.0[2] += rhs.z();
+    }
+}
+
+impl Sub for ECEF {
+    type Output = ECEF;
+    fn sub(self, rhs: ECEF) -> ECEF {
+        ECEF([self.x() - rhs.x(), self.y() - rhs.y(), self.z() - rhs.z()])
+    }
+}
+
+impl Sub<&ECEF> for ECEF {
+    type Output = ECEF;
+    fn sub(self, rhs: &ECEF) -> ECEF {
+        self - *rhs
+    }
+}
+
+impl Sub<&ECEF> for &ECEF {
+    type Output = ECEF;
+    fn sub(self, rhs: &ECEF) -> ECEF {
+        *self - *rhs
+    }
+}
+
+impl SubAssign for ECEF {
+    fn sub_assign(&mut self, rhs: ECEF) {
+        *self -= &rhs;
+    }
+}
+
+impl SubAssign<&ECEF> for ECEF {
+    fn sub_assign(&mut self, rhs: &ECEF) {
+        self.0[0] -= rhs.x();
+        self.0[1] -= rhs.y();
+        self.0[2] -= rhs.z();
+    }
+}
+
+impl Mul<ECEF> for f64 {
+    type Output = ECEF;
+    fn mul(self, rhs: ECEF) -> ECEF {
+        ECEF([self * rhs.x(), self * rhs.y(), self * rhs.z()])
+    }
+}
+
+impl Mul<&ECEF> for f64 {
+    type Output = ECEF;
+    fn mul(self, rhs: &ECEF) -> ECEF {
+        self * *rhs
+    }
+}
+
+impl MulAssign<f64> for ECEF {
+    fn mul_assign(&mut self, rhs: f64) {
+        *self *= &rhs;
+    }
+}
+
+impl MulAssign<&f64> for ECEF {
+    fn mul_assign(&mut self, rhs: &f64) {
+        self.0[0] *= *rhs;
+        self.0[1] *= *rhs;
+        self.0[2] *= *rhs;
+    }
+}
+
 /// Local North East Down reference frame coordinates
 ///
 /// Internally stored as an array of 3 [f64](std::f64) values: N, E, D all in meters
@@ -417,8 +522,102 @@ impl Default for AzimuthElevation {
     }
 }
 
+/// Complete coordinate used for transforming between reference frames
+///
+/// Velocities are optional, but when present they will be transformed
+#[derive(Debug, PartialEq, PartialOrd, Clone, Copy)]
+pub struct Coordinate {
+    reference_frame: ReferenceFrame,
+    position: ECEF,
+    velocity: Option<ECEF>,
+    epoch: GpsTime,
+}
+
+impl Coordinate {
+    pub fn new(
+        reference_frame: ReferenceFrame,
+        position: ECEF,
+        velocity: Option<ECEF>,
+        epoch: GpsTime,
+    ) -> Self {
+        Coordinate {
+            reference_frame,
+            position,
+            velocity,
+            epoch,
+        }
+    }
+
+    pub fn without_velocity(
+        reference_frame: ReferenceFrame,
+        position: ECEF,
+        epoch: GpsTime,
+    ) -> Self {
+        Coordinate {
+            reference_frame,
+            position,
+            velocity: None,
+            epoch,
+        }
+    }
+
+    pub fn with_velocity(
+        reference_frame: ReferenceFrame,
+        position: ECEF,
+        velocity: ECEF,
+        epoch: GpsTime,
+    ) -> Self {
+        Coordinate {
+            reference_frame,
+            position,
+            velocity: Some(velocity),
+            epoch,
+        }
+    }
+
+    pub fn reference_frame(&self) -> ReferenceFrame {
+        self.reference_frame
+    }
+
+    pub fn position(&self) -> ECEF {
+        self.position
+    }
+
+    pub fn velocity(&self) -> Option<ECEF> {
+        self.velocity
+    }
+
+    pub fn epoch(&self) -> GpsTime {
+        self.epoch
+    }
+
+    /// Use the velocity term to adjust the epoch of the coordinate.
+    /// When a coordinate has no velocity the position won't be changed.
+    pub fn adjust_epoch(&self, new_epoch: &GpsTime) -> Self {
+        let dt =
+            new_epoch.to_fractional_year_hardcoded() - self.epoch.to_fractional_year_hardcoded();
+        let v = self.velocity.unwrap_or_default();
+
+        Coordinate {
+            position: self.position + dt * v,
+            velocity: self.velocity,
+            epoch: *new_epoch,
+            reference_frame: self.reference_frame,
+        }
+    }
+
+    pub fn transform_to(&self, new_frame: ReferenceFrame) -> Result<Self, TransformationNotFound> {
+        let transformation = get_transformation(self.reference_frame, new_frame)?;
+        Ok(transformation.transform(self))
+    }
+}
+
 #[cfg(test)]
 mod tests {
+    use float_eq::assert_float_eq;
+
+    use crate::time::UtcTime;
+
     use super::*;
 
     const D2R: f64 = std::f64::consts::PI / 180.0;
@@ -516,4 +715,71 @@ mod tests {
             assert!(height_err.abs() < MAX_DIST_ERROR_M);
         }
     }
+
+    #[test]
+    fn ecef_ops() {
+        let a = ECEF::new(1.0, 2.0, 3.0);
+        let b = ECEF::new(4.0, 5.0, 6.0);
+
+        let result = a + b;
+        assert_eq!(5.0, result.x());
+        assert_eq!(7.0, result.y());
+        assert_eq!(9.0, result.z());
+
+        let result = a + a + a;
+        assert_eq!(3.0, result.x());
+        assert_eq!(6.0, result.y());
+        assert_eq!(9.0, result.z());
+
+        let result = a - b;
+        assert_eq!(-3.0, result.x());
+        assert_eq!(-3.0, result.y());
+        assert_eq!(-3.0, result.z());
+
+        let result = 2.0 * a;
+        assert_eq!(2.0, result.x());
+        assert_eq!(4.0, result.y());
+        assert_eq!(6.0, result.z());
+
+        let mut result = a;
+        result += b;
+        assert_eq!(5.0, result.x());
+        assert_eq!(7.0, result.y());
+        assert_eq!(9.0, result.z());
+
+        let mut result = a;
+        result -= b;
+        assert_eq!(-3.0, result.x());
+        assert_eq!(-3.0, result.y());
+        assert_eq!(-3.0, result.z());
+
+        let mut result = a;
+        result *= 2.0;
+        assert_eq!(2.0, result.x());
+        assert_eq!(4.0, result.y());
+        assert_eq!(6.0, result.z());
+    }
+
+    #[test]
+    fn coordinate_epoch() {
+        let initial_epoch = UtcTime::from_date(2020, 1, 1, 0, 0, 0.).to_gps_hardcoded();
+        let new_epoch = UtcTime::from_date(2021, 1, 1, 0, 0, 0.).to_gps_hardcoded();
+        let initial_coord = Coordinate::with_velocity(
+            ReferenceFrame::ITRF2020,
+            ECEF::new(0.0, 0.0, 0.0),
+            ECEF::new(1.0, 2.0, 3.0),
+            initial_epoch,
+        );
+
+        let new_coord = initial_coord.adjust_epoch(&new_epoch);
+
+        assert_eq!(initial_coord.reference_frame, new_coord.reference_frame);
+        assert_float_eq!(new_coord.position.x(), 1.0, abs <= 0.001);
+        assert_float_eq!(new_coord.position.y(), 2.0, abs <= 0.001);
+        assert_float_eq!(new_coord.position.z(), 3.0, abs <= 0.001);
+        assert_float_eq!(new_coord.velocity.unwrap().x(), 1.0, abs <= 0.001);
+        assert_float_eq!(new_coord.velocity.unwrap().y(), 2.0, abs <= 0.001);
+        assert_float_eq!(new_coord.velocity.unwrap().z(), 3.0, abs <= 0.001);
+        assert_eq!(new_epoch, new_coord.epoch());
+    }
 }

swiftnav/src/lib.rs

@@ -64,6 +64,7 @@ pub mod ephemeris;
 pub mod geoid;
 pub mod ionosphere;
 pub mod navmeas;
+pub mod reference_frame;
 pub mod signal;
 pub mod solver;
 pub mod time;