AP_Math: correct Polygon_closest_distance_point() for use with lat/lng at small distances

add polygon proximity tests

Author: andyp1per

libraries/AP_Math/polygon.cpp

@@ -17,6 +17,7 @@
  */
 
 #include "AP_Math.h"
+#include "float.h"
 
 #pragma GCC optimize("O2")
 
@@ -124,7 +125,6 @@ template bool Polygon_complete<int32_t>(const Vector2l *V, unsigned n);
 template bool Polygon_outside<float>(const Vector2f &P, const Vector2f *V, unsigned n);
 template bool Polygon_complete<float>(const Vector2f *V, unsigned n);
 
-
 /*
   determine if the polygon of N verticies defined by points V is
   intersected by a line from point p1 to point p2
@@ -196,21 +196,96 @@ float Polygon_closest_distance_line(const Vector2f *V, unsigned N, const Vector2
     return sqrtf(closest_sq);
 }
 
+const ftype EARTH_RADIUS_METERS = 6378137.0; // Earth's radius in meters (WGS84)
+
+// Converts 1e7 degrees to radians
+static inline ftype to_radians(ftype degrees_1e7)
+{
+    return radians(degrees_1e7 / 1.0e7);
+}
+
+// Converts (latitude, longitude) in radians to 3D Cartesian coordinates on a unit sphere
+static void latlng_to_cartesian(ftype latRad, ftype lonRad, Vector3F& v)
+{
+    v.x = cosF(latRad) * cosF(lonRad);
+    v.y = cosF(latRad) * sinF(lonRad);
+    v.z = sinF(latRad);
+}
+
+// Haversine distance between two points given in 1e7 degrees
+static ftype haversine(long lat1_1e7, long lon1_1e7, long lat2_1e7, long lon2_1e7)
+{
+    ftype lat1 = to_radians(lat1_1e7);
+    ftype lat2 = to_radians(lat2_1e7);
+
+    ftype dLat = to_radians(lat2_1e7 - lat1_1e7);
+    ftype dLon = to_radians(lon2_1e7 - lon1_1e7);
+
+    // (d/2r)^2
+    ftype a = dLat * dLat / 4 + cosF(lat2) * cosF(lat1) * dLon * dLon / 4;
+    ftype d = sqrtF(a) * 2 * EARTH_RADIUS_METERS;
+
+    return d;
+}
+
+// Compute closest distance from a point to a line segment on a sphere
+static ftype closest_distance_to_segment(const Vector2l& point, const Vector2l& line_start, const Vector2l& line_end)
+{
+    // Convert lat/lon to 3D Cartesian coordinates on a unit sphere
+    Vector3F v1, v2, vp;
+    latlng_to_cartesian(to_radians(line_start.x), to_radians(line_start.y), v1);
+    latlng_to_cartesian(to_radians(line_end.x), to_radians(line_end.y), v2);
+    latlng_to_cartesian(to_radians(point.x), to_radians(point.y), vp);
+
+    // Compute vector from lineStart to lineEnd (v) and from lineStart to point (w)
+    Vector3F v = v2 - v1;
+    Vector3F w = vp - v1;
+
+    // Project w onto v
+    ftype dot_vw = v * w;
+    ftype dot_vv = v * v;
+    ftype t = dot_vw / dot_vv;
+
+    Vector3F closest;
+    if (t < 0) {
+        // Closest to line start
+        closest = v1;
+    } else if (t > 1) {
+        // Closest to line end
+        closest = v2;
+    } else {
+        // Closest point is on the line
+        closest = v1 + v * t;
+    }
+
+    // Convert closest point back to lat/lon (reverse cartesian)
+    ftype norm = closest.length();
+    ftype closestLat = asinF(closest.z / norm); // Latitude from z-coordinate
+    ftype closestLon = atan2F(closest.y, closest.x); // Longitude from x and y
+
+    // Convert back to 1e7 degrees
+    long closestLat_1e7 = static_cast<long>(degrees(closestLat) * 1e7);
+    long closestLon_1e7 = static_cast<long>(degrees(closestLon) * 1e7);
+
+    // Calculate the Haversine distance from the point to the closest point on the line
+    return haversine(point.x, point.y, closestLat_1e7, closestLon_1e7);
+}
+
 /*
-  return the closest distance that point p comes to an edge of closed
-  polygon V, defined by N points
+  return the closest distance in meters that point p in lat/lng 1e7 comes to an edge of closed
+  polygon V, defined by N points of lat/lng 1e7
  */
-float Polygon_closest_distance_point(const Vector2f *V, unsigned N, const Vector2f &p)
+ftype Polygon_closest_distance_point(const Vector2l *V, unsigned N, const Vector2l &p)
 {
-    float closest_sq = FLT_MAX;
-    for (uint8_t i=0; i<N-1; i++) {
-        const Vector2f &v1 = V[i];
-        const Vector2f &v2 = V[i+1];
+    ftype closest = std::numeric_limits<ftype>::max();
+    for (uint8_t i=0; i<N; i++) {
+        const Vector2l &v1 = V[i];
+        const Vector2l &v2 = V[(i+1) % N];
 
-        float dist_sq = Vector2f::closest_distance_between_line_and_point_squared(v1, v2, p);
-        if (dist_sq < closest_sq) {
-            closest_sq = dist_sq;
+        ftype distance = closest_distance_to_segment(p, v1, v2);
+        if (distance < closest) {
+            closest = distance;
         }
     }
-    return sqrtf(closest_sq);
+    return closest;
 }

libraries/AP_Math/polygon.h

@@ -40,7 +40,7 @@ bool Polygon_intersects(const Vector2f *V, unsigned N, const Vector2f &p1, const
 float Polygon_closest_distance_line(const Vector2f *V, unsigned N, const Vector2f &p1, const Vector2f &p2);
 
 /*
-  return the closest distance that a point p comes to an edge of
-  closed polygon V, defined by N points
+  return the closest distance that a point p in lat/lng comes to an edge of
+  closed polygon V, defined by N points of lat/lng
  */
-float Polygon_closest_distance_point(const Vector2f *V, unsigned N, const Vector2f &p);
+ftype Polygon_closest_distance_point(const Vector2l *V, unsigned N, const Vector2l &p);

libraries/AP_Math/tests/test_polygon.cpp

@@ -304,6 +304,63 @@ TEST(Polygon, obc)
     TEST_POLYGON_POINTS(OBC_boundary, OBC_test_points);
 }
 
+#define TEST_POLYGON_DISTANCE_POINTS(POLYGON, TEST_POINTS)                       \
+    do {                                                                \
+        for (uint32_t i = 0; i < ARRAY_SIZE(TEST_POINTS); i++) {        \
+            EXPECT_TRUE(fabsF(TEST_POINTS[i].distance -                       \
+                      Polygon_closest_distance_point(POLYGON,           \
+                                                     ARRAY_SIZE(POLYGON),\
+                                                     TEST_POINTS[i].point)) <= 1.0f); \
+        }                                                               \
+    } while(0)
+
+// Center of London (Charing Cross)
+const int32_t CENTER_LAT = static_cast<int32_t>(51.5085 * 1E7); // 515085000
+const int32_t CENTER_LON = static_cast<int32_t>(-0.1257 * 1E7); // -1257000
+
+const int32_t CENTER_NORTH_LAT = static_cast<int32_t>((51.5085  + 0.0003366)* 1E7); // 37.5m from edge
+const int32_t CENTER_EAST_LON = static_cast<int32_t>((-0.1257 + 0.0005388)* 1E7); //  37.5m from edge
+const int32_t CENTER_SOUTH_LAT = static_cast<int32_t>((51.5085 - 0.0003366) * 1E7); // 37.5m from edge
+const int32_t CENTER_WEST_LON = static_cast<int32_t>((-0.1257 - 0.0005388) * 1E7); // 37.5m from edge
+
+// Bounding box coordinates (in 1E7 degrees)
+const int32_t NORTH_WEST_LAT = static_cast<int32_t>((51.5085 + 0.0006732) * 1E7); // 515092732
+const int32_t NORTH_WEST_LON = static_cast<int32_t>((-0.1257 - 0.0010776) * 1E7); // -1267776
+
+const int32_t NORTH_EAST_LAT = static_cast<int32_t>((51.5085 + 0.0006732) * 1E7); // 515092732
+const int32_t NORTH_EAST_LON = static_cast<int32_t>((-0.1257 + 0.0010776) * 1E7); // -1246224
+
+const int32_t SOUTH_WEST_LAT = static_cast<int32_t>((51.5085 - 0.0006732) * 1E7); // 515080268
+const int32_t SOUTH_WEST_LON = static_cast<int32_t>((-0.1257 - 0.0010776) * 1E7); // -1267776
+
+const int32_t SOUTH_EAST_LAT = static_cast<int32_t>((51.5085 - 0.0006732) * 1E7); // 515080268
+const int32_t SOUTH_EAST_LON = static_cast<int32_t>((-0.1257 + 0.0010776) * 1E7); // -1246224
+
+// Array of coordinates in [latitude, longitude] pairs for each corner
+static const Vector2l London_boundary[] {
+    Vector2l(NORTH_WEST_LAT, NORTH_WEST_LON), // Northwest corner
+    Vector2l(NORTH_EAST_LAT, NORTH_EAST_LON), // Northeast corner
+    Vector2l(SOUTH_EAST_LAT, SOUTH_EAST_LON), // Southeast corner
+    Vector2l(SOUTH_WEST_LAT, SOUTH_WEST_LON), // Southwest corner
+    Vector2l(NORTH_WEST_LAT, NORTH_WEST_LON), // Northwest corner
+};
+
+static const struct {
+    Vector2l point;
+    float distance;
+} London_test_points[] = {
+    { Vector2l(CENTER_LAT, CENTER_LON), 75.0f, },
+    { Vector2l(CENTER_NORTH_LAT, CENTER_LON), 37.5f, },
+    { Vector2l(CENTER_LAT, CENTER_EAST_LON), 37.5f, },
+    { Vector2l(CENTER_SOUTH_LAT, CENTER_LON), 37.5f, },
+    { Vector2l(CENTER_LAT, CENTER_WEST_LON), 37.5f, },
+};
+
+TEST(Polygon, London_distance)
+{
+    TEST_POLYGON_DISTANCE_POINTS(London_boundary, London_test_points);
+}
+
 static const Vector2f PROX_boundary[] = {
     Vector2f{938.315063f,388.662872f},
     Vector2f{545.622803f,1317.25f},

libraries/AP_Math/vector2.cpp

@@ -457,4 +457,4 @@ template bool Vector2<long>::operator ==(const Vector2<long> &v) const;
 template bool Vector2<long>::operator !=(const Vector2<long> &v) const;
 template bool Vector2<int>::operator ==(const Vector2<int> &v) const;
 template bool Vector2<int>::operator !=(const Vector2<int> &v) const;
-
+template int32_t Vector2l::closest_distance_between_line_and_point_squared(Vector2l const&, Vector2l const&, Vector2l const&);