docs/attachments/LUCENE-7140/LUCENE-7140.patch

diff --git a/lucene/spatial3d/src/java/org/apache/lucene/spatial3d/geom/PlanetModel.java b/lucene/spatial3d/src/java/org/apache/lucene/spatial3d/geom/PlanetModel.java
index d45d776..9f53851 100644
--- a/lucene/spatial3d/src/java/org/apache/lucene/spatial3d/geom/PlanetModel.java
+++ b/lucene/spatial3d/src/java/org/apache/lucene/spatial3d/geom/PlanetModel.java
@@ -188,65 +188,95 @@ public class PlanetModel {
     return (x * x + y * y) * inverseAb * inverseAb + z * z * inverseC * inverseC - 1.0 > Vector.MINIMUM_RESOLUTION;
   }
   
+  /** Compute a GeoPoint that's a bisection between two other GeoPoints.
+   * @param pt1 is the first point.
+   * @param pt2 is the second point.
+   * @return the bisection point, or null if a unique one cannot be found.
+   */
+  public GeoPoint bisection(final GeoPoint pt1, final GeoPoint pt2) {
+    final double A0 = (pt1.x + pt2.x) * 0.5;
+    final double B0 = (pt1.y + pt2.y) * 0.5;
+    final double C0 = (pt1.z + pt2.z) * 0.5;
+      
+    final double denom = inverseAbSquared * A0 * A0 +
+      inverseAbSquared * B0 * B0 +
+      inverseCSquared * C0 * C0;
+          
+    if(denom < Vector.MINIMUM_RESOLUTION) {
+      // Bisection is undefined
+      return null;
+    }
+      
+    final double t = Math.sqrt(1.0 / denom);
+      
+    return new GeoPoint(t * A0, t * B0, t * C0);
+  }
+  
   /** Compute surface distance between two points.
-   * @param p1 is the first point.
-   * @param p2 is the second point.
+   * @param pt1 is the first point.
+   * @param pt2 is the second point.
    * @return the adjusted angle, when multiplied by the mean earth radius, yields a surface distance.  This will differ
    * from GeoPoint.arcDistance() only when the planet model is not a sphere. @see {@link GeoPoint#arcDistance(GeoPoint)}
    */
-  public double surfaceDistance(final GeoPoint p1, final GeoPoint p2) {
-    final double latA = p1.getLatitude();
-    final double lonA = p1.getLongitude();
-    final double latB = p2.getLatitude();
-    final double lonB = p2.getLongitude();
+  public double surfaceDistance(final GeoPoint pt1, final GeoPoint pt2) {
+    final double L = pt2.getLongitude() - pt1.getLongitude();
+    final double U1 = Math.atan((1.0-flattening) * Math.tan(pt1.getLatitude()));
+    final double U2 = Math.atan((1.0-flattening) * Math.tan(pt2.getLatitude()));
+
+    final double sinU1 = Math.sin(U1);
+    final double cosU1 = Math.cos(U1);
+    final double sinU2 = Math.sin(U2);
+    final double cosU2 = Math.cos(U2);
+
+    final double dCosU1CosU2 = cosU1 * cosU2;
+    final double dCosU1SinU2 = cosU1 * sinU2;
+
+    final double dSinU1SinU2 = sinU1 * sinU2;
+    final double dSinU1CosU2 = sinU1 * cosU2;
 
-    final double L = lonB - lonA;
-    final double oF = 1.0 - this.flattening;
-    final double U1 = Math.atan(oF * Math.tan(latA));
-    final double U2 = Math.atan(oF * Math.tan(latB));
-    final double sU1 = Math.sin(U1);
-    final double cU1 = Math.cos(U1);
-    final double sU2 = Math.sin(U2);
-    final double cU2 = Math.cos(U2);
 
-    double sigma, sinSigma, cosSigma;
-    double cos2Alpha, cos2SigmaM;
-    
     double lambda = L;
-    double iters = 100;
+    double lambdaP = Math.PI * 2.0;
+    int iterLimit = 0;
+    double cosSqAlpha;
+    double sinSigma;
+    double cos2SigmaM;
+    double cosSigma;
+    double sigma;
+    double sinAlpha;
+    double C;
+    double sinLambda, cosLambda;
 
     do {
-      final double sinLambda = Math.sin(lambda);
-      final double cosLambda = Math.cos(lambda);
-      sinSigma = Math.sqrt((cU2 * sinLambda) * (cU2 * sinLambda) + (cU1 * sU2 - sU1 * cU2 * cosLambda)
-          * (cU1 * sU2 - sU1 * cU2 * cosLambda));
-      if (Math.abs(sinSigma) < Vector.MINIMUM_RESOLUTION)
-        return 0.0;
+      sinLambda = Math.sin(lambda);
+      cosLambda = Math.cos(lambda);
+      sinSigma = Math.sqrt((cosU2*sinLambda) * (cosU2*sinLambda) +
+                                    (dCosU1SinU2 - dSinU1CosU2 * cosLambda) * (dCosU1SinU2 - dSinU1CosU2 * cosLambda));
 
-      cosSigma = sU1 * sU2 + cU1 * cU2 * cosLambda;
+      if (sinSigma==0.0) {
+        return 0.0;
+      }
+      cosSigma = dSinU1SinU2 + dCosU1CosU2 * cosLambda;
       sigma = Math.atan2(sinSigma, cosSigma);
-      final double sinAlpha = cU1 * cU2 * sinLambda / sinSigma;
-      cos2Alpha = 1.0 - sinAlpha * sinAlpha;
-      cos2SigmaM = cosSigma - 2.0 * sU1 * sU2 / cos2Alpha;
-
-      final double c = this.flattening * 0.625 * cos2Alpha * (4.0 + this.flattening * (4.0 - 3.0 * cos2Alpha));
-      final double lambdaP = lambda;
-      lambda = L + (1.0 - c) * this.flattening * sinAlpha * (sigma + c * sinSigma * (cos2SigmaM + c * cosSigma *
-          (-1.0 + 2.0 * cos2SigmaM * cos2SigmaM)));
-      if (Math.abs(lambda - lambdaP) < Vector.MINIMUM_RESOLUTION)
-        break;
-    } while (--iters > 0);
+      sinAlpha = dCosU1CosU2 * sinLambda / sinSigma;
+      cosSqAlpha = 1.0 - sinAlpha * sinAlpha;
+      cos2SigmaM = cosSigma - 2.0 * dSinU1SinU2 / cosSqAlpha;
 
-    if (iters == 0)
-      return 0.0;
+      if (Double.isNaN(cos2SigmaM))
+        cos2SigmaM = 0.0;  // equatorial line: cosSqAlpha=0
+      C = flattening / 16.0 * cosSqAlpha * (4.0 + flattening * (4.0 - 3.0 * cosSqAlpha));
+      lambdaP = lambda;
+      lambda = L + (1.0 - C) * flattening * sinAlpha *
+        (sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1.0 + 2.0 * cos2SigmaM *cos2SigmaM)));
+    } while (Math.abs(lambda-lambdaP) > Vector.MINIMUM_RESOLUTION && ++iterLimit < 40);
 
-    final double uSq = cos2Alpha * this.squareRatio;
-    final double A = 1.0 + uSq * 0.00006103515625 * (4096.0 + uSq * (-768.0 + uSq * (320.0 - 175.0 * uSq)));
-    final double B = uSq * 0.0009765625 * (256.0 + uSq * (-128.0 + uSq * (74.0 - 47.0 * uSq)));
-    final double deltaSigma = B * sinSigma * (cos2SigmaM + B * 0.25 * (cosSigma * (-1.0 + 2.0 * cos2SigmaM * cos2SigmaM) - B * 0.16666666666666666666667 * cos2SigmaM
-            * (-3.0 + 4.0 * sinSigma * sinSigma) * (-3.0 + 4.0 * cos2SigmaM * cos2SigmaM)));
+    final double uSq = cosSqAlpha * this.squareRatio;
+    final double A = 1.0 + uSq / 16384.0 * (4096.0 + uSq * (-768.0 + uSq * (320.0 - 175.0 * uSq)));
+    final double B = uSq / 1024.0 * (256.0 + uSq * (-128.0 + uSq * (74.0 - 47.0 * uSq)));
+    final double deltaSigma = B * sinSigma * (cos2SigmaM + B / 4.0 * (cosSigma * (-1.0 + 2.0 * cos2SigmaM * cos2SigmaM)-
+                                        B / 6.0 * cos2SigmaM * (-3.0 + 4.0 * sinSigma * sinSigma) * (-3.0 + 4.0 * cos2SigmaM * cos2SigmaM)));
 
-    return this.c * A * (sigma - deltaSigma);
+    return c * A * (sigma - deltaSigma);
   }
 
   @Override
diff --git a/lucene/spatial3d/src/test/org/apache/lucene/spatial3d/geom/GeoPointTest.java b/lucene/spatial3d/src/test/org/apache/lucene/spatial3d/geom/GeoPointTest.java
index ed17928..ea12c40 100644
--- a/lucene/spatial3d/src/test/org/apache/lucene/spatial3d/geom/GeoPointTest.java
+++ b/lucene/spatial3d/src/test/org/apache/lucene/spatial3d/geom/GeoPointTest.java
@@ -68,8 +68,40 @@ public class GeoPointTest extends LuceneTestCase {
       final double surfaceDistance = PlanetModel.SPHERE.surfaceDistance(p1,p2);
       assertEquals(arcDistance, surfaceDistance, 1e-6);
     }
+    // Now try some WGS84 points (taken randomly and compared against a known-good implementation)
+    assertEquals(1.1444648695765323, PlanetModel.WGS84.surfaceDistance(
+      new GeoPoint(PlanetModel.WGS84, 0.038203808753702884, -0.6701260455506466),
+      new GeoPoint(PlanetModel.WGS84, -0.8453720422675458, 0.1737353153814496)), 1e-6);
+    assertEquals(1.4345148695890722, PlanetModel.WGS84.surfaceDistance(
+      new GeoPoint(PlanetModel.WGS84, 0.5220926323378574, 0.6758041581907408),
+      new GeoPoint(PlanetModel.WGS84, -0.8453720422675458, 0.1737353153814496)), 1e-6);
+    assertEquals(2.32418144616446, PlanetModel.WGS84.surfaceDistance(
+      new GeoPoint(PlanetModel.WGS84, 0.09541335760967473, 1.2091829760623236),
+      new GeoPoint(PlanetModel.WGS84, -0.8501591797459979, -2.3044806381627594)), 1e-6);
+    assertEquals(2.018421047005435, PlanetModel.WGS84.surfaceDistance(
+      new GeoPoint(PlanetModel.WGS84, 0.3402853531962009, -0.43544195327249957),
+      new GeoPoint(PlanetModel.WGS84, -0.8501591797459979, -2.3044806381627594)), 1e-6);
   }
 
+  @Test
+  public void testBisection() {
+    final int times = atLeast(100);
+    for (int i = 0; i < times; i++) {
+      final double p1Lat = (randomFloat() * 180.0 - 90.0) * DEGREES_TO_RADIANS;
+      final double p1Lon = (randomFloat() * 360.0 - 180.0) * DEGREES_TO_RADIANS;
+      final double p2Lat = (randomFloat() * 180.0 - 90.0) * DEGREES_TO_RADIANS;
+      final double p2Lon = (randomFloat() * 360.0 - 180.0) * DEGREES_TO_RADIANS;
+      final GeoPoint p1 = new GeoPoint(PlanetModel.WGS84, p1Lat, p1Lon);
+      final GeoPoint p2 = new GeoPoint(PlanetModel.WGS84, p2Lat, p2Lon);
+      final GeoPoint pMid = PlanetModel.WGS84.bisection(p1, p2);
+      if (pMid != null) {
+        final double arcDistance = p1.arcDistance(p2);
+        final double sum = pMid.arcDistance(p1) + pMid.arcDistance(p2);
+        assertEquals(arcDistance, sum, 1e-6);
+      }
+    }
+  }
+  
   @Test(expected = IllegalArgumentException.class)
   public void testBadLatLon() {
     new GeoPoint(PlanetModel.SPHERE, 50.0, 32.2);