geo/algorithm/
vincenty_distance.rs

1// A few resources:
2//
3// - http://www.movable-type.co.uk/scripts/latlong-vincenty.html
4// - https://nathanrooy.github.io/posts/2016-12-18/vincenty-formula-with-python/
5// - https://github.com/janantala/GPS-distance/blob/master/java/Distance.java
6
7use crate::{CoordFloat, Point, EARTH_FLATTENING, EQUATORIAL_EARTH_RADIUS, POLAR_EARTH_RADIUS};
8use num_traits::FromPrimitive;
9use std::{error, fmt};
10
11/// Determine the distance between two geometries using [Vincenty’s formulae].
12///
13/// [Vincenty’s formulae]: https://en.wikipedia.org/wiki/Vincenty%27s_formulae
14pub trait VincentyDistance<T, Rhs = Self> {
15    /// Determine the distance between two geometries using [Vincenty’s
16    /// formulae].
17    ///
18    /// # Units
19    ///
20    /// - return value: meters
21    ///
22    /// # Examples
23    ///
24    /// ```
25    /// use geo::prelude::*;
26    /// use geo::point;
27    ///
28    /// // New York City
29    /// let p1 = point!(x: -74.006f64, y: 40.7128f64);
30    ///
31    /// // London
32    /// let p2 = point!(x: -0.1278f64, y: 51.5074f64);
33    ///
34    /// let distance = p1.vincenty_distance(&p2).unwrap();
35    ///
36    /// assert_eq!(
37    ///     5_585_234., // meters
38    ///     distance.round()
39    /// );
40    /// ```
41    ///
42    /// [Vincenty’s formulae]: https://en.wikipedia.org/wiki/Vincenty%27s_formulae
43    fn vincenty_distance(&self, rhs: &Rhs) -> Result<T, FailedToConvergeError>;
44}
45
46impl<T> VincentyDistance<T, Point<T>> for Point<T>
47where
48    T: CoordFloat + FromPrimitive,
49{
50    #[allow(non_snake_case)]
51    fn vincenty_distance(&self, rhs: &Point<T>) -> Result<T, FailedToConvergeError> {
52        let t_1 = T::one();
53        let t_2 = T::from(2).unwrap();
54        let t_3 = T::from(3).unwrap();
55        let t_4 = T::from(4).unwrap();
56        let t_6 = T::from(6).unwrap();
57        let t_47 = T::from(47).unwrap();
58        let t_16 = T::from(16).unwrap();
59        let t_74 = T::from(74).unwrap();
60        let t_128 = T::from(128).unwrap();
61        let t_175 = T::from(175).unwrap();
62        let t_256 = T::from(256).unwrap();
63        let t_320 = T::from(320).unwrap();
64        let t_768 = T::from(768).unwrap();
65        let t_1024 = T::from(1024).unwrap();
66        let t_4096 = T::from(4096).unwrap();
67        let t_16384 = T::from(16384).unwrap();
68
69        let a = T::from(EQUATORIAL_EARTH_RADIUS).unwrap();
70        let b = T::from(POLAR_EARTH_RADIUS).unwrap();
71        let f = T::from(EARTH_FLATTENING).unwrap();
72        // Difference in longitude
73        let L = (rhs.x() - self.x()).to_radians();
74        // Reduced latitude (latitude on the auxiliary sphere)
75        let U1 = ((t_1 - f) * self.y().to_radians().tan()).atan();
76        // Reduced latitude (latitude on the auxiliary sphere)
77        let U2 = ((t_1 - f) * rhs.y().to_radians().tan()).atan();
78        let (sinU1, cosU1) = U1.sin_cos();
79        let (sinU2, cosU2) = U2.sin_cos();
80        let mut cosSqAlpha;
81        let mut sinSigma;
82        let mut cos2SigmaM;
83        let mut cosSigma;
84        let mut sigma;
85        // Longitude of the points on the auxiliary sphere
86        let mut lambda = L;
87        let mut lambdaP;
88        let mut iterLimit = 100;
89
90        loop {
91            let (sinLambda, cosLambda) = lambda.sin_cos();
92            sinSigma = ((cosU2 * sinLambda) * (cosU2 * sinLambda)
93                + (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda)
94                    * (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda))
95                .sqrt();
96
97            if sinSigma.is_zero() {
98                return if self == rhs {
99                    // coincident points
100                    Ok(T::zero())
101                } else {
102                    // antipodal points, for which vincenty does not converge
103                    Err(FailedToConvergeError)
104                };
105            }
106
107            cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
108            sigma = sinSigma.atan2(cosSigma);
109            let sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma;
110            cosSqAlpha = t_1 - sinAlpha * sinAlpha;
111
112            if cosSqAlpha.is_zero() {
113                // equatorial geodesics require special handling
114                // per [Algorithms for geodesics, Charles F. F. Karney](https://arxiv.org/pdf/1109.4448.pdf)
115                cos2SigmaM = T::zero()
116            } else {
117                cos2SigmaM = cosSigma - t_2 * sinU1 * sinU2 / cosSqAlpha;
118            }
119
120            let C = f / t_16 * cosSqAlpha * (t_4 + f * (t_4 - t_3 * cosSqAlpha));
121            lambdaP = lambda;
122            lambda = L
123                + (t_1 - C)
124                    * f
125                    * sinAlpha
126                    * (sigma
127                        + C * sinSigma
128                            * (cos2SigmaM + C * cosSigma * (-t_1 + t_2 * cos2SigmaM * cos2SigmaM)));
129
130            if (lambda - lambdaP).abs() <= T::from(1e-12).unwrap() {
131                break;
132            }
133
134            iterLimit -= 1;
135
136            if iterLimit == 0 {
137                break;
138            }
139        }
140
141        if iterLimit == 0 {
142            return Err(FailedToConvergeError);
143        }
144
145        let uSq = cosSqAlpha * (a * a - b * b) / (b * b);
146        let A = t_1 + uSq / t_16384 * (t_4096 + uSq * (-t_768 + uSq * (t_320 - t_175 * uSq)));
147        let B = uSq / t_1024 * (t_256 + uSq * (-t_128 + uSq * (t_74 - t_47 * uSq)));
148
149        let deltaSigma = B
150            * sinSigma
151            * (cos2SigmaM
152                + B / t_4
153                    * (cosSigma * (-t_1 + t_2 * cos2SigmaM * cos2SigmaM)
154                        - B / t_6
155                            * cos2SigmaM
156                            * (-t_3 + t_4 * sinSigma * sinSigma)
157                            * (-t_3 + t_4 * cos2SigmaM * cos2SigmaM)));
158
159        let s = b * A * (sigma - deltaSigma);
160
161        Ok(s)
162    }
163}
164
165#[derive(Eq, PartialEq, Debug)]
166pub struct FailedToConvergeError;
167
168impl fmt::Display for FailedToConvergeError {
169    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
170        write!(f, "Vincenty algorithm failed to converge")
171    }
172}
173
174impl error::Error for FailedToConvergeError {
175    fn description(&self) -> &str {
176        "Vincenty algorithm failed to converge"
177    }
178}
179
180#[cfg(test)]
181mod test {
182    use super::*;
183
184    #[test]
185    fn test_vincenty_distance_1() {
186        let a = Point::new(17.072561, 48.154563);
187        let b = Point::new(17.072562, 48.154564);
188        assert_relative_eq!(
189            a.vincenty_distance(&b).unwrap(),
190            0.13378944117648012,
191            epsilon = 1.0e-6
192        );
193    }
194
195    #[test]
196    fn test_vincenty_distance_2() {
197        let a = Point::new(17.072561, 48.154563);
198        let b = Point::new(17.064064, 48.158800);
199        assert_relative_eq!(
200            a.vincenty_distance(&b).unwrap(),
201            788.4148295236967,
202            epsilon = 1.0e-6
203        );
204    }
205
206    #[test]
207    fn test_vincenty_distance_3() {
208        let a = Point::new(17.107558, 48.148636);
209        let b = Point::new(16.372477, 48.208810);
210        assert_relative_eq!(
211            a.vincenty_distance(&b).unwrap(),
212            55073.68246366003,
213            epsilon = 1.0e-6
214        );
215    }
216
217    #[test]
218    fn test_vincenty_distance_equatorial() {
219        let a = Point::new(0.0, 0.0);
220        let b = Point::new(100.0, 0.0);
221        assert_relative_eq!(
222            a.vincenty_distance(&b).unwrap(),
223            11131949.079,
224            epsilon = 1.0e-3
225        );
226    }
227
228    #[test]
229    fn test_vincenty_distance_coincident() {
230        let a = Point::new(12.3, 4.56);
231        let b = Point::new(12.3, 4.56);
232        assert_relative_eq!(a.vincenty_distance(&b).unwrap(), 0.0, epsilon = 1.0e-3);
233    }
234
235    #[test]
236    fn test_vincenty_distance_antipodal() {
237        let a = Point::new(2.0, 4.0);
238        let b = Point::new(-178.0, -4.0);
239        assert_eq!(a.vincenty_distance(&b), Err(FailedToConvergeError))
240    }
241}
geo/algorithm/vincenty_distance.rs

geo/algorithm/
vincenty_distance.rs
