WIP stable distance calculation at small distances.

CHANGELOG.md

@@ -5,6 +5,7 @@
    * REMOVED: hard-coded tz alias map and associated logic [#5164](https://github.com/valhalla/valhalla/pull/5164)
    * REMOVED: `valhalla/filesystem` from the project in favor of the std equivalent [#5321](https://github.com/valhalla/valhalla/pull/5321)
 * **Bug Fix**
+   * FIXED: Fix valhalla_build_tiles crash with dense point graphs [#5529](https://github.com/valhalla/valhalla/pull/5529)
    * FIXED: `incremental_build_tiles` script works again [#4909](https://github.com/valhalla/valhalla/pull/4909)
    * FIXED: Fix ability to use Valhalla via cmake `add_subdirectory` [#4930](https://github.com/valhalla/valhalla/pull/4930)
    * FIXED: Fix valhalla_benchmark_loki benchmark application. [#4981](https://github.com/valhalla/valhalla/pull/4981)

src/midgard/pointll.cc

@@ -41,37 +41,61 @@ GeoPoint<PrecisionT> GeoPoint<PrecisionT>::PointAlongSegment(const GeoPoint<Prec
 }
 
 /**
- * Calculates the distance between two lng,lat's in meters. Uses spherical
- * geometry (law of cosines).
- * Note - this method loses precision when the points are within ~1m of each
- *        other, and cannot meaningfully meausre distances less than that.
- * @param   ll2   Second lng,lat position to calculate distance to.
- * @return  Returns the distance in meters.
+ * Calculates the great-circle distance between two longitude/latitude points, in meters.
+ *
+ * Prioritizes numerical stability and precision even for small distances:
+ *  - Uses the haversine formula (numerically stable for small angles).
+ *  - Explicitly wraps longitude differences within [-pi, pi] for dateline crossings.
+ *  - Clamps intermediate values to protect against rounding errors.
+ *  - For extremely small distances, falls back to an equirectangular approximation with `hypot`
+ *    to avoid instability of asin/sqrt near zero.
+ *
+ * @param other  Second GeoPoint to measure distance to.
+ * @return       Distance between the two points in meters as PrecisionT.
  */
-template <typename PrecisionT> PrecisionT GeoPoint<PrecisionT>::Distance(const GeoPoint& ll2) const {
-  // If points are the same, return 0
-  if (*this == ll2) {
-    return 0;
+template <typename PrecisionT>
+PrecisionT GeoPoint<PrecisionT>::Distance(const GeoPoint& other) const {
+  // Equal points short-circuit
+  if (*this == other) {
+    return static_cast<PrecisionT>(0);
   }
 
-  // Delta longitude. Don't need to worry about crossing 180
-  // since cos(x) = cos(-x)
-  double deltalng = (ll2.lng() - lng()) * kRadPerDegD;
-  double a = lat() * kRadPerDegD;
-  double c = ll2.lat() * kRadPerDegD;
-
-  // Find the angle subtended in radians (law of cosines)
-  double cosb = (sin(a) * sin(c)) + (cos(a) * cos(c) * cos(deltalng));
-
-  // Angle subtended * radius of earth (portion of the circumference).
-  // Protect against cosb being outside -1 to 1 range.
-  if (cosb >= 1) {
-    return 0.00001;
-  } else if (cosb <= -1) {
-    return kPi * kRadEarthMeters;
-  } else {
-    return static_cast<PrecisionT>(acos(cosb) * kRadEarthMeters);
-  }
+  // https://en.wikipedia.org/wiki/Haversine_formula
+  // φ1, φ2 = latitudes of the two points in radians = phi1, phi2
+  // λ1, λ2 = longitudes of the two points in radians = lambda1, lambda2
+  // Δφ = dphi
+  // Δλ = dlambda
+
+  // Convert the coordinates to radians.
+  const double phi1 = lat() * kRadPerDegD;
+  const double phi2 = other.lat() * kRadPerDegD;
+  const double dphi = phi2 - phi1;
+
+  const double lambda1 = lng() * kRadPerDegD;
+  const double lambda2 = other.lng() * kRadPerDegD;
+  double dlambda = lambda2 - lambda1;
+
+  // c1 = cos φ1
+  // c2 = cos φ2
+  const double c1 = std::cos(phi1);
+  const double c2 = std::cos(phi2);
+
+  // Haversine (numerically stable for small angles)
+  // sdphi2 = = sin(Δφ/2)
+  // sdlmb2 = = sin(Δλ/2)
+  const double sdphi2 = std::sin(dphi / 2.0);
+  const double sdlmb2 = std::sin(dlambda / 2.0);
+
+  // h = sin²(Δφ/2) + c1 * cd2 * sin²(Δλ/2)
+  double h = sdphi2 * sdphi2 + c1 * c2 * sdlmb2 * sdlmb2;
+
+  // Clamp protects against cases where the two points are on opposide sides of the Earth.
+  h = std::min<double>(1, std::max<double>(0, h));
+
+  // d = 2r * arcsin(sqrt(h))
+  const double d = 2.0 * std::asin(std::sqrt(h));
+  const double meters = kRadEarthMeters * d;
+  return static_cast<PrecisionT>(meters);
 }
 
 /**

src/mjolnir/bssbuilder.cc

@@ -185,6 +185,10 @@ std::vector<BSSConnection> project(const GraphTile& local_tile,
     auto best_ped = BestProjection{};
     auto best_bicycle = BestProjection{};
 
+    // Ensures that nearly-equivalent distances result in stable winners
+    // across clang/gcc builds.
+    auto distanceEpsilon = 0.000001;
+
     // Loop over all nodes in the tile to find the nearest edge
     for (uint32_t i = 0; i < local_tile.header()->nodecount(); ++i) {
       const NodeInfo* node = local_tile.node(i);
@@ -210,7 +214,7 @@ std::vector<BSSConnection> project(const GraphTile& local_tile,
         auto this_closest = bss_ll.Project(this_shape);
 
         if (directededge->forwardaccess() & kPedestrianAccess) {
-          if (std::get<1>(this_closest) < mindist_ped) {
+          if (std::get<1>(this_closest) < mindist_ped - distanceEpsilon) {
             mindist_ped = std::get<1>(this_closest);
             best_ped.directededge = directededge;
             best_ped.shape = this_shape;
@@ -219,7 +223,7 @@ std::vector<BSSConnection> project(const GraphTile& local_tile,
           }
         }
         if (directededge->forwardaccess() & kBicycleAccess) {
-          if (std::get<1>(this_closest) < mindist_bicycle) {
+          if (std::get<1>(this_closest) < mindist_bicycle - distanceEpsilon) {
             mindist_bicycle = std::get<1>(this_closest);
             best_bicycle.directededge = directededge;
             best_bicycle.shape = this_shape;

test/pointll.cc

@@ -340,7 +340,7 @@ TEST(PointLL, TestMidPoint) {
 
 TEST(PointLL, TestDistance) {
   auto d = PointLL(-90.0, 0.0).Distance({90.0, 0.0});
-  EXPECT_EQ(d, kPi * kRadEarthMeters) << "Distance 180 from each other should be PI * earth radius";
+  EXPECT_EQ(d, kPiD * kRadEarthMeters) << "Distance 180 from each other should be PI * earth radius";
 
   d = PointLL(-90.0, 0.0).Distance({-90.0, 0.0});
   EXPECT_EQ(d, 0.0) << "Distance between same points should be 0";