Merge #438

438: fix vincenty for equatorial and coincident points r=urschrei a=michaelkirk

fixes #418 

From Algorithms for geodesics, Charles F. F. Karney∗
available https://arxiv.org/pdf/1109.4448.pdf

> For equatorial geodesics (φ1 = 0 and α1 = 1 2π), Eq. (11) is
> indeterminate; in this case, take σ1 = 0

Also fixes vincenty for coincident points.

Co-authored-by: Michael Kirk <michael.code@endoftheworl.de>

geo/src/algorithm/vincenty_distance.rs

@@ -92,14 +92,30 @@ where
                 + (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda)
                     * (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda))
                 .sqrt();
+
             if sinSigma.is_zero() {
-                return Err(FailedToConvergeError);
+                if self == rhs {
+                    // coincident points
+                    return Ok(T::zero());
+                } else {
+                    // anitpodal points, for which vincenty does not converge
+                    return Err(FailedToConvergeError);
+                }
             }
+
             cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
             sigma = sinSigma.atan2(cosSigma);
             let sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma;
             cosSqAlpha = t_1 - sinAlpha * sinAlpha;
-            cos2SigmaM = cosSigma - t_2 * sinU1 * sinU2 / cosSqAlpha;
+
+            if cosSqAlpha.is_zero() {
+                // equatorial geodesics require special handling
+                // per [Algorithms for geodesics, Charles F. F. Karney](https://arxiv.org/pdf/1109.4448.pdf)
+                cos2SigmaM = T::zero()
+            } else {
+                cos2SigmaM = cosSigma - t_2 * sinU1 * sinU2 / cosSqAlpha;
+            }
+
             let C = f / t_16 * cosSqAlpha * (t_4 + f * (t_4 - t_3 * cosSqAlpha));
             lambdaP = lambda;
             lambda = L
@@ -145,7 +161,7 @@ where
     }
 }
 
-#[derive(Debug)]
+#[derive(Eq, PartialEq, Debug)]
 pub struct FailedToConvergeError;
 
 impl fmt::Display for FailedToConvergeError {
@@ -196,4 +212,33 @@ mod test {
             epsilon = 1.0e-6
         );
     }
+
+    #[test]
+    fn test_vincenty_distance_equatorial() {
+        let a = Point::<f64>::new(0.0, 0.0);
+        let b = Point::<f64>::new(100.0, 0.0);
+        assert_relative_eq!(
+            a.vincenty_distance(&b).unwrap(),
+            11131949.079,
+            epsilon = 1.0e-3
+        );
+    }
+
+    #[test]
+    fn test_vincenty_distance_coincident() {
+        let a = Point::<f64>::new(12.3, 4.56);
+        let b = Point::<f64>::new(12.3, 4.56);
+        assert_relative_eq!(
+            a.vincenty_distance(&b).unwrap(),
+            0.0,
+            epsilon = 1.0e-3
+        );
+    }
+
+    #[test]
+    fn test_vincenty_distance_antipodal() {
+        let a = Point::<f64>::new(2.0, 4.0);
+        let b = Point::<f64>::new(-178.0, -4.0);
+        assert_eq!(a.vincenty_distance(&b), Err(FailedToConvergeError))
+    }
 }