diff --git a/swiftnav/Cargo.toml b/swiftnav/Cargo.toml index cc7b248..ff1b90d 100644 --- a/swiftnav/Cargo.toml +++ b/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" diff --git a/swiftnav/src/coords.rs b/swiftnav/src/coords.rs index d3c11ec..c37e403 100644 --- a/swiftnav/src/coords.rs +++ b/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 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 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, + epoch: GpsTime, +} + +impl Coordinate { + pub fn new( + reference_frame: ReferenceFrame, + position: ECEF, + velocity: Option, + 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 { + 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 { + 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()); + } } diff --git a/swiftnav/src/lib.rs b/swiftnav/src/lib.rs index 6954276..97e53b6 100644 --- a/swiftnav/src/lib.rs +++ b/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; diff --git a/swiftnav/src/reference_frame.rs b/swiftnav/src/reference_frame.rs new file mode 100644 index 0000000..bdb18e8 --- /dev/null +++ b/swiftnav/src/reference_frame.rs @@ -0,0 +1,748 @@ +// Copyright (c) 2024 Swift Navigation Inc. +// Contact: Swift Navigation +// +// This source is subject to the license found in the file 'LICENSE' which must +// be be distributed together with this source. All other rights reserved. +// +// THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, +// EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. +//! Geodetic reference frame transformations +//! +//! Geodetic reference frames define the coordinate system used to represent +//! positions on the Earth. Different reference frames are commonly used in +//! different regions of the world, and for different purposes. For example, +//! global reference frames, such as the International Terrestrial Reference +//! Frame (ITRF), are used for global positioning, while regional reference +//! frames, such as the European Terrestrial Reference Frame (ETRF), are used +//! for regional positioning. Due to the movement of the earth's crust apparently +//! fixed positions will move over time. Because of this it's important to note +//! only take note of a position, but also the time at which that position was +//! determined. In most regions of the earth the crust moves at a constant speed, +//! meaning that if you are able to determine the local velocity of the crust you +//! can easily determine what the position of a static point would have been in +//! the past. It is commong for regional reference frames to define a common reference +//! epoch that all positions should be transformed to, allowing the direct comparison +//! of positions even if they were determined at different times. Regional reference +//! frames also typically are defined to be "fixed" to a particular tectonic plate, +//! meaning the large majority of the velocity for points on that tectonic plate +//! are cancelled out. In contrast, global reference frames are not fixed to +//! any particular tectonic plate, so most places on earth will have a measurable +//! velocity. Global reference frames also typically do not have a common reference +//! epoch, so determining one's local velocity is important to be able to compare +//! positions or to transform a coordinate from a global reference frame to a regional +//! reference frame. +//! +//! This module provides several types and functions to help transform a set of coordinates +//! from one reference frame to another, and from one epoch to another. Several sets of +//! transformation parameters are included for converting between common reference frames. +//! To start out, you must have a [`Coordinate`](crate::coords::Coordinate) that you want to +//! transform. This consists of a position, an epoch, and a reference frame as well as an optional +//! velocity. You then need to get the [`Transformation`](crate::reference_frame::Transformation) +//! object that describes the transformation from the reference frame of the coordinate to the +//! desired reference frame. You can then call the `transform` method on the transformation object +//! to get a new coordinate in the desired reference frame. This transformation will change the +//! position and velocity of the coordinate, but it does not the change the epoch of the coordinate. +//! If you need to change the epoch of the coordinate you will need to use the [`Coordinate::adjust_epoch`](crate::coords::Coordinate::adjust_epoch) +//! method which uses the velocity of the coordinate to determine the position at the new epoch. +//! +//! # Example +//! ``` +//! use swiftnav::{ +//! coords::{Coordinate, ECEF}, +//! reference_frame::{get_transformation, ReferenceFrame, TransformationNotFound}, +//! time::UtcTime +//! }; +//! +//! let transformation = get_transformation(ReferenceFrame::ITRF2014, ReferenceFrame::NAD83_2011) +//! .unwrap(); +//! +//! let epoch_2020 = UtcTime::from_date(2020, 3, 15, 0, 0, 0.).to_gps_hardcoded(); +//! let itrf_coord = Coordinate::with_velocity( +//! ReferenceFrame::ITRF2014, // The reference frame of the coordinate +//! ECEF::new(-2703764.0, -4261273.0, 3887158.0), // The position of the coordinate +//! ECEF::new(-0.221, 0.254, 0.122), // The velocity of the coordinate +//! epoch_2020); // The epoch of the coordinate +//! +//! let epoch_2010 = UtcTime::from_date(2010, 1, 1, 0, 0, 0.).to_gps_hardcoded(); +//! let itrf_coord = itrf_coord.adjust_epoch(&epoch_2010); // Change the epoch of the coordinate +//! +//! let nad83_coord = transformation.transform(&itrf_coord); +//! // Alternatively, you can use the `transform_to` method on the coordinate itself +//! let nad83_coord: Result = +//! itrf_coord.transform_to(ReferenceFrame::NAD83_2011); +//! ``` +//! + +use crate::coords::{Coordinate, ECEF}; +use std::fmt; +use strum::{Display, EnumIter, EnumString}; + +/// Reference Frames +#[derive(Debug, PartialEq, Eq, PartialOrd, Ord, Clone, Copy, EnumString, Display, EnumIter)] +#[strum(serialize_all = "UPPERCASE")] +pub enum ReferenceFrame { + ITRF2008, + ITRF2014, + ITRF2020, + ETRF2008, + ETRF2014, + ETRF2020, + /// i.e. NAD83(2011) + #[strum(to_string = "NAD83(2011)", serialize = "NAD83_2011")] + NAD83_2011, + /// i.e. NAD83(CSRS) - Canadian Spatial Reference System + #[allow(non_camel_case_types)] + #[strum(to_string = "NAD83(CSRS)", serialize = "NAD83_CSRS")] + NAD83_CSRS, +} + +/// 15-parameter Helmert transformation parameters +/// +/// This transformation consists of a 3 dimensional translation, +/// 3 dimensional rotation, and a universal scaling. All terms, +/// except for the reference epoch, have a an additional time +/// dependent term. The rotations are typically very small, so +/// the small angle approximation is used. +/// +/// There are several sign and scale conventions in use with +/// Helmert transformations. In this implementation we follow +/// the IERS conventions, meaning the translations are in +/// millimeters, the rotations are in milliarcseconds, and +/// the scaling is in parts per billion. We also follow the +/// IERS convention for the sign of the rotation terms. +#[derive(Debug, PartialEq, PartialOrd, Clone, Copy)] +struct TimeDependentHelmertParams { + tx: f64, + tx_dot: f64, + ty: f64, + ty_dot: f64, + tz: f64, + tz_dot: f64, + s: f64, + s_dot: f64, + rx: f64, + rx_dot: f64, + ry: f64, + ry_dot: f64, + rz: f64, + rz_dot: f64, + epoch: f64, +} + +impl TimeDependentHelmertParams { + const TRANSLATE_SCALE: f64 = 1.0e-3; + const SCALE_SCALE: f64 = 1.0e-9; + const ROTATE_SCALE: f64 = (std::f64::consts::PI / 180.0) * (0.001 / 3600.0); + + /// Reverses the transformation. Since this is a linear transformation we simply negate all terms + fn invert(&mut self) { + self.tx *= -1.0; + self.tx_dot *= -1.0; + self.ty *= -1.0; + self.ty_dot *= -1.0; + self.tz *= -1.0; + self.tz_dot *= -1.0; + self.s *= -1.0; + self.s_dot *= -1.0; + self.rx *= -1.0; + self.rx_dot *= -1.0; + self.ry *= -1.0; + self.ry_dot *= -1.0; + self.rz *= -1.0; + self.rz_dot *= -1.0; + } + + /// Apply the transformation on a position at a specific epoch + fn transform_position(&self, position: &ECEF, epoch: f64) -> ECEF { + let dt = epoch - self.epoch; + let tx = (self.tx + self.tx_dot * dt) * Self::TRANSLATE_SCALE; + let ty = (self.ty + self.ty_dot * dt) * Self::TRANSLATE_SCALE; + let tz = (self.tz + self.tz_dot * dt) * Self::TRANSLATE_SCALE; + let s = (self.s + self.s_dot * dt) * Self::SCALE_SCALE; + let rx = (self.rx + self.rx_dot * dt) * Self::ROTATE_SCALE; + let ry = (self.ry + self.ry_dot * dt) * Self::ROTATE_SCALE; + let rz = (self.rz + self.rz_dot * dt) * Self::ROTATE_SCALE; + + let x = position.x() + tx + (s * position.x()) + (-rz * position.y()) + (ry * position.z()); + let y = position.y() + ty + (rz * position.x()) + (s * position.y()) + (-rx * position.z()); + let z = position.z() + tz + (-ry * position.x()) + (rx * position.y()) + (s * position.z()); + + ECEF::new(x, y, z) + } + + /// Apply the transformation on a velocity at a specific position + fn transform_velocity(&self, velocity: &ECEF, position: &ECEF) -> ECEF { + let tx = self.tx_dot * Self::TRANSLATE_SCALE; + let ty = self.ty_dot * Self::TRANSLATE_SCALE; + let tz = self.tz_dot * Self::TRANSLATE_SCALE; + let s = self.s_dot * Self::SCALE_SCALE; + let rx = self.rx_dot * Self::ROTATE_SCALE; + let ry = self.ry_dot * Self::ROTATE_SCALE; + let rz = self.rz_dot * Self::ROTATE_SCALE; + + let x = velocity.x() + tx + (s * position.x()) + (-rz * position.y()) + (ry * position.z()); + let y = velocity.y() + ty + (rz * position.x()) + (s * position.y()) + (-rx * position.z()); + let z = velocity.z() + tz + (-ry * position.x()) + (rx * position.y()) + (s * position.z()); + + ECEF::new(x, y, z) + } +} + +/// A transformation from one reference frame to another. +#[derive(Debug, PartialEq, PartialOrd, Clone, Copy)] +pub struct Transformation { + from: ReferenceFrame, + to: ReferenceFrame, + params: TimeDependentHelmertParams, +} + +impl Transformation { + /// Transform the given coordinate, producing a new coordinate. + /// + /// Reference frame transformations do not change the epoch of the + /// coordinate. + pub fn transform(&self, coord: &Coordinate) -> Coordinate { + assert!( + coord.reference_frame() == self.from, + "Coordinate reference frame does not match transformation from reference frame" + ); + + let new_position = self.params.transform_position( + &coord.position(), + coord.epoch().to_fractional_year_hardcoded(), + ); + let new_velocity = coord + .velocity() + .as_ref() + .map(|velocity| self.params.transform_velocity(velocity, &coord.position())); + Coordinate::new(self.to, new_position, new_velocity, coord.epoch()) + } + + /// Reverse the transformation + pub fn invert(mut self) -> Self { + std::mem::swap(&mut self.from, &mut self.to); + self.params.invert(); + self + } +} + +/// Error indicating that no transformation was found between two reference frames +/// +/// This error is returned when trying to find a transformation between two reference frames +/// and no transformation is found. +#[derive(Debug, PartialEq, Eq, PartialOrd, Ord, Clone, Copy)] +pub struct TransformationNotFound(ReferenceFrame, ReferenceFrame); + +impl fmt::Display for TransformationNotFound { + fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { + write!(f, "No transformation found from {} to {}", self.0, self.1) + } +} + +impl std::error::Error for TransformationNotFound {} + +/// Find a transformation from one reference frame to another +/// +/// We currently only support a limited set of transformations. +/// If no transformation is found, `None` is returned. +pub fn get_transformation( + from: ReferenceFrame, + to: ReferenceFrame, +) -> Result { + TRANSFORMATIONS + .iter() + .find(|t| (t.from == from && t.to == to) || (t.from == to && t.to == from)) + .map(|t| { + if t.from == from && t.to == to { + *t + } else { + (*t).invert() + } + }) + .ok_or(TransformationNotFound(from, to)) +} + +const TRANSFORMATIONS: [Transformation; 9] = [ + Transformation { + from: ReferenceFrame::ITRF2020, + to: ReferenceFrame::ITRF2014, + params: TimeDependentHelmertParams { + tx: -1.4, + tx_dot: 0.0, + ty: -0.9, + ty_dot: -0.1, + tz: 1.4, + tz_dot: 0.2, + s: -0.42, + s_dot: 0.0, + rx: 0.0, + rx_dot: 0.0, + ry: 0.0, + ry_dot: 0.0, + rz: 0.0, + rz_dot: 0.0, + epoch: 2015.0, + }, + }, + Transformation { + from: ReferenceFrame::ITRF2020, + to: ReferenceFrame::ITRF2008, + params: TimeDependentHelmertParams { + tx: 0.2, + tx_dot: 0.0, + ty: 1.0, + ty_dot: -0.1, + tz: 3.3, + tz_dot: 0.1, + s: -0.29, + s_dot: 0.03, + rx: 0.0, + rx_dot: 0.0, + ry: 0.0, + ry_dot: 0.0, + rz: 0.0, + rz_dot: 0.0, + epoch: 2015.0, + }, + }, + Transformation { + from: ReferenceFrame::ITRF2020, + to: ReferenceFrame::ETRF2020, + params: TimeDependentHelmertParams { + tx: 0.0, + tx_dot: 0.0, + ty: 0.0, + ty_dot: 0.0, + tz: 0.0, + tz_dot: 0.0, + s: 0.0, + s_dot: 0.0, + rx: 0.0, + rx_dot: 0.086, + ry: 0.0, + ry_dot: 0.519, + rz: 0.0, + rz_dot: -0.753, + epoch: 1989.0, + }, + }, + Transformation { + from: ReferenceFrame::ITRF2020, + to: ReferenceFrame::ETRF2014, + params: TimeDependentHelmertParams { + tx: -1.4, + tx_dot: 0.0, + ty: -0.9, + ty_dot: -0.1, + tz: 1.4, + tz_dot: 0.2, + s: -0.42, + s_dot: 0.0, + rx: 2.21, + rx_dot: 0.085, + ry: 13.806, + ry_dot: 0.531, + rz: -20.02, + rz_dot: -0.77, + epoch: 2015.0, + }, + }, + Transformation { + from: ReferenceFrame::ITRF2014, + to: ReferenceFrame::NAD83_2011, + params: TimeDependentHelmertParams { + tx: 1005.30, + tx_dot: 0.79, + ty: -1909.21, + ty_dot: -0.60, + tz: -541.57, + tz_dot: -1.44, + s: 0.36891, + s_dot: -0.07201, + rx: -26.78138, + rx_dot: -0.06667, + ry: 0.42027, + ry_dot: 0.75744, + rz: -10.93206, + rz_dot: 0.05133, + epoch: 2010.0, + }, + }, + Transformation { + from: ReferenceFrame::ITRF2014, + to: ReferenceFrame::ETRF2014, + params: TimeDependentHelmertParams { + tx: 0.0, + tx_dot: 0.0, + ty: 0.0, + ty_dot: 0.0, + tz: 0.0, + tz_dot: 0.0, + s: 0.0, + s_dot: 0.0, + rx: 0.0, + rx_dot: 0.085, + ry: 0.0, + ry_dot: 0.531, + rz: 0.0, + rz_dot: -0.770, + epoch: 1989.0, + }, + }, + Transformation { + from: ReferenceFrame::ITRF2008, + to: ReferenceFrame::NAD83_CSRS, + params: TimeDependentHelmertParams { + tx: 1003.70, + tx_dot: 0.79, + ty: -1911.11, + ty_dot: -0.60, + tz: -543.97, + tz_dot: -1.34, + s: 0.38891, + s_dot: -0.10201, + rx: -26.78138, + rx_dot: -0.06667, + ry: 0.42027, + ry_dot: 0.75744, + rz: -10.93206, + rz_dot: 0.05133, + epoch: 2010.0, + }, + }, + Transformation { + from: ReferenceFrame::ITRF2014, + to: ReferenceFrame::NAD83_CSRS, + params: TimeDependentHelmertParams { + tx: 1005.30, + tx_dot: 0.79, + ty: -1909.21, + ty_dot: -0.60, + tz: -541.57, + tz_dot: -1.44, + s: 0.36891, + s_dot: -0.07201, + rx: -26.78138, + rx_dot: -0.06667, + ry: 0.42027, + ry_dot: 0.75744, + rz: -10.93206, + rz_dot: 0.05133, + epoch: 2010.0, + }, + }, + Transformation { + from: ReferenceFrame::ITRF2020, + to: ReferenceFrame::NAD83_CSRS, + params: TimeDependentHelmertParams { + tx: 1003.90, + tx_dot: 0.79, + ty: -1909.61, + ty_dot: -0.70, + tz: -541.17, + tz_dot: -1.24, + s: -0.05109, + s_dot: -0.07201, + rx: -26.78138, + rx_dot: -0.06667, + ry: 0.42027, + ry_dot: 0.75744, + rz: -10.93206, + rz_dot: 0.05133, + epoch: 2010.0, + }, + }, +]; + +#[cfg(test)] +mod tests { + use super::*; + use float_eq::assert_float_eq; + use std::str::FromStr; + + #[test] + fn reference_frame_strings() { + assert_eq!(ReferenceFrame::ITRF2008.to_string(), "ITRF2008"); + assert_eq!( + ReferenceFrame::from_str("ITRF2008"), + Ok(ReferenceFrame::ITRF2008) + ); + assert_eq!(ReferenceFrame::ITRF2014.to_string(), "ITRF2014"); + assert_eq!( + ReferenceFrame::from_str("ITRF2014"), + Ok(ReferenceFrame::ITRF2014) + ); + assert_eq!(ReferenceFrame::ITRF2020.to_string(), "ITRF2020"); + assert_eq!( + ReferenceFrame::from_str("ITRF2020"), + Ok(ReferenceFrame::ITRF2020) + ); + assert_eq!(ReferenceFrame::ETRF2008.to_string(), "ETRF2008"); + assert_eq!( + ReferenceFrame::from_str("ETRF2008"), + Ok(ReferenceFrame::ETRF2008) + ); + assert_eq!(ReferenceFrame::ETRF2014.to_string(), "ETRF2014"); + assert_eq!( + ReferenceFrame::from_str("ETRF2014"), + Ok(ReferenceFrame::ETRF2014) + ); + assert_eq!(ReferenceFrame::ETRF2020.to_string(), "ETRF2020"); + assert_eq!( + ReferenceFrame::from_str("ETRF2020"), + Ok(ReferenceFrame::ETRF2020) + ); + assert_eq!(ReferenceFrame::NAD83_2011.to_string(), "NAD83(2011)"); + assert_eq!( + ReferenceFrame::from_str("NAD83_2011"), + Ok(ReferenceFrame::NAD83_2011) + ); + assert_eq!( + ReferenceFrame::from_str("NAD83(2011)"), + Ok(ReferenceFrame::NAD83_2011) + ); + assert_eq!(ReferenceFrame::NAD83_CSRS.to_string(), "NAD83(CSRS)"); + assert_eq!( + ReferenceFrame::from_str("NAD83_CSRS"), + Ok(ReferenceFrame::NAD83_CSRS) + ); + assert_eq!( + ReferenceFrame::from_str("NAD83(CSRS)"), + Ok(ReferenceFrame::NAD83_CSRS) + ); + } + + #[test] + fn helmert_position_translations() { + let params = TimeDependentHelmertParams { + tx: 1.0 / TimeDependentHelmertParams::TRANSLATE_SCALE, + tx_dot: 0.1 / TimeDependentHelmertParams::TRANSLATE_SCALE, + ty: 2.0 / TimeDependentHelmertParams::TRANSLATE_SCALE, + ty_dot: 0.2 / TimeDependentHelmertParams::TRANSLATE_SCALE, + tz: 3.0 / TimeDependentHelmertParams::TRANSLATE_SCALE, + tz_dot: 0.3 / TimeDependentHelmertParams::TRANSLATE_SCALE, + s: 0.0, + s_dot: 0.0, + rx: 0.0, + rx_dot: 0.0, + ry: 0.0, + ry_dot: 0.0, + rz: 0.0, + rz_dot: 0.0, + epoch: 2010.0, + }; + let initial_position = ECEF::default(); + + let transformed_position = params.transform_position(&initial_position, 2010.0); + assert_float_eq!(transformed_position.x(), 1.0, abs_all <= 1e-4); + assert_float_eq!(transformed_position.y(), 2.0, abs_all <= 1e-4); + assert_float_eq!(transformed_position.z(), 3.0, abs_all <= 1e-4); + + let transformed_position = params.transform_position(&initial_position, 2011.0); + assert_float_eq!(transformed_position.x(), 1.1, abs_all <= 1e-4); + assert_float_eq!(transformed_position.y(), 2.2, abs_all <= 1e-4); + assert_float_eq!(transformed_position.z(), 3.3, abs_all <= 1e-4); + } + + #[test] + fn helmert_position_scaling() { + let params = TimeDependentHelmertParams { + tx: 0.0, + tx_dot: 0.0, + ty: 0.0, + ty_dot: 0.0, + tz: 0.0, + tz_dot: 0.0, + s: 1.0 / TimeDependentHelmertParams::SCALE_SCALE, + s_dot: 0.1 / TimeDependentHelmertParams::SCALE_SCALE, + rx: 90.0, + rx_dot: 0.0, + ry: 0.0, + ry_dot: 0.0, + rz: 0.0, + rz_dot: 0.0, + epoch: 2010.0, + }; + let initial_position = ECEF::new(1., 2., 3.); + + let transformed_position = params.transform_position(&initial_position, 2010.0); + assert_float_eq!(transformed_position.x(), 2.0, abs_all <= 1e-4); + assert_float_eq!(transformed_position.y(), 4.0, abs_all <= 1e-4); + assert_float_eq!(transformed_position.z(), 6.0, abs_all <= 1e-4); + + let transformed_position = params.transform_position(&initial_position, 2011.0); + assert_float_eq!(transformed_position.x(), 2.1, abs_all <= 1e-4); + assert_float_eq!(transformed_position.y(), 4.2, abs_all <= 1e-4); + assert_float_eq!(transformed_position.z(), 6.3, abs_all <= 1e-4); + } + + #[test] + fn helmert_position_rotations() { + let params = TimeDependentHelmertParams { + tx: 0.0, + tx_dot: 0.0, + ty: 0.0, + ty_dot: 0.0, + tz: 0.0, + tz_dot: 0.0, + s: 0.0, + s_dot: 0.0, + rx: 1.0 / TimeDependentHelmertParams::ROTATE_SCALE, + rx_dot: 0.1 / TimeDependentHelmertParams::ROTATE_SCALE, + ry: 2.0 / TimeDependentHelmertParams::ROTATE_SCALE, + ry_dot: 0.2 / TimeDependentHelmertParams::ROTATE_SCALE, + rz: 3.0 / TimeDependentHelmertParams::ROTATE_SCALE, + rz_dot: 0.3 / TimeDependentHelmertParams::ROTATE_SCALE, + epoch: 2010.0, + }; + let initial_position = ECEF::new(1.0, 1.0, 1.0); + + let transformed_position = params.transform_position(&initial_position, 2010.0); + assert_float_eq!(transformed_position.x(), 0.0, abs_all <= 1e-4); + assert_float_eq!(transformed_position.y(), 3.0, abs_all <= 1e-4); + assert_float_eq!(transformed_position.z(), 0.0, abs_all <= 1e-4); + + let transformed_position = params.transform_position(&initial_position, 2011.0); + assert_float_eq!(transformed_position.x(), -0.1, abs_all <= 1e-9); + assert_float_eq!(transformed_position.y(), 3.2, abs_all <= 1e-9); + assert_float_eq!(transformed_position.z(), -0.1, abs_all <= 1e-9); + } + + #[test] + fn helmert_velocity_translations() { + let params = TimeDependentHelmertParams { + tx: 1.0 / TimeDependentHelmertParams::TRANSLATE_SCALE, + tx_dot: 0.1 / TimeDependentHelmertParams::TRANSLATE_SCALE, + ty: 2.0 / TimeDependentHelmertParams::TRANSLATE_SCALE, + ty_dot: 0.2 / TimeDependentHelmertParams::TRANSLATE_SCALE, + tz: 3.0 / TimeDependentHelmertParams::TRANSLATE_SCALE, + tz_dot: 0.3 / TimeDependentHelmertParams::TRANSLATE_SCALE, + s: 0.0, + s_dot: 0.0, + rx: 0.0, + rx_dot: 0.0, + ry: 0.0, + ry_dot: 0.0, + rz: 0.0, + rz_dot: 0.0, + epoch: 2010.0, + }; + let initial_velocity = ECEF::default(); + let position = ECEF::default(); + + let transformed_velocity = params.transform_velocity(&initial_velocity, &position); + assert_float_eq!(transformed_velocity.x(), 0.1, abs_all <= 1e-4); + assert_float_eq!(transformed_velocity.y(), 0.2, abs_all <= 1e-4); + assert_float_eq!(transformed_velocity.z(), 0.3, abs_all <= 1e-4); + } + + #[test] + fn helmert_velocity_scaling() { + let params = TimeDependentHelmertParams { + tx: 0.0, + tx_dot: 0.0, + ty: 0.0, + ty_dot: 0.0, + tz: 0.0, + tz_dot: 0.0, + s: 1.0 / TimeDependentHelmertParams::SCALE_SCALE, + s_dot: 0.1 / TimeDependentHelmertParams::SCALE_SCALE, + rx: 90.0, + rx_dot: 0.0, + ry: 0.0, + ry_dot: 0.0, + rz: 0.0, + rz_dot: 0.0, + epoch: 2010.0, + }; + let initial_velocity = ECEF::default(); + let position = ECEF::new(1., 2., 3.); + + let transformed_velocity = params.transform_velocity(&initial_velocity, &position); + assert_float_eq!(transformed_velocity.x(), 0.1, abs_all <= 1e-4); + assert_float_eq!(transformed_velocity.y(), 0.2, abs_all <= 1e-4); + assert_float_eq!(transformed_velocity.z(), 0.3, abs_all <= 1e-4); + } + + #[test] + fn helmert_velocity_rotations() { + let params = TimeDependentHelmertParams { + tx: 0.0, + tx_dot: 0.0, + ty: 0.0, + ty_dot: 0.0, + tz: 0.0, + tz_dot: 0.0, + s: 0.0, + s_dot: 0.0, + rx: 1.0 / TimeDependentHelmertParams::ROTATE_SCALE, + rx_dot: 0.1 / TimeDependentHelmertParams::ROTATE_SCALE, + ry: 2.0 / TimeDependentHelmertParams::ROTATE_SCALE, + ry_dot: 0.2 / TimeDependentHelmertParams::ROTATE_SCALE, + rz: 3.0 / TimeDependentHelmertParams::ROTATE_SCALE, + rz_dot: 0.3 / TimeDependentHelmertParams::ROTATE_SCALE, + epoch: 2010.0, + }; + let initial_velocity = ECEF::default(); + let position = ECEF::new(4., 5., 6.); + + let transformed_velocity = params.transform_velocity(&initial_velocity, &position); + assert_float_eq!(transformed_velocity.x(), -0.3, abs_all <= 1e-4); + assert_float_eq!(transformed_velocity.y(), 0.6, abs_all <= 1e-4); + assert_float_eq!(transformed_velocity.z(), -0.3, abs_all <= 1e-4); + } + + #[test] + fn helmert_invert() { + let mut params = TimeDependentHelmertParams { + tx: 1.0, + tx_dot: 0.1, + ty: 2.0, + ty_dot: 0.2, + tz: 3.0, + tz_dot: 0.3, + s: 4.0, + s_dot: 0.4, + rx: 5.0, + rx_dot: 0.5, + ry: 6.0, + ry_dot: 0.6, + rz: 7.0, + rz_dot: 0.7, + epoch: 2010.0, + }; + params.invert(); + assert_float_eq!(params.tx, -1.0, abs_all <= 1e-4); + assert_float_eq!(params.tx_dot, -0.1, abs_all <= 1e-4); + assert_float_eq!(params.ty, -2.0, abs_all <= 1e-4); + assert_float_eq!(params.ty_dot, -0.2, abs_all <= 1e-4); + assert_float_eq!(params.tz, -3.0, abs_all <= 1e-4); + assert_float_eq!(params.tz_dot, -0.3, abs_all <= 1e-4); + assert_float_eq!(params.s, -4.0, abs_all <= 1e-4); + assert_float_eq!(params.s_dot, -0.4, abs_all <= 1e-4); + assert_float_eq!(params.rx, -5.0, abs_all <= 1e-4); + assert_float_eq!(params.rx_dot, -0.5, abs_all <= 1e-4); + assert_float_eq!(params.ry, -6.0, abs_all <= 1e-4); + assert_float_eq!(params.ry_dot, -0.6, abs_all <= 1e-4); + assert_float_eq!(params.rz, -7.0, abs_all <= 1e-4); + assert_float_eq!(params.rz_dot, -0.7, abs_all <= 1e-4); + assert_float_eq!(params.epoch, 2010.0, abs_all <= 1e-4); + params.invert(); + assert_float_eq!(params.tx, 1.0, abs_all <= 1e-4); + assert_float_eq!(params.tx_dot, 0.1, abs_all <= 1e-4); + assert_float_eq!(params.ty, 2.0, abs_all <= 1e-4); + assert_float_eq!(params.ty_dot, 0.2, abs_all <= 1e-4); + assert_float_eq!(params.tz, 3.0, abs_all <= 1e-4); + assert_float_eq!(params.tz_dot, 0.3, abs_all <= 1e-4); + assert_float_eq!(params.s, 4.0, abs_all <= 1e-4); + assert_float_eq!(params.s_dot, 0.4, abs_all <= 1e-4); + assert_float_eq!(params.rx, 5.0, abs_all <= 1e-4); + assert_float_eq!(params.rx_dot, 0.5, abs_all <= 1e-4); + assert_float_eq!(params.ry, 6.0, abs_all <= 1e-4); + assert_float_eq!(params.ry_dot, 0.6, abs_all <= 1e-4); + assert_float_eq!(params.rz, 7.0, abs_all <= 1e-4); + assert_float_eq!(params.rz_dot, 0.7, abs_all <= 1e-4); + assert_float_eq!(params.epoch, 2010.0, abs_all <= 1e-4); + } +} diff --git a/swiftnav/src/time.rs b/swiftnav/src/time.rs index 84b0f7b..7040a21 100644 --- a/swiftnav/src/time.rs +++ b/swiftnav/src/time.rs @@ -312,6 +312,16 @@ impl GpsTime { self.tow().total_cmp(&other) } } + + pub fn to_fractional_year(&self, utc_params: &UtcParams) -> f64 { + let utc = self.to_utc(utc_params); + utc.to_fractional_year() + } + + pub fn to_fractional_year_hardcoded(&self) -> f64 { + let utc = self.to_utc_hardcoded(); + utc.to_fractional_year() + } } impl fmt::Debug for GpsTime { @@ -768,6 +778,21 @@ impl UtcTime { } gps } + + pub fn to_fractional_year(&self) -> f64 { + let year = self.year() as f64; + let days = self.day_of_year() as f64; + let hours = self.hour() as f64; + let minutes = self.minute() as f64; + let seconds = self.seconds(); + let total_days = days + hours / 24. + minutes / 1440. + seconds / 86400.; + + if is_leap_year(self.year()) { + year + total_days / 366.0 + } else { + year + total_days / 365.0 + } + } } impl Default for UtcTime { @@ -868,6 +893,10 @@ impl From for MJD { } } +pub fn is_leap_year(year: u16) -> bool { + ((year % 4 == 0) && (year % 100 != 0)) || (year % 400 == 0) +} + #[cfg(test)] mod tests { use super::*; @@ -1669,4 +1698,13 @@ mod tests { assert_eq!(gps.wn(), swiftnav_sys::GLO_EPOCH_WN as i16); assert!((gps.tow() - swiftnav_sys::GLO_EPOCH_TOW as f64).abs() < 1e-9); } + + #[test] + fn is_leap_year() { + use super::is_leap_year; + assert!(!is_leap_year(2019)); + assert!(is_leap_year(2020)); + assert!(!is_leap_year(1900)); + assert!(is_leap_year(2000)); + } } diff --git a/swiftnav/tests/reference_frames.rs b/swiftnav/tests/reference_frames.rs new file mode 100644 index 0000000..2a23548 --- /dev/null +++ b/swiftnav/tests/reference_frames.rs @@ -0,0 +1,479 @@ +use float_eq::assert_float_eq; +use swiftnav::{ + coords::{Coordinate, ECEF}, + reference_frame::{get_transformation, ReferenceFrame}, + time::{GpsTime, UtcTime}, +}; + +fn make_epoch(year: u16) -> GpsTime { + UtcTime::from_date(year, 1, 1, 0, 0, 0.).to_gps_hardcoded() +} + +/// Truth data obtained from https://www.epncb.oma.be/_productsservices/coord_trans/ +#[test] +fn euref_itrf2014() { + let initial_coords = Coordinate::new( + ReferenceFrame::ITRF2020, + ECEF::new(4027894.006, 307045.600, 4919474.910), + Some(ECEF::new(0.01, 0.2, 0.030)), + make_epoch(2000), + ); + let transformation = + get_transformation(ReferenceFrame::ITRF2020, ReferenceFrame::ITRF2014).unwrap(); + let result_coords = transformation.transform(&initial_coords); + assert_float_eq!(result_coords.position().x(), 4027894.0029, abs <= 0.0001); + assert_float_eq!(result_coords.position().y(), 307045.6005, abs <= 0.0001); + assert_float_eq!(result_coords.position().z(), 4919474.9063, abs <= 0.0001); + assert!(result_coords.velocity().is_some()); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().x(), + 0.0100, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().y(), + 0.1999, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().z(), + 0.0302, + abs <= 0.1 + ); + assert_eq!(result_coords.epoch(), make_epoch(2000)); + assert_eq!(result_coords.reference_frame(), ReferenceFrame::ITRF2014); +} + +/// Truth data obtained from https://www.epncb.oma.be/_productsservices/coord_trans/ +#[test] +fn euref_itrf2008() { + let initial_coords = Coordinate::new( + ReferenceFrame::ITRF2020, + ECEF::new(4027894.006, 307045.600, 4919474.910), + Some(ECEF::new(0.01, 0.2, 0.030)), + make_epoch(2000), + ); + let transformation = + get_transformation(ReferenceFrame::ITRF2020, ReferenceFrame::ITRF2008).unwrap(); + let result_coords = transformation.transform(&initial_coords); + assert_float_eq!(result_coords.position().x(), 4027894.0032, abs <= 0.0001); + assert_float_eq!(result_coords.position().y(), 307045.6023, abs <= 0.0001); + assert_float_eq!(result_coords.position().z(), 4919474.9082, abs <= 0.0001); + assert!(result_coords.velocity().is_some()); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().x(), + 0.0101, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().y(), + 0.1999, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().z(), + 0.0302, + abs <= 0.1 + ); + assert_eq!(result_coords.epoch(), make_epoch(2000)); + assert_eq!(result_coords.reference_frame(), ReferenceFrame::ITRF2008); +} + +/// Truth data obtained from https://www.epncb.oma.be/_productsservices/coord_trans/ +#[test] +fn euref_etrf2020() { + let initial_coords = Coordinate::new( + ReferenceFrame::ITRF2020, + ECEF::new(4027894.006, 307045.600, 4919474.910), + Some(ECEF::new(0.01, 0.2, 0.030)), + make_epoch(2000), + ); + let transformation = + get_transformation(ReferenceFrame::ITRF2020, ReferenceFrame::ETRF2020).unwrap(); + let result_coords = transformation.transform(&initial_coords); + assert_float_eq!(result_coords.position().x(), 4027894.1545, abs <= 0.0001); + assert_float_eq!(result_coords.position().y(), 307045.4157, abs <= 0.0001); + assert_float_eq!(result_coords.position().z(), 4919474.7999, abs <= 0.0001); + assert!(result_coords.velocity().is_some()); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().x(), + 0.0235, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().y(), + 0.1832, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().z(), + 0.0200, + abs <= 0.1 + ); + assert_eq!(result_coords.epoch(), make_epoch(2000)); + assert_eq!(result_coords.reference_frame(), ReferenceFrame::ETRF2020); +} + +/// Truth data obtained from https://www.epncb.oma.be/_productsservices/coord_trans/ +#[test] +fn euref_etrf2014() { + let initial_coords = Coordinate::new( + ReferenceFrame::ITRF2020, + ECEF::new(4027894.006, 307045.600, 4919474.910), + Some(ECEF::new(0.01, 0.2, 0.030)), + make_epoch(2000), + ); + let transformation = + get_transformation(ReferenceFrame::ITRF2020, ReferenceFrame::ETRF2014).unwrap(); + let result_coords = transformation.transform(&initial_coords); + assert_float_eq!(result_coords.position().x(), 4027894.1548, abs <= 0.0001); + assert_float_eq!(result_coords.position().y(), 307045.4128, abs <= 0.0001); + assert_float_eq!(result_coords.position().z(), 4919474.7937, abs <= 0.0001); + assert!(result_coords.velocity().is_some()); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().x(), + 0.0238, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().y(), + 0.1828, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().z(), + 0.0200, + abs <= 0.1 + ); + assert_eq!(result_coords.epoch(), make_epoch(2000)); + assert_eq!(result_coords.reference_frame(), ReferenceFrame::ETRF2014); +} + +/// Truth data obtained from https://www.epncb.oma.be/_productsservices/coord_trans/ +#[test] +fn euref_etrf2014_reverse() { + let initial_coords = Coordinate::new( + ReferenceFrame::ETRF2014, + ECEF::new(4027894.006, 307045.600, 4919474.910), + Some(ECEF::new(0.01, 0.2, 0.030)), + make_epoch(2000), + ); + let transformation = + get_transformation(ReferenceFrame::ETRF2014, ReferenceFrame::ITRF2020).unwrap(); + let result_coords = transformation.transform(&initial_coords); + assert_float_eq!(result_coords.position().x(), 4027893.8572, abs <= 0.0001); + assert_float_eq!(result_coords.position().y(), 307045.7872, abs <= 0.0001); + assert_float_eq!(result_coords.position().z(), 4919475.0263, abs <= 0.0001); + assert!(result_coords.velocity().is_some()); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().x(), + -0.0038, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().y(), + 0.2172, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().z(), + 0.0400, + abs <= 0.1 + ); + assert_eq!(result_coords.epoch(), make_epoch(2000)); + assert_eq!(result_coords.reference_frame(), ReferenceFrame::ITRF2020); +} + +/// Truth data obtained from https://www.epncb.oma.be/_productsservices/coord_trans/ +#[test] +fn euref_adjust_epoch() { + let initial_coords = Coordinate::new( + ReferenceFrame::ITRF2020, + ECEF::new(4027894.006, 307045.600, 4919474.910), + Some(ECEF::new(0.01, 0.2, 0.030)), + make_epoch(2000), + ); + let result_coords = initial_coords.adjust_epoch(&make_epoch(2008)); + assert_float_eq!(result_coords.position().x(), 4027894.0860, abs <= 0.0001); + assert_float_eq!(result_coords.position().y(), 307047.2000, abs <= 0.0001); + assert_float_eq!(result_coords.position().z(), 4919475.1500, abs <= 0.0001); + assert!(result_coords.velocity().is_some()); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().x(), + 0.01, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().y(), + 0.2, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().z(), + 0.03, + abs <= 0.1 + ); + assert_eq!(result_coords.epoch(), make_epoch(2008)); + assert_eq!(result_coords.reference_frame(), ReferenceFrame::ITRF2020); +} + +/// Truth data obtained from https://www.epncb.oma.be/_productsservices/coord_trans/ +#[test] +fn euref_complete_transform() { + let initial_coords = Coordinate::new( + ReferenceFrame::ITRF2020, + ECEF::new(4027894.006, 307045.600, 4919474.910), + Some(ECEF::new(0.01, 0.2, 0.030)), + make_epoch(2000), + ); + let transformation = + get_transformation(ReferenceFrame::ITRF2020, ReferenceFrame::ETRF2014).unwrap(); + + // Test adjusting the epoch first then transforming + let result_coords = transformation.transform(&initial_coords.adjust_epoch(&make_epoch(2008))); + assert_float_eq!(result_coords.position().x(), 4027894.3453, abs <= 0.0001); + assert_float_eq!(result_coords.position().y(), 307046.8755, abs <= 0.0001); + assert_float_eq!(result_coords.position().z(), 4919474.9533, abs <= 0.0001); + assert!(result_coords.velocity().is_some()); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().x(), + 0.0238, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().y(), + 0.1828, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().z(), + 0.0200, + abs <= 0.1 + ); + assert_eq!(result_coords.epoch(), make_epoch(2008)); + assert_eq!(result_coords.reference_frame(), ReferenceFrame::ETRF2014); + + // Test transforming first then adjusting the epoch + let result_coords = transformation + .transform(&initial_coords) + .adjust_epoch(&make_epoch(2008)); + assert_float_eq!(result_coords.position().x(), 4027894.3453, abs <= 0.0001); + assert_float_eq!(result_coords.position().y(), 307046.8755, abs <= 0.0001); + assert_float_eq!(result_coords.position().z(), 4919474.9533, abs <= 0.0001); + assert!(result_coords.velocity().is_some()); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().x(), + 0.0238, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().y(), + 0.1828, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().z(), + 0.0200, + abs <= 0.1 + ); + assert_eq!(result_coords.epoch(), make_epoch(2008)); + assert_eq!(result_coords.reference_frame(), ReferenceFrame::ETRF2014); +} + +/// Truth data obtained from https://geodesy.noaa.gov/TOOLS/Htdp/Htdp.shtml +#[test] +fn htdp_nad83_2011_fixed_date() { + let initial_coords = Coordinate::new( + ReferenceFrame::ITRF2014, + ECEF::new(-2705105.358, -4262045.769, 3885381.686), + Some(ECEF::new(-0.02225, 0.02586, 0.01258)), + make_epoch(2010), + ); + + let transformation = + get_transformation(ReferenceFrame::ITRF2014, ReferenceFrame::NAD83_2011).unwrap(); + + let result_coords = transformation.transform(&initial_coords); + assert_float_eq!(result_coords.position().x(), -2705104.572, abs <= 0.001); + assert_float_eq!(result_coords.position().y(), -4262047.032, abs <= 0.001); + assert_float_eq!(result_coords.position().z(), 3885381.705, abs <= 0.001); + assert!(result_coords.velocity().is_some()); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().x(), + -0.00594, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().y(), + 0.02615, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().z(), + 0.02217, + abs <= 0.1 + ); + assert_eq!(result_coords.epoch(), make_epoch(2010)); + assert_eq!(result_coords.reference_frame(), ReferenceFrame::NAD83_2011); +} + +/// Truth data obtained from https://geodesy.noaa.gov/TOOLS/Htdp/Htdp.shtml +#[test] +fn htdp_nad83_2011_adjust_epoch() { + let initial_coords = Coordinate::new( + ReferenceFrame::ITRF2014, + ECEF::new(-2705105.358, -4262045.769, 3885381.686), + Some(ECEF::new(-0.02225, 0.02586, 0.01258)), + make_epoch(2020), + ); + + let transformation = + get_transformation(ReferenceFrame::ITRF2014, ReferenceFrame::NAD83_2011).unwrap(); + + let result_coords = transformation.transform(&initial_coords.adjust_epoch(&make_epoch(2010))); + assert_float_eq!(result_coords.position().x(), -2705104.349, abs <= 0.001); + assert_float_eq!(result_coords.position().y(), -4262047.291, abs <= 0.001); + assert_float_eq!(result_coords.position().z(), 3885381.579, abs <= 0.001); + assert!(result_coords.velocity().is_some()); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().x(), + -0.00594, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().y(), + 0.02615, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().z(), + 0.02217, + abs <= 0.1 + ); + assert_eq!(result_coords.epoch(), make_epoch(2010)); + assert_eq!(result_coords.reference_frame(), ReferenceFrame::NAD83_2011); + + let result_coords = transformation + .transform(&initial_coords) + .adjust_epoch(&make_epoch(2010)); + assert_float_eq!(result_coords.position().x(), -2705104.349, abs <= 0.001); + assert_float_eq!(result_coords.position().y(), -4262047.291, abs <= 0.001); + assert_float_eq!(result_coords.position().z(), 3885381.579, abs <= 0.001); + assert!(result_coords.velocity().is_some()); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().x(), + -0.00594, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().y(), + 0.02615, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().z(), + 0.02217, + abs <= 0.1 + ); + assert_eq!(result_coords.epoch(), make_epoch(2010)); + assert_eq!(result_coords.reference_frame(), ReferenceFrame::NAD83_2011); +} + +/// Truth data obtained from https://webapp.csrs-scrs.nrcan-rncan.gc.ca/geod/tools-outils/trx.php +#[test] +fn trx_nad83_csrs_fixed_date() { + let initial_coords = Coordinate::new( + ReferenceFrame::ITRF2020, + ECEF::new(1267458.677, -4294620.216, 4526843.210), + Some(ECEF::new(-0.01578, -0.00380, 0.00466)), + make_epoch(2010), + ); + + let transformation = + get_transformation(ReferenceFrame::ITRF2020, ReferenceFrame::NAD83_CSRS).unwrap(); + + let result_coords = transformation.transform(&initial_coords); + assert_float_eq!(result_coords.position().x(), 1267459.462, abs <= 0.001); + assert_float_eq!(result_coords.position().y(), -4294621.605, abs <= 0.001); + assert_float_eq!(result_coords.position().z(), 4526843.224, abs <= 0.001); + assert!(result_coords.velocity().is_some()); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().x(), + 0.00261, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().y(), + -0.00241, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().z(), + -0.00017, + abs <= 0.1 + ); + assert_eq!(result_coords.epoch(), make_epoch(2010)); + assert_eq!(result_coords.reference_frame(), ReferenceFrame::NAD83_CSRS); +} + +/// Truth data obtained from https://geodesy.noaa.gov/TOOLS/Htdp/Htdp.shtml +#[test] +fn trx_nad83_csrs_adjust_epoch() { + let initial_coords = Coordinate::new( + ReferenceFrame::ITRF2020, + ECEF::new(1267458.677, -4294620.216, 4526843.210), + Some(ECEF::new(-0.01578, -0.00380, 0.00466)), + make_epoch(2020), + ); + + let transformation = + get_transformation(ReferenceFrame::ITRF2020, ReferenceFrame::NAD83_CSRS).unwrap(); + + let result_coords = transformation.transform(&initial_coords.adjust_epoch(&make_epoch(2010))); + assert_float_eq!(result_coords.position().x(), 1267459.620, abs <= 0.001); + assert_float_eq!(result_coords.position().y(), -4294621.567, abs <= 0.001); + assert_float_eq!(result_coords.position().z(), 4526843.177, abs <= 0.001); + assert!(result_coords.velocity().is_some()); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().x(), + 0.00261, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().y(), + -0.00241, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().z(), + -0.00017, + abs <= 0.1 + ); + assert_eq!(result_coords.epoch(), make_epoch(2010)); + assert_eq!(result_coords.reference_frame(), ReferenceFrame::NAD83_CSRS); + + let result_coords = transformation + .transform(&initial_coords) + .adjust_epoch(&make_epoch(2010)); + assert_float_eq!(result_coords.position().x(), 1267459.620, abs <= 0.001); + assert_float_eq!(result_coords.position().y(), -4294621.567, abs <= 0.001); + assert_float_eq!(result_coords.position().z(), 4526843.177, abs <= 0.001); + assert!(result_coords.velocity().is_some()); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().x(), + 0.00261, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().y(), + -0.00241, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().z(), + -0.00017, + abs <= 0.1 + ); + assert_eq!(result_coords.epoch(), make_epoch(2010)); + assert_eq!(result_coords.reference_frame(), ReferenceFrame::NAD83_CSRS); +}