nav_types/
ecef.rs

1use crate::enu::ENU;
2use crate::nvector::NVector;
3use crate::utils::RealFieldCopy;
4use crate::wgs84::{ECCENTRICITY_SQ, SEMI_MAJOR_AXIS, SEMI_MINOR_AXIS, WGS84};
5use crate::Access;
6use na::{Matrix3, Point3};
7use std::convert::{From, Into};
8use std::ops::{Add, AddAssign, Sub, SubAssign};
9
10/// Earth Centered Earth Fixed position
11///
12/// This struct represents a position in the ECEF coordinate system.
13/// See: [ECEF](https://en.wikipedia.org/wiki/ECEF) for general description.
14#[derive(Debug, Copy, Clone, PartialEq)]
15pub struct ECEF<N: RealFieldCopy>(Point3<N>);
16
17impl<N: RealFieldCopy> ECEF<N> {
18    /// Create a new ECEF position
19    pub fn new(x: N, y: N, z: N) -> ECEF<N> {
20        ECEF(Point3::new(x, y, z))
21    }
22}
23
24impl<N: RealFieldCopy> ECEF<N> {
25    /// Get the X component of this position
26    pub fn x(&self) -> N {
27        self.0.x
28    }
29
30    /// Get the Y component of this position
31    pub fn y(&self) -> N {
32        self.0.y
33    }
34
35    /// Get the Z component of this position
36    pub fn z(&self) -> N {
37        self.0.z
38    }
39}
40
41impl<N: RealFieldCopy> ECEF<N> {
42    /// Create a rotation matrix from ECEF frame to ENU frame
43    fn r_enu_from_ecef(self) -> Matrix3<N> {
44        let wgs = WGS84::from(self);
45        Matrix3::new(
46            -wgs.longitude_radians().sin(),
47            wgs.longitude_radians().cos(),
48            N::zero(),
49            -wgs.latitude_radians().sin() * wgs.longitude_radians().cos(),
50            -wgs.latitude_radians().sin() * wgs.longitude_radians().sin(),
51            wgs.latitude_radians().cos(),
52            wgs.latitude_radians().cos() * wgs.longitude_radians().cos(),
53            wgs.latitude_radians().cos() * wgs.longitude_radians().sin(),
54            wgs.latitude_radians().sin(),
55        )
56    }
57
58    /// Create a rotation matrix from ENU frame to ECEF frame
59    fn r_ecef_from_enu(self) -> Matrix3<N> {
60        self.r_enu_from_ecef().transpose()
61    }
62
63    /// Euclidean distance between two ECEF positions
64    pub fn distance(&self, other: &ECEF<N>) -> N
65    where
66        N: RealFieldCopy,
67    {
68        (other.0 - self.0).norm()
69    }
70}
71
72impl<N, T> Add<T> for ECEF<N>
73where
74    N: RealFieldCopy,
75    T: Into<ENU<N>>,
76{
77    type Output = ECEF<N>;
78    fn add(self, right: T) -> ECEF<N> {
79        let enu = T::into(right);
80        let pos = self.0 + self.r_ecef_from_enu() * enu.access();
81        ECEF(pos)
82    }
83}
84
85impl<N, T> AddAssign<T> for ECEF<N>
86where
87    N: RealFieldCopy + AddAssign,
88    T: Into<ENU<N>>,
89{
90    fn add_assign(&mut self, right: T) {
91        let enu = T::into(right);
92        self.0 = self.0 + (self.r_ecef_from_enu() * enu.access());
93    }
94}
95
96impl<N: RealFieldCopy> Sub<ECEF<N>> for ECEF<N> {
97    type Output = ENU<N>;
98    fn sub(self, right: ECEF<N>) -> ENU<N> {
99        let enu = right.r_enu_from_ecef() * (self.0 - right.0);
100        ENU::new(enu.x, enu.y, enu.z)
101    }
102}
103
104impl<N, T> Sub<T> for ECEF<N>
105where
106    N: RealFieldCopy,
107    T: Into<ENU<N>>,
108{
109    type Output = ECEF<N>;
110    fn sub(self, right: T) -> ECEF<N> {
111        let enu = T::into(right);
112        ECEF(self.0 - (self.r_ecef_from_enu() * enu.access()))
113    }
114}
115
116impl<N, T> SubAssign<T> for ECEF<N>
117where
118    N: RealFieldCopy + SubAssign,
119    T: Into<ENU<N>>,
120{
121    fn sub_assign(&mut self, right: T) {
122        let enu = T::into(right);
123        self.0 = self.0 - (self.r_ecef_from_enu() * enu.access());
124    }
125}
126
127// This macro implements most standard operations for position types that
128// can be converted to ECEF
129macro_rules! ecef_impl {
130    ($T:ident) => {
131        impl<N, T> Add<T> for $T<N>
132        where
133            N: RealFieldCopy,
134            T: Into<ENU<N>>,
135        {
136            type Output = $T<N>;
137            fn add(self, right: T) -> Self {
138                $T::from(ECEF::from(self) + right)
139            }
140        }
141
142        impl<N, T> AddAssign<T> for $T<N>
143        where
144            N: RealFieldCopy + AddAssign,
145            T: Into<ENU<N>>,
146        {
147            fn add_assign(&mut self, right: T) {
148                *self = $T::from(ECEF::from(*self) + right);
149            }
150        }
151
152        impl<N, T> Sub<T> for $T<N>
153        where
154            N: RealFieldCopy,
155            T: Into<ENU<N>>,
156        {
157            type Output = $T<N>;
158            fn sub(self, right: T) -> $T<N> {
159                $T::from(ECEF::from(self) - right)
160            }
161        }
162
163        impl<N: RealFieldCopy> Sub<$T<N>> for $T<N> {
164            type Output = ENU<N>;
165            fn sub(self, right: $T<N>) -> ENU<N> {
166                ECEF::from(self) - ECEF::from(right)
167            }
168        }
169
170        impl<N, T> SubAssign<T> for $T<N>
171        where
172            N: RealFieldCopy + SubAssign,
173            T: Into<ENU<N>>,
174        {
175            fn sub_assign(&mut self, right: T) {
176                *self = $T::from(ECEF::from(*self) - right);
177            }
178        }
179    };
180}
181
182// In accordance with Gade(2010) all N-Vector operations works through
183// converting to ECEF and then back
184ecef_impl!(NVector);
185// The only way to work with Latitude/Longitude is to convert to ECEF
186ecef_impl!(WGS84);
187
188impl<N: RealFieldCopy> From<WGS84<N>> for ECEF<N> {
189    fn from(wgs: WGS84<N>) -> ECEF<N> {
190        // Conversion from:
191        // https://doi.org/10.1007/s00190-004-0375-4
192        let semi_major_axis = N::from_f64(SEMI_MAJOR_AXIS).unwrap();
193        let ecc_part = N::from_f64(ECCENTRICITY_SQ).unwrap();
194        let sin_part = N::from_f64(0.5).unwrap()
195            * (N::one() - (N::from_f64(2.0).unwrap() * wgs.latitude_radians()).cos());
196
197        let n = semi_major_axis / (N::one() - ecc_part * sin_part).sqrt();
198        let altitude = wgs.altitude();
199
200        let x = (altitude + n) * wgs.latitude_radians().cos() * wgs.longitude_radians().cos();
201        let y = (altitude + n) * wgs.latitude_radians().cos() * wgs.longitude_radians().sin();
202        let z = (altitude + n - N::from_f64(ECCENTRICITY_SQ).unwrap() * n)
203            * wgs.latitude_radians().sin();
204
205        ECEF::new(x, y, z)
206    }
207}
208
209impl<N: RealFieldCopy> From<NVector<N>> for ECEF<N> {
210    fn from(f: NVector<N>) -> ECEF<N> {
211        // Constants used for calculation
212        let x = f.vector().z;
213        let y = f.vector().y;
214        let z = -f.vector().x;
215        let a_over_b = N::from_f64(SEMI_MAJOR_AXIS).unwrap().powi(2)
216            / N::from_f64(SEMI_MINOR_AXIS).unwrap().powi(2);
217        // Multiplication part
218        let mul = N::from_f64(SEMI_MINOR_AXIS).unwrap()
219            / (x.powi(2) + a_over_b * y.powi(2) + a_over_b * z.powi(2)).sqrt();
220        // NOTE: The following has been rearranged to follow ECEF convention
221        // that Z points towards the north pole
222        ECEF::new(
223            -(mul * a_over_b * z + z * f.altitude()),
224            mul * a_over_b * y + y * f.altitude(),
225            mul * x + x * f.altitude(),
226        )
227    }
228}
229
230#[cfg(test)]
231mod tests {
232    use super::*;
233    use crate::enu::ENU;
234    use crate::ned::NED;
235    use crate::nvector::NVector;
236    use crate::wgs84::WGS84;
237    use crate::Access;
238    use assert::close;
239
240    // Helper method to check that two ECEF positions are equal
241    fn ecef_close(a: ECEF<f64>, b: ECEF<f64>) {
242        close(a.x(), b.x(), 0.000001);
243        close(a.y(), b.y(), 0.000001);
244        close(a.z(), b.z(), 0.000001);
245    }
246
247    quickcheck! {
248        fn from_wgs84(wgs: WGS84<f64>) -> () {
249            let test = WGS84::from(ECEF::from(wgs));
250
251            close(wgs.latitude_radians(), test.latitude_radians(), 0.000001);
252            close(wgs.longitude_radians(), test.longitude_radians(), 0.000001);
253            close(wgs.altitude(), test.altitude(), 0.000001);
254        }
255
256        fn from_nvector(wgs: WGS84<f64>) -> () {
257            let ans = ECEF::from(wgs);
258            let test = ECEF::from(NVector::from(wgs));
259
260            ecef_close(ans, test);
261        }
262
263        fn rotation(a: WGS84<f64>, b: WGS84<f64>) -> () {
264            // Convert position into ECEF, we use WGS84 for simplicity
265            let ecef_a = ECEF::from(a);
266            let ecef_b = ECEF::from(b);
267
268            // Calculate the vector between the two positions in ENU frame
269            // and in Earth frame
270            let vec_enu = ecef_b - ecef_a;
271            let vec_e = ecef_b.0 - ecef_a.0;
272
273            // Retrieve the rotation matrix for A converting ENU -> Earth
274            let r_ecef_from_enu = ecef_a.r_ecef_from_enu();
275            let vec_e2 = r_ecef_from_enu * vec_enu.access();
276
277            // These should be equivalent:
278            close(vec_e.as_ref(), vec_e2.as_ref(), 0.0000001);
279        }
280
281        fn add_vector(a: WGS84<f64>, b: WGS84<f64>) -> () {
282            // Test that the difference between two positions added to
283            // the first position leads to the second position
284            let ecef_a = ECEF::from(a);
285            let ecef_b = ECEF::from(b);
286
287            let vec_ab = ecef_b - ecef_a;
288            let ecef_b2 = ecef_a + vec_ab;
289
290            ecef_close(ecef_b, ecef_b2);
291        }
292
293        // fn distance(a: WGS84<f64>, b: WGS84<f64>) -> () {
294        //     // Test that the vector A->B and B->A is of equal length
295        //     let dist_ab = (ECEF::from(b) - ECEF::from(a)).norm();
296        //     let dist_ba = (ECEF::from(a) - ECEF::from(b)).norm();
297
298        //     close(dist_ab, dist_ba, 0.000001);
299        // }
300
301        fn add_ned(a: WGS84<f64>, n: f64, e: f64, d: f64) -> () {
302            // Test that adding a random NED vector to an ECEF position
303            // is the same as adding the ENU version of the vector
304            let ecef = ECEF::from(a);
305            let enu_vec = ENU::new(e, n, -d);
306            let ned_vec = NED::new(n, e, d);
307
308            let ecef_enu = ecef + enu_vec;
309            let ecef_ned = ecef + ned_vec;
310            ecef_close(ecef_enu, ecef_ned);
311        }
312
313        fn sub_ned(a: WGS84<f64>, n: f64, e: f64, d: f64) -> () {
314            // See `add_ned`
315            let ecef = ECEF::from(a);
316            let enu_vec = ENU::new(e, n, -d);
317            let ned_vec = NED::new(n, e, d);
318
319            let ecef_enu = ecef - enu_vec;
320            let ecef_ned = ecef - ned_vec;
321            ecef_close(ecef_enu, ecef_ned);
322        }
323
324        fn add_enu(a: WGS84<f64>, e: f64, n: f64, u: f64) -> () {
325            // Test that adding a random ENU vector to an ECEF position
326            // than taking the difference between those two positions
327            // result in the original ENU vector
328            let ecef = ECEF::from(a);
329            let enu = ENU::new(e, n, u);
330
331            let ecef_2 = ecef + enu;
332            let enu_2 = ecef_2 - ecef;
333            close(enu.access().as_ref(), enu_2.access().as_ref(), 0.0000001);
334        }
335    }
336}
nav_types/ecef.rs

nav_types/
ecef.rs
