nav_types/
nvector.rs

1use crate::ecef::ECEF;
2use crate::utils::RealFieldCopy;
3use crate::wgs84::{ECCENTRICITY_SQ, SEMI_MAJOR_AXIS, WGS84};
4use na::Vector3;
5use std::convert::From;
6
7/// N-Vector position
8///
9/// The N-Vector represents unique points on the earth's surface.
10/// The advantage of N-Vectors is that they have no inconsistencies around
11/// the poles compared to WGS84 Latitude, Longitude format.
12/// See: [nvector](http://www.navlab.net/nvector/) for detailed information.
13#[derive(PartialEq, Clone, Copy, Debug)]
14pub struct NVector<N: RealFieldCopy> {
15    vec: Vector3<N>,
16    alt: N,
17}
18
19impl<N: RealFieldCopy> NVector<N> {
20    /// Create a new NVector
21    pub fn new(vec: Vector3<N>, altitude: N) -> NVector<N> {
22        NVector { vec, alt: altitude }
23    }
24}
25
26impl<N: RealFieldCopy> NVector<N> {
27    /// Get the vector component of this position
28    pub fn vector(&self) -> Vector3<N> {
29        self.vec
30    }
31
32    /// Get the altitude of this position
33    pub fn altitude(&self) -> N {
34        self.alt
35    }
36}
37
38impl<N: RealFieldCopy> From<WGS84<N>> for NVector<N> {
39    fn from(f: WGS84<N>) -> NVector<N> {
40        // This implementation defines the ECEF coordinate system to have the Z
41        // axes point directly north, this affects the way which N-vectors are
42        // defined. See: Table 2 in Gade(2010).
43        // NOTE: This is consistent with the ECEF implementation in this crate
44        let vec = Vector3::new(
45            f.longitude_radians().cos() * f.latitude_radians().cos(),
46            f.longitude_radians().sin() * f.latitude_radians().cos(),
47            f.latitude_radians().sin(),
48        );
49        NVector::new(vec, f.altitude())
50    }
51}
52
53impl<N: RealFieldCopy> From<ECEF<N>> for NVector<N> {
54    #![allow(clippy::many_single_char_names)]
55    fn from(ecef: ECEF<N>) -> NVector<N> {
56        // These are often used constants below:
57        // a²
58        let a_sq = N::from_f64(SEMI_MAJOR_AXIS).unwrap().powi(2);
59        // e²
60        let e_2 = N::from_f64(ECCENTRICITY_SQ).unwrap();
61        // e⁴
62        let e_4 = e_2.powi(2);
63
64        // Select correct axis form ECEF vector
65        let x = ecef.z();
66        let y = ecef.y();
67        let z = -ecef.x();
68
69        let p = (y.powi(2) + z.powi(2)) / a_sq;
70        let q = ((N::one() - e_2) / a_sq) * x.powi(2);
71        let r = (p + q - e_4) / N::from_f64(6.0).unwrap();
72        let s = (e_4 * p * q) / (N::from_f64(4.0).unwrap() * r.powi(3));
73        let t = (N::one() + s + (s * (N::from_f64(2.0).unwrap() + s)).sqrt()).cbrt();
74        let u = r * (N::one() + t + t.recip());
75        let v = (u.powi(2) + e_4 * q).sqrt();
76        let w = e_2 * ((u + v - q) / (N::from_f64(2.0).unwrap() * v));
77        let k = (u + v + w.powi(2)).sqrt() - w;
78        let d = (k * (y.powi(2) + z.powi(2)).sqrt()) / (k + e_2);
79
80        let altitude = ((k + e_2 - N::one()) / k) * (d.powi(2) + x.powi(2)).sqrt();
81
82        let denom = (d.powi(2) + x.powi(2)).sqrt().recip();
83        let mul = k / (k + e_2);
84        let vec = Vector3::new(-mul * z * denom, mul * y * denom, x * denom);
85
86        NVector::new(vec, altitude)
87    }
88}
89
90#[cfg(test)]
91mod tests {
92    use super::*;
93    use crate::ecef::ECEF;
94    use crate::wgs84::WGS84;
95    use assert::close;
96
97    quickcheck! {
98        fn from_wgs84(wgs: WGS84<f64>) -> () {
99            let test = WGS84::from(NVector::from(wgs));
100
101            close(wgs.latitude_radians(), test.latitude_radians(), 0.000001);
102            close(wgs.longitude_radians(), test.longitude_radians(), 0.000001);
103            close(wgs.altitude(), test.altitude(), 0.000001);
104        }
105
106        fn from_ecef(wgs: WGS84<f64>) -> () {
107            let ans = NVector::from(wgs);
108            let test = NVector::from(ECEF::from(wgs));
109
110            close(ans.altitude(), test.altitude(), 0.000001);
111            close(ans.vector().as_ref(), test.vector().as_ref(), 0.000001)
112        }
113    }
114}
nav_types/nvector.rs

nav_types/
nvector.rs
