nav_types/
wgs84.rs

1use crate::ecef::ECEF;
2use crate::nvector::NVector;
3use crate::utils::RealFieldCopy;
4use std::convert::From;
5
6#[cfg(test)]
7use quickcheck::{Arbitrary, Gen};
8
9pub const INVERSE_FLATTENING: f64 = 298.257_223_563;
10pub const FLATTENING: f64 = 1.0 / INVERSE_FLATTENING;
11/// Semi-major axis of WGS84 ellipsoid in meters. This represents the
12/// radius of the WGS84 ellipsoid.
13pub const SEMI_MAJOR_AXIS: f64 = 6_378_137.0;
14pub const SEMI_MINOR_AXIS: f64 = SEMI_MAJOR_AXIS * (1.0 - FLATTENING);
15pub const ECCENTRICITY_SQ: f64 = 2.0 * FLATTENING - FLATTENING * FLATTENING;
16
17/// Geodetic position
18///
19/// This struct represents a position in the geodetic system on the WGS84
20/// ellipsoid.
21/// See: [WGS84](https://en.wikipedia.org/wiki/World_Geodetic_System) for
22/// more information.
23/// The `serde` feature allows this to be Serialized / Deserialized.
24/// If serialized into json, it will look like this. Enabled thought
25/// the `serde` feature
26/// ```json
27/// {
28///    "latitude": 0.0,
29///    "longitude": 0.0,
30///    "altitude": 0.0
31/// }
32/// ```
33/// Note: latitude and longitude values will be in radians
34#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
35#[derive(PartialEq, Clone, Copy, Debug)]
36pub struct WGS84<N> {
37    // Represented as radians
38    #[cfg_attr(feature = "serde", serde(rename = "latitude"))]
39    lat: N,
40    // Represented as radians
41    #[cfg_attr(feature = "serde", serde(rename = "longitude"))]
42    lon: N,
43    #[cfg_attr(feature = "serde", serde(rename = "altitude"))]
44    alt: N,
45}
46
47impl<N: RealFieldCopy> WGS84<N>
48where
49    f64: From<N>,
50{
51    /// Create a new WGS84 position
52    ///
53    /// # Arguments
54    /// - `latitude` in degrees
55    /// - `longitude` in degrees
56    /// - `altitude` in meters
57    ///
58    /// # Panics
59    /// This will panic if `latitude` or `longitude` are not defined on the
60    /// WGS84 ellipsoid.
61    pub fn from_degrees_and_meters(latitude: N, longitude: N, altitude: N) -> WGS84<N> {
62        assert!(
63            latitude.abs() <= N::from_f64(90.0).unwrap(),
64            "Latitude must be in the range [-90, 90]"
65        );
66        assert!(
67            longitude.abs() <= N::from_f64(180.0).unwrap(),
68            "Longitude must be in the range [-180, 180]"
69        );
70        WGS84 {
71            lat: N::from_f64(f64::from(latitude).to_radians()).unwrap(),
72            lon: N::from_f64(f64::from(longitude).to_radians()).unwrap(),
73            alt: altitude,
74        }
75    }
76
77    /// Try to create a new WGS84 position
78    ///
79    /// # Arguments
80    /// - `latitude` in degrees
81    /// - `longitude` in degrees
82    /// - `altitude` in meters
83    pub fn try_from_degrees_and_meters(latitude: N, longitude: N, altitude: N) -> Option<WGS84<N>> {
84        if latitude.abs() <= N::from_f64(90.0).unwrap()
85            && longitude.abs() <= N::from_f64(180.0).unwrap()
86        {
87            Some(WGS84::from_degrees_and_meters(
88                latitude, longitude, altitude,
89            ))
90        } else {
91            None
92        }
93    }
94
95    /// Create a new WGS84 position
96    ///
97    /// # Arguments
98    /// - `latitude` in radians
99    /// - `longitude` in radians
100    /// - `altitude` in meters
101    ///
102    /// # Panics
103    /// This will panic if `latitude` or `longitude` are not defined on the
104    /// WGS84 ellipsoid.
105    pub fn from_radians_and_meters(latitude: N, longitude: N, altitude: N) -> WGS84<N> {
106        assert!(
107            latitude.abs() <= N::from_f64(std::f64::consts::FRAC_PI_2).unwrap(),
108            "Latitude must be in the range [-π/2, π/2]"
109        );
110        assert!(
111            longitude.abs() <= N::from_f64(std::f64::consts::PI).unwrap(),
112            "Longitude must be in the range [-π, π]"
113        );
114        WGS84 {
115            lat: latitude,
116            lon: longitude,
117            alt: altitude,
118        }
119    }
120
121    /// Try to create a new WGS84 position
122    ///
123    /// # Arguments
124    /// - `latitude` in radians
125    /// - `longitude` in radians
126    /// - `altitude` in meters
127    pub fn try_from_radians_and_meters(latitude: N, longitude: N, altitude: N) -> Option<WGS84<N>> {
128        if latitude.abs() <= N::from_f64(std::f64::consts::FRAC_PI_2).unwrap()
129            && longitude.abs() <= N::from_f64(std::f64::consts::PI).unwrap()
130        {
131            Some(WGS84 {
132                lat: latitude,
133                lon: longitude,
134                alt: altitude,
135            })
136        } else {
137            None
138        }
139    }
140
141    /// Get latitude of position, in degrees
142    pub fn latitude_degrees(&self) -> N {
143        N::from_f64(f64::from(self.lat).to_degrees()).unwrap()
144    }
145
146    /// Get longitude of position, in degrees
147    pub fn longitude_degrees(&self) -> N {
148        N::from_f64(f64::from(self.lon).to_degrees()).unwrap()
149    }
150
151    /// Distance between two WGS84 positions
152    ///
153    /// This function uses the haversin formula to calculate the distance
154    /// between two positions. For more control convert to ECEF and calculate
155    /// the difference.
156    ///
157    /// # Examples
158    /// ```rust
159    /// use nav_types::WGS84;
160    ///
161    /// let oslo = WGS84::from_degrees_and_meters(59.95, 10.75, 0.0);
162    /// let stockholm = WGS84::from_degrees_and_meters(59.329444, 18.068611, 0.0);
163    ///
164    /// println!("Great circle distance between Oslo and Stockholm: {:?}",
165    ///     oslo.distance(&stockholm));
166    /// ```
167    pub fn distance(&self, other: &WGS84<N>) -> N {
168        let delta_lat = other.latitude_radians() - self.latitude_radians();
169        let delta_lon = other.longitude_radians() - self.longitude_radians();
170
171        let a = (delta_lat / N::from_f64(2.0).unwrap()).sin().powi(2)
172            + self.latitude_radians().cos()
173                * other.latitude_radians().cos()
174                * (delta_lon / N::from_f64(2.0).unwrap()).sin().powi(2);
175        let c = N::from_f64(2.0).unwrap() * a.sqrt().asin();
176
177        N::from_f64(SEMI_MAJOR_AXIS).unwrap() * c + (self.altitude() - other.altitude()).abs()
178    }
179}
180
181impl<N: Copy> WGS84<N> {
182    /// Get altitude of position, in meters
183    pub fn altitude(&self) -> N {
184        self.alt
185    }
186    /// Get latitude of position, in radians
187    pub fn latitude_radians(&self) -> N {
188        self.lat
189    }
190    /// Get longitude of position, in radians
191    pub fn longitude_radians(&self) -> N {
192        self.lon
193    }
194}
195
196impl<N: RealFieldCopy> From<NVector<N>> for WGS84<N> {
197    fn from(f: NVector<N>) -> WGS84<N> {
198        // This implementation defines the ECEF coordinate system to have the Z
199        // axes point directly north, this affects the way which N-vectors are
200        // defined. See: Table 2 in Gade(2010).
201        // NOTE: This is consistent with the ECEF implementation in this crate
202        let x = f.vector().z;
203        let y = f.vector().y;
204        let z = -f.vector().x;
205
206        // See equation (5) in Gade(2010)
207        let longitude = y.atan2(-z);
208        // Equatorial component, see equation (6) Gade(2010)
209        let equat = (y.powi(2) + z.powi(2)).sqrt();
210        // See equation (6) Gade(2010)
211        let latitude = x.atan2(equat);
212
213        WGS84 {
214            lat: latitude,
215            lon: longitude,
216            alt: f.altitude(),
217        }
218    }
219}
220
221impl<N: RealFieldCopy> From<ECEF<N>> for WGS84<N> {
222    #![allow(clippy::many_single_char_names)]
223    fn from(ecef: ECEF<N>) -> WGS84<N> {
224        // https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#The_application_of_Ferrari's_solution
225        let a = N::from_f64(SEMI_MAJOR_AXIS).unwrap();
226        let b = N::from_f64(SEMI_MINOR_AXIS).unwrap();
227        let r_squared = (ecef.x() * ecef.x()) + (ecef.y() * ecef.y());
228        let r = r_squared.sqrt();
229        let z_squared = ecef.z() * ecef.z();
230        let z = z_squared.sqrt();
231        let a_squared = a * a;
232        let b_squared = b * b;
233        let e_squared = N::one() - (b_squared / a_squared);
234        let e_dot_squared = (a_squared - b_squared) / b_squared;
235        let f = N::from_f64(54.0).unwrap() * b_squared * z_squared;
236        let g = r_squared + ((N::one() - e_squared) * z_squared)
237            - (e_squared * (a_squared - b_squared));
238        let g_squared = g * g;
239        let c = (e_squared * e_squared * f * r_squared) / (g * g_squared);
240        let s = (N::one() + c + ((c * c) + c + c).sqrt()).cbrt();
241        let s_plus_one_over_s_plus_one = s + (N::one() / s) + N::one();
242        let p = f
243            / (N::from_f64(3.0).unwrap()
244                * s_plus_one_over_s_plus_one
245                * s_plus_one_over_s_plus_one
246                * g_squared);
247        let q = (N::one() + (N::from_f64(2.0).unwrap() * e_squared * e_squared * p)).sqrt();
248        let r_0 = (-(p * e_squared * r) / (N::one() + q))
249            + (a_squared / N::from_f64(2.0).unwrap() * (N::one() + (N::one() / q))
250                - ((p * (N::one() - e_squared) * z_squared) / (q * (N::one() + q)))
251                - (p * r_squared / N::from_f64(2.0).unwrap()))
252            .sqrt();
253        let r_minus_e_squared_r0 = r - (e_squared * r_0);
254        let r_minus_e_squared_r0_squared = r_minus_e_squared_r0 * r_minus_e_squared_r0;
255        let u = (r_minus_e_squared_r0_squared + z_squared).sqrt();
256        let v = (r_minus_e_squared_r0_squared + ((N::one() - e_squared) * z_squared)).sqrt();
257        let z_0 = (b_squared * z) / (a * v);
258        let h = u * (N::one() - (b_squared / (a * v)));
259        let phi = ecef.z().signum() * ((z + (e_dot_squared * z_0)) / r).atan().abs();
260        let lambda = ecef.y().atan2(ecef.x());
261        WGS84 {
262            lat: phi,
263            lon: lambda,
264            alt: h,
265        }
266    }
267}
268
269#[cfg(test)]
270impl Arbitrary for WGS84<f64> {
271    fn arbitrary<G: Gen>(g: &mut G) -> WGS84<f64> {
272        use rand::Rng;
273        let lat = g.gen_range(-90.0, 90.0);
274        let lon = g.gen_range(-180.0, 180.0);
275        // NOTE: Minimum altitude is radius of the earth, however, due to
276        // float precision we have shrunk that down a bit, the values
277        // below might still cause problems
278        let alt = g.gen_range(-6300000.0, 10000000.0);
279
280        WGS84::from_degrees_and_meters(lat, lon, alt)
281    }
282}
283
284#[cfg(test)]
285mod tests {
286    use super::*;
287    use crate::enu::ENU;
288    use assert::close;
289    use quickcheck::{quickcheck, TestResult};
290
291    #[test]
292    #[cfg_attr(not(feature = "serde"), ignore)]
293    fn test_ser_de() {
294        #[cfg(feature = "serde")]
295        {
296            use serde_test::{assert_tokens, Token};
297            let oslo: WGS84<f64> = WGS84 {
298                lat: 1.0463,
299                lon: 0.1876,
300                alt: 0.0,
301            };
302            assert_tokens(
303                &oslo,
304                &[
305                    Token::Struct {
306                        name: "WGS84",
307                        len: 3,
308                    },
309                    Token::Str("latitude"),
310                    Token::F64(1.0463),
311                    Token::Str("longitude"),
312                    Token::F64(0.1876),
313                    Token::Str("altitude"),
314                    Token::F64(0.0),
315                    Token::StructEnd,
316                ],
317            );
318        }
319        #[cfg(not(feature = "serde"))]
320        {
321            panic!("This test requires the serde feature to be enabled");
322        }
323    }
324
325    fn create_wgs84(latitude: f32, longitude: f32, altitude: f32) -> TestResult {
326        // This function is used to check that illegal latitude and longitude
327        // values panics
328        if latitude.abs() <= 90.0 && longitude.abs() <= 180.0 {
329            // If both latitude and longitude are within acceptable ranges
330            // we tell quickcheck to discard the test so that it will
331            // re-generate a test with different parameters hopefully
332            // testing other parameters which will fail
333            TestResult::discard()
334        } else {
335            // If either latitude or longitude is outside acceptable range
336            // the test must fail
337            TestResult::must_fail(move || {
338                WGS84::from_degrees_and_meters(latitude, longitude, altitude);
339            })
340        }
341    }
342
343    #[test]
344    fn test_create_wgs84() {
345        quickcheck(create_wgs84 as fn(f32, f32, f32) -> TestResult);
346    }
347
348    #[test]
349    fn dateline() {
350        // This test ensures that when moving west from the dateline
351        // the longitude resets to 180
352        let a = WGS84::from_degrees_and_meters(20.0, 180.0, 0.0);
353        let vec = ENU::new(-10.0, 0.0, 0.0);
354
355        let ans = a + vec;
356        // NOTE: Precision here is rather arbitrary and depends on the
357        // length of the vector used and so on, we are mostly interested
358        // in seeing that the longitude is reset to positive
359        close(ans.longitude_degrees(), 180.0, 0.001);
360        close(ans.latitude_degrees(), 20.0, 0.001);
361    }
362
363    #[test]
364    fn conversion_inversion_ecef() {
365        let oslo: WGS84<f64> = WGS84::from_degrees_and_meters(59.95, 10.75, 0.0);
366        let stockholm: WGS84<f64> = WGS84::from_degrees_and_meters(59.329444, 18.068611, 0.0);
367
368        for &place in [oslo, stockholm].iter() {
369            let distance = place.distance(&WGS84::from(ECEF::from(place)));
370            close(distance, 0.0, 0.00000001);
371        }
372    }
373
374    #[test]
375    fn conversion_ecef() {
376        let oslo_wgs84: WGS84<f64> = WGS84::from_degrees_and_meters(59.95, 10.75, 0.0);
377        let oslo_ecef: ECEF<f64> = ECEF::new(3145735.0, 597236.0, 5497690.0);
378
379        for &(place_wgs84, place_ecef) in [(oslo_wgs84, oslo_ecef)].iter() {
380            let distance = place_wgs84.distance(&WGS84::from(place_ecef));
381            close(distance, 0.0, 1.0);
382        }
383    }
384
385    #[test]
386    fn add_enu() {
387        let oslo: WGS84<f64> = WGS84::from_degrees_and_meters(59.95, 10.75, 0.0);
388        let oslo_high: WGS84<f64> = WGS84::from_degrees_and_meters(59.95, 10.75, 100.0);
389
390        let stockholm: WGS84<f64> = WGS84::from_degrees_and_meters(59.329444, 18.068611, 0.0);
391        let stockholm_high: WGS84<f64> =
392            WGS84::from_degrees_and_meters(59.329444, 18.068611, 100.0);
393
394        for &(place, place_high) in [(oslo, oslo_high), (stockholm, stockholm_high)].iter() {
395            let distance =
396                ECEF::from(place_high).distance(&(ECEF::from(place) + ENU::new(0.0, 0.0, 100.0)));
397            close(distance, 0.0, 0.00001);
398        }
399    }
400
401    quickcheck! {
402        fn distance_haversin(a: WGS84<f64>, b: WGS84<f64>) -> () {
403            // Trivially true:
404            close(a.distance(&b), b.distance(&a), 0.0000001);
405        }
406    }
407}
nav_types/wgs84.rs

nav_types/
wgs84.rs
