[H3] Compute destination point from distance and azimuth using planar 3d math (#93084)

iverase · web-flow · commit 4224026fb21b · 2023-01-23T13:30:19.000+01:00
Replace the method geoAzDistanceRads from using trigonometric maths to use planar 3d math
diff --git a/docs/changelog/93084.yaml b/docs/changelog/93084.yaml
@@ -0,0 +1,6 @@
+pr: 93084
+summary: "[H3] Compute destination point from distance and azimuth using planar 3d\
+  \ math"
+area: Geo
+type: enhancement
+issues: []
diff --git a/libs/h3/src/main/java/org/elasticsearch/h3/Constants.java b/libs/h3/src/main/java/org/elasticsearch/h3/Constants.java
@@ -30,6 +30,10 @@ final class Constants {
      * sqrt(3) / 2.0
      */
     public static double M_SQRT3_2 = 0.8660254037844386467637231707529361834714;
+    /**
+     * 2.0 * PI
+     */
+    public static final double M_2PI = 6.28318530717958647692528676655900576839433;
     /**
      * max H3 resolution; H3 version 1 has 16 resolutions, numbered 0 through 15
      */
diff --git a/libs/h3/src/main/java/org/elasticsearch/h3/LatLng.java b/libs/h3/src/main/java/org/elasticsearch/h3/LatLng.java
@@ -28,7 +28,12 @@
 public final class LatLng {
 
     /** Minimum Angular resolution. */
-    public static final double MINIMUM_ANGULAR_RESOLUTION = Math.PI * 1.0e-12; // taken from lucene's spatial3d
+    private static final double MINIMUM_ANGULAR_RESOLUTION = Math.PI * 1.0e-12; // taken from lucene's spatial3d
+
+    /**
+     * pi / 2.0
+     */
+    private static final double M_PI_2 = 1.5707963267948966;
 
     // lat / lon in radians
     private final double lon;
@@ -67,13 +72,59 @@ public double getLonDeg() {
      * @return The azimuth in radians.
      */
     double geoAzimuthRads(double lat, double lon) {
+        // algorithm from the original H3 library
         final double cosLat = FastMath.cos(lat);
         return FastMath.atan2(
             cosLat * FastMath.sin(lon - this.lon),
             FastMath.cos(this.lat) * FastMath.sin(lat) - FastMath.sin(this.lat) * cosLat * FastMath.cos(lon - this.lon)
         );
     }
 
+    /**
+     * Computes the point on the sphere with a specified azimuth and distance from
+     * this point.
+     *
+     * @param az       The desired azimuth.
+     * @param distance The desired distance.
+     * @return The LatLng point.
+     */
+    LatLng geoAzDistanceRads(double az, double distance) {
+        // algorithm from the original H3 library
+        az = Vec2d.posAngleRads(az);
+        final double sinDistance = FastMath.sin(distance);
+        final double cosDistance = FastMath.cos(distance);
+        final double sinP1Lat = FastMath.sin(getLatRad());
+        final double cosP1Lat = FastMath.cos(getLatRad());
+        final double sinlat = Math.max(-1.0, Math.min(1.0, sinP1Lat * cosDistance + cosP1Lat * sinDistance * FastMath.cos(az)));
+        final double lat = FastMath.asin(sinlat);
+        if (Math.abs(lat - M_PI_2) < Constants.EPSILON) { // north pole
+            return new LatLng(M_PI_2, 0.0);
+        } else if (Math.abs(lat + M_PI_2) < Constants.EPSILON) { // south pole
+            return new LatLng(-M_PI_2, 0.0);
+        } else {
+            final double cosLat = FastMath.cos(lat);
+            final double sinlng = Math.max(-1.0, Math.min(1.0, FastMath.sin(az) * sinDistance / cosLat));
+            final double coslng = Math.max(-1.0, Math.min(1.0, (cosDistance - sinP1Lat * FastMath.sin(lat)) / cosP1Lat / cosLat));
+            return new LatLng(lat, constrainLng(getLonRad() + FastMath.atan2(sinlng, coslng)));
+        }
+    }
+
+    /**
+     * constrainLng makes sure longitudes are in the proper bounds
+     *
+     * @param lng The origin lng value
+     * @return The corrected lng value
+     */
+    private static double constrainLng(double lng) {
+        while (lng > Math.PI) {
+            lng = lng - Constants.M_2PI;
+        }
+        while (lng < -Math.PI) {
+            lng = lng + Constants.M_2PI;
+        }
+        return lng;
+    }
+
     /**
      * Determines the maximum latitude of the great circle defined by this LatLng to the provided LatLng.
      *
@@ -92,7 +143,7 @@ private static double greatCircleMaxLatitude(LatLng latLng1, LatLng latLng2) {
         assert latLng1.lat >= latLng2.lat;
         final double az = latLng1.geoAzimuthRads(latLng2.lat, latLng2.lon);
         // the great circle contains the maximum latitude only if the azimuth is between -90 and 90 degrees.
-        if (Math.abs(az) < Math.PI / 2) {
+        if (Math.abs(az) < M_PI_2) {
             return FastMath.acos(Math.abs(FastMath.sin(az) * FastMath.cos(latLng1.lat)));
         }
         return latLng1.lat;
@@ -116,14 +167,14 @@ private static double greatCircleMinLatitude(LatLng latLng1, LatLng latLng2) {
         // we compute the min latitude using Clairaut's formula (https://streckenflug.at/download/formeln.pdf)
         final double az = latLng1.geoAzimuthRads(latLng2.lat, latLng2.lon);
         // the great circle contains the minimum latitude only if the azimuth is not between -90 and 90 degrees.
-        if (Math.abs(az) > Math.PI / 2) {
+        if (Math.abs(az) > M_PI_2) {
             // note the sign
             return -FastMath.acos(Math.abs(FastMath.sin(az) * FastMath.cos(latLng1.lat)));
         }
         return latLng1.lat;
     }
 
-    private boolean isNumericallyIdentical(LatLng latLng) {
+    boolean isNumericallyIdentical(LatLng latLng) {
         return Math.abs(this.lat - latLng.lat) < MINIMUM_ANGULAR_RESOLUTION && Math.abs(this.lon - latLng.lon) < MINIMUM_ANGULAR_RESOLUTION;
     }
 
diff --git a/libs/h3/src/main/java/org/elasticsearch/h3/Vec2d.java b/libs/h3/src/main/java/org/elasticsearch/h3/Vec2d.java
@@ -87,19 +87,6 @@ final class Vec2d {
         { 2.361378999196363184, 0.266983896803167583, 4.455774101589558636 },  // face 19
     };
 
-    /**
-     * pi
-     */
-    private static final double M_PI = 3.14159265358979323846;
-    /**
-     * pi / 2.0
-     */
-    private static final double M_PI_2 = 1.5707963267948966;
-    /**
-     * 2.0 * PI
-     */
-    public static final double M_2PI = 6.28318530717958647692528676655900576839433;
-
     private final double x;  /// < x component
     private final double y;  /// < y component
 
@@ -155,7 +142,7 @@ public LatLng hex2dToGeo(int face, int res, boolean substrate) {
         // find theta as an azimuth
         theta = posAngleRads(faceAxesAzRadsCII[face][0] - theta);
         // now find the point at (r,theta) from the face center
-        return geoAzDistanceRads(faceCenterGeo[face], theta, r);
+        return Vec3d.faceCenterPoint[face].geoAzDistanceRads(theta, r);
     }
 
     /**
@@ -313,106 +300,11 @@ private double v2dMag() {
      */
     static double posAngleRads(double rads) {
         if (rads < 0.0) {
-            return rads + M_2PI;
-        } else if (rads >= M_2PI) {
-            return rads - M_2PI;
+            return rads + Constants.M_2PI;
+        } else if (rads >= Constants.M_2PI) {
+            return rads - Constants.M_2PI;
         } else {
             return rads;
         }
     }
-
-    /**
-     * Computes the point on the sphere a specified azimuth and distance from
-     * another point.
-     *
-     * @param p1       The first spherical coordinates.
-     * @param az       The desired azimuth from p1.
-     * @param distance The desired distance from p1, must be non-negative.
-     *                 p1.
-     */
-    private static LatLng geoAzDistanceRads(LatLng p1, double az, double distance) {
-        if (distance < Constants.EPSILON) {
-            return p1;
-        }
-
-        double sinlat, sinlng, coslng;
-
-        az = posAngleRads(az);
-
-        double lat, lon;
-
-        // check for due north/south azimuth
-        if (az < Constants.EPSILON || Math.abs(az - M_PI) < Constants.EPSILON) {
-            if (az < Constants.EPSILON) {// due north
-                lat = p1.getLatRad() + distance;
-            } else { // due south
-                lat = p1.getLatRad() - distance;
-            }
-            if (Math.abs(lat - M_PI_2) < Constants.EPSILON) { // north pole
-                lat = M_PI_2;
-                lon = 0.0;
-            } else if (Math.abs(lat + M_PI_2) < Constants.EPSILON) { // south pole
-                lat = -M_PI_2;
-                lon = 0.0;
-            } else {
-                lon = constrainLng(p1.getLonRad());
-            }
-        } else { // not due north or south
-            final double sinDistance = FastMath.sin(distance);
-            final double cosDistance = FastMath.cos(distance);
-            final double sinP1Lat = FastMath.sin(p1.getLatRad());
-            final double cosP1Lat = FastMath.cos(p1.getLatRad());
-            sinlat = sinP1Lat * cosDistance + cosP1Lat * sinDistance * FastMath.cos(az);
-            if (sinlat > 1.0) {
-                sinlat = 1.0;
-            }
-            if (sinlat < -1.0) {
-                sinlat = -1.0;
-            }
-            lat = FastMath.asin(sinlat);
-            if (Math.abs(lat - M_PI_2) < Constants.EPSILON)  // north pole
-            {
-                lat = M_PI_2;
-                lon = 0.0;
-            } else if (Math.abs(lat + M_PI_2) < Constants.EPSILON)  // south pole
-            {
-                lat = -M_PI_2;
-                lon = 0.0;
-            } else {
-                final double cosLat = FastMath.cos(lat);
-                sinlng = FastMath.sin(az) * sinDistance / cosLat;
-                coslng = (cosDistance - sinP1Lat * FastMath.sin(lat)) / cosP1Lat / cosLat;
-                if (sinlng > 1.0) {
-                    sinlng = 1.0;
-                }
-                if (sinlng < -1.0) {
-                    sinlng = -1.0;
-                }
-                if (coslng > 1.0) {
-                    coslng = 1.0;
-                }
-                if (coslng < -1.0) {
-                    coslng = -1.0;
-                }
-                lon = constrainLng(p1.getLonRad() + FastMath.atan2(sinlng, coslng));
-            }
-        }
-        return new LatLng(lat, lon);
-    }
-
-    /**
-     * constrainLng makes sure longitudes are in the proper bounds
-     *
-     * @param lng The origin lng value
-     * @return The corrected lng value
-     */
-    private static double constrainLng(double lng) {
-        while (lng > M_PI) {
-            lng = lng - (2 * M_PI);
-        }
-        while (lng < -M_PI) {
-            lng = lng + (2 * M_PI);
-        }
-        return lng;
-    }
 }
diff --git a/libs/h3/src/main/java/org/elasticsearch/h3/Vec3d.java b/libs/h3/src/main/java/org/elasticsearch/h3/Vec3d.java
@@ -167,6 +167,51 @@ private static double square(double x) {
         return FastMath.atan2(sign * magnitude(c1c2X, c1c2Y, c1c2Z), dotProduct(c1X, c1Y, c1Z, c2X, c2Y, c2Z));
     }
 
+    /**
+     * Computes the point on the sphere with a specified azimuth and distance from
+     * this point.
+     *
+     * @param az       The desired azimuth.
+     * @param distance The desired distance.
+     * @return The LatLng point.
+     */
+    LatLng geoAzDistanceRads(double az, double distance) {
+        az = Vec2d.posAngleRads(az);
+        // from https://www.movable-type.co.uk/scripts/latlong-vectors.html
+        // N = {0,0,1} – vector representing north pole
+        // d̂e = N×a – east vector at a
+        // dn = a×de – north vector at a
+        // d = dn·cosθ + de·sinθ – direction vector in dir’n of θ
+        // b = a·cosδ + d·sinδ
+
+        // east direction vector @ n1 (Gade's k_e_E)
+        final double magnitude = magnitude(this.x, this.y, 0);
+        final double deX = -this.y / magnitude;
+        final double deY = this.x / magnitude;
+
+        // north direction vector @ n1 (Gade's (k_n_E)
+        final double dnX = -this.z * deY;
+        final double dnY = this.z * deX;
+        final double dnZ = this.x * deY - this.y * deX;
+
+        final double sinAz = FastMath.sin(az);
+        final double cosAz = FastMath.cos(az);
+        final double sinDistance = FastMath.sin(distance);
+        final double cosDistance = FastMath.cos(distance);
+
+        // direction vector @ n1 (≡ C×n1; C = great circle)
+        final double dX = dnX * cosAz + deX * sinAz;
+        final double dY = dnY * cosAz + deY * sinAz;
+        final double dZ = dnZ * cosAz;
+
+        // Gade's n_EB_E = component of n2 parallel to n1 + component of n2 perpendicular to n1
+        final double n2X = this.x * cosDistance + dX * sinDistance;
+        final double n2Y = this.y * cosDistance + dY * sinDistance;
+        final double n2Z = this.z * cosDistance + dZ * sinDistance;
+
+        return new LatLng(FastMath.asin(n2Z), FastMath.atan2(n2Y, n2X));
+    }
+
     /**
      * Calculate the dot product between two 3D coordinates.
      *
diff --git a/libs/h3/src/test/java/org/elasticsearch/h3/LatLngTests.java b/libs/h3/src/test/java/org/elasticsearch/h3/LatLngTests.java
@@ -28,12 +28,23 @@
 
 public class LatLngTests extends ESTestCase {
 
-    public void testLatLonVec3d() {
+    public void testLatLonAzimuthRads() {
         final GeoPoint point = safePoint();
         for (int i = 0; i < Vec3d.faceCenterPoint.length; i++) {
             final double azVec3d = Vec3d.faceCenterPoint[i].geoAzimuthRads(point.x, point.y, point.z);
             final double azVec2d = Vec2d.faceCenterGeo[i].geoAzimuthRads(point.getLatitude(), point.getLongitude());
             assertEquals("Face " + i, azVec2d, azVec3d, 1e-12);
+
+        }
+    }
+
+    public void testLatLonAzDistanceRads() {
+        final double az = randomDoubleBetween(-2 * Math.PI, 2 * Math.PI, true);
+        final double distance = randomDoubleBetween(-Math.PI, Math.PI / 2, true);
+        for (int i = 0; i < Vec3d.faceCenterPoint.length; i++) {
+            final LatLng latLng3d = Vec3d.faceCenterPoint[i].geoAzDistanceRads(az, distance);
+            final LatLng latLng2d = Vec2d.faceCenterGeo[i].geoAzDistanceRads(az, distance);
+            assertTrue("Face " + i, latLng2d.isNumericallyIdentical(latLng3d));
         }
     }
 
