From 39a69530d23d0b4f7a6fcb195fa530e7391dabf4 Mon Sep 17 00:00:00 2001 From: Jeff Verkoeyen Date: Sat, 20 Sep 2025 09:40:05 -0400 Subject: [PATCH 1/8] WIP stable distance calculation at small distances. --- src/midgard/pointll.cc | 58 ++++++++++++++++++++++++++---------------- 1 file changed, 36 insertions(+), 22 deletions(-) diff --git a/src/midgard/pointll.cc b/src/midgard/pointll.cc index 7634e4c8a1..21144c8f6c 100644 --- a/src/midgard/pointll.cc +++ b/src/midgard/pointll.cc @@ -48,30 +48,44 @@ GeoPoint GeoPoint::PointAlongSegment(const GeoPoint PrecisionT GeoPoint::Distance(const GeoPoint& ll2) const { - // If points are the same, return 0 - if (*this == ll2) { - return 0; +template +PrecisionT GeoPoint::Distance(const GeoPoint& other) const { + // Equal points short-circuit + if (*this == other) return static_cast(0); + + // Promote to extended precision for the trig + const long double phi1 = static_cast(lat()) * static_cast(kRadPerDegD); + const long double phi2 = static_cast(other.lat()) * static_cast(kRadPerDegD); + const long double dphi = phi2 - phi1; + + long double dlambda = static_cast(other.lng() - lng()) * static_cast(kRadPerDegD); + // No need to wrap at +/-pi for distance; cos terms handle it, but keeping dlambda small helps tiny-angle math + if (dlambda > M_PI) dlambda -= 2.0L * M_PI; + if (dlambda < -M_PI) dlambda += 2.0L * M_PI; + + const long double c1 = std::cos(phi1); + const long double c2 = std::cos(phi2); + + // Haversine (well-conditioned for small angles) + const long double sdphi2 = std::sin(dphi * 0.5L); + const long double sdlmb2 = std::sin(dlambda * 0.5L); + long double h = sdphi2*sdphi2 + c1*c2*sdlmb2*sdlmb2; + + // Clamp due to rounding + h = std::min(1, std::max(0, h)); + + // For extremely tiny separations, avoid asin(sqrt(h)) altogether + // Use equirectangular approximation with hypot, which is relatively stable + if (h < 1e-24L) { + const long double x = dlambda * std::cos((phi1 + phi2) * 0.5L); + const long double y = dphi; + const long double d = static_cast(kRadEarthMeters) * std::hypot(x, y); + return static_cast(d); } - // 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(acos(cosb) * kRadEarthMeters); - } + const long double d = 2.0L * std::asin(std::sqrt(h)); + const long double meters = static_cast(kRadEarthMeters) * d; + return static_cast(meters); } /** From 2ba6422b20325a1fe1bac017ac5f8e4bd41ec59e Mon Sep 17 00:00:00 2001 From: Jeff Verkoeyen Date: Sat, 20 Sep 2025 15:08:47 -0400 Subject: [PATCH 2/8] Update changelog. --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index dd85aaafae..13387ce1a6 100644 --- a/CHANGELOG.md +++ b/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) From 4d62f4ad54dc86481f329130efe518092f63bc87 Mon Sep 17 00:00:00 2001 From: Jeff Verkoeyen Date: Sat, 20 Sep 2025 15:17:29 -0400 Subject: [PATCH 3/8] Format. --- src/midgard/pointll.cc | 23 ++++++++++++++++------- 1 file changed, 16 insertions(+), 7 deletions(-) diff --git a/src/midgard/pointll.cc b/src/midgard/pointll.cc index 21144c8f6c..0ddc59a6d4 100644 --- a/src/midgard/pointll.cc +++ b/src/midgard/pointll.cc @@ -51,17 +51,26 @@ GeoPoint GeoPoint::PointAlongSegment(const GeoPoint PrecisionT GeoPoint::Distance(const GeoPoint& other) const { // Equal points short-circuit - if (*this == other) return static_cast(0); + if (*this == other) { + return static_cast(0); + } // Promote to extended precision for the trig const long double phi1 = static_cast(lat()) * static_cast(kRadPerDegD); - const long double phi2 = static_cast(other.lat()) * static_cast(kRadPerDegD); + const long double phi2 = + static_cast(other.lat()) * static_cast(kRadPerDegD); const long double dphi = phi2 - phi1; - long double dlambda = static_cast(other.lng() - lng()) * static_cast(kRadPerDegD); - // No need to wrap at +/-pi for distance; cos terms handle it, but keeping dlambda small helps tiny-angle math - if (dlambda > M_PI) dlambda -= 2.0L * M_PI; - if (dlambda < -M_PI) dlambda += 2.0L * M_PI; + long double dlambda = + static_cast(other.lng() - lng()) * static_cast(kRadPerDegD); + // No need to wrap at +/-pi for distance; cos terms handle it, but keeping dlambda small helps + // tiny-angle math + if (dlambda > M_PI) { + dlambda -= 2.0L * M_PI; + } + if (dlambda < -M_PI) { + dlambda += 2.0L * M_PI; + } const long double c1 = std::cos(phi1); const long double c2 = std::cos(phi2); @@ -69,7 +78,7 @@ PrecisionT GeoPoint::Distance(const GeoPoint& other) const { // Haversine (well-conditioned for small angles) const long double sdphi2 = std::sin(dphi * 0.5L); const long double sdlmb2 = std::sin(dlambda * 0.5L); - long double h = sdphi2*sdphi2 + c1*c2*sdlmb2*sdlmb2; + long double h = sdphi2 * sdphi2 + c1 * c2 * sdlmb2 * sdlmb2; // Clamp due to rounding h = std::min(1, std::max(0, h)); From 8dbf6b5499659e922ccc9c25849f7bffb6edb324 Mon Sep 17 00:00:00 2001 From: Jeff Verkoeyen Date: Sat, 20 Sep 2025 15:25:44 -0400 Subject: [PATCH 4/8] Cleanup. --- src/midgard/pointll.cc | 39 +++++++++++++++++++++++---------------- 1 file changed, 23 insertions(+), 16 deletions(-) diff --git a/src/midgard/pointll.cc b/src/midgard/pointll.cc index 0ddc59a6d4..057076a6b5 100644 --- a/src/midgard/pointll.cc +++ b/src/midgard/pointll.cc @@ -41,12 +41,18 @@ GeoPoint GeoPoint::PointAlongSegment(const GeoPoint PrecisionT GeoPoint::Distance(const GeoPoint& other) const { @@ -61,32 +67,33 @@ PrecisionT GeoPoint::Distance(const GeoPoint& other) const { static_cast(other.lat()) * static_cast(kRadPerDegD); const long double dphi = phi2 - phi1; - long double dlambda = + long double deltalng = static_cast(other.lng() - lng()) * static_cast(kRadPerDegD); - // No need to wrap at +/-pi for distance; cos terms handle it, but keeping dlambda small helps + // No need to wrap at +/-pi for distance; cos terms handle it, but keeping deltalng small helps // tiny-angle math - if (dlambda > M_PI) { - dlambda -= 2.0L * M_PI; + if (deltalng > M_PI) { + deltalng -= 2.0L * M_PI; } - if (dlambda < -M_PI) { - dlambda += 2.0L * M_PI; + if (deltalng < -M_PI) { + deltalng += 2.0L * M_PI; } const long double c1 = std::cos(phi1); const long double c2 = std::cos(phi2); - // Haversine (well-conditioned for small angles) + // Haversine (numerically stable for small angles) + // https://en.wikipedia.org/wiki/Haversine_formula const long double sdphi2 = std::sin(dphi * 0.5L); - const long double sdlmb2 = std::sin(dlambda * 0.5L); + const long double sdlmb2 = std::sin(deltalng * 0.5L); long double h = sdphi2 * sdphi2 + c1 * c2 * sdlmb2 * sdlmb2; // Clamp due to rounding h = std::min(1, std::max(0, h)); - // For extremely tiny separations, avoid asin(sqrt(h)) altogether + // For extremely tiny distances, avoid asin(sqrt(h)) altogether // Use equirectangular approximation with hypot, which is relatively stable if (h < 1e-24L) { - const long double x = dlambda * std::cos((phi1 + phi2) * 0.5L); + const long double x = deltalng * std::cos((phi1 + phi2) * 0.5L); const long double y = dphi; const long double d = static_cast(kRadEarthMeters) * std::hypot(x, y); return static_cast(d); From 29f0c17c189741676d6a441815506ac70a7fbc0e Mon Sep 17 00:00:00 2001 From: Jeff Verkoeyen Date: Sat, 20 Sep 2025 16:17:55 -0400 Subject: [PATCH 5/8] Fix test. --- test/pointll.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/pointll.cc b/test/pointll.cc index dd3b7ef57c..ecb5096a26 100644 --- a/test/pointll.cc +++ b/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"; From b2676f90a908daa66d288931561650034449c81b Mon Sep 17 00:00:00 2001 From: Jeff Verkoeyen Date: Sat, 20 Sep 2025 18:56:41 -0400 Subject: [PATCH 6/8] Add epsilon checks. --- src/mjolnir/bssbuilder.cc | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/mjolnir/bssbuilder.cc b/src/mjolnir/bssbuilder.cc index 9db91efb24..aa8e44fae7 100644 --- a/src/mjolnir/bssbuilder.cc +++ b/src/mjolnir/bssbuilder.cc @@ -185,6 +185,8 @@ std::vector project(const GraphTile& local_tile, auto best_ped = BestProjection{}; auto best_bicycle = BestProjection{}; + 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 +212,7 @@ std::vector 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 +221,7 @@ std::vector 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; From 9de476e11669579b6878bfe1ef03ce18660f58fb Mon Sep 17 00:00:00 2001 From: Jeff Verkoeyen Date: Sat, 20 Sep 2025 20:40:04 -0400 Subject: [PATCH 7/8] Simplify the math. --- src/midgard/pointll.cc | 70 +++++++++++++++++++----------------------- 1 file changed, 32 insertions(+), 38 deletions(-) diff --git a/src/midgard/pointll.cc b/src/midgard/pointll.cc index 057076a6b5..b3c02c62f6 100644 --- a/src/midgard/pointll.cc +++ b/src/midgard/pointll.cc @@ -44,8 +44,7 @@ GeoPoint GeoPoint::PointAlongSegment(const GeoPoint::Distance(const GeoPoint& other) const { return static_cast(0); } - // Promote to extended precision for the trig - const long double phi1 = static_cast(lat()) * static_cast(kRadPerDegD); - const long double phi2 = - static_cast(other.lat()) * static_cast(kRadPerDegD); - const long double dphi = phi2 - phi1; - - long double deltalng = - static_cast(other.lng() - lng()) * static_cast(kRadPerDegD); - // No need to wrap at +/-pi for distance; cos terms handle it, but keeping deltalng small helps - // tiny-angle math - if (deltalng > M_PI) { - deltalng -= 2.0L * M_PI; - } - if (deltalng < -M_PI) { - deltalng += 2.0L * M_PI; - } + // 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; - const long double c1 = std::cos(phi1); - const long double c2 = std::cos(phi2); + // c1 = cos φ1 + // c2 = cos φ2 + const double c1 = std::cos(phi1); + const double c2 = std::cos(phi2); // Haversine (numerically stable for small angles) - // https://en.wikipedia.org/wiki/Haversine_formula - const long double sdphi2 = std::sin(dphi * 0.5L); - const long double sdlmb2 = std::sin(deltalng * 0.5L); - long double h = sdphi2 * sdphi2 + c1 * c2 * sdlmb2 * sdlmb2; - - // Clamp due to rounding - h = std::min(1, std::max(0, h)); - - // For extremely tiny distances, avoid asin(sqrt(h)) altogether - // Use equirectangular approximation with hypot, which is relatively stable - if (h < 1e-24L) { - const long double x = deltalng * std::cos((phi1 + phi2) * 0.5L); - const long double y = dphi; - const long double d = static_cast(kRadEarthMeters) * std::hypot(x, y); - return static_cast(d); - } + // 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(1, std::max(0, h)); - const long double d = 2.0L * std::asin(std::sqrt(h)); - const long double meters = static_cast(kRadEarthMeters) * d; + // d = 2r * arcsin(sqrt(h)) + const double d = 2.0 * std::asin(std::sqrt(h)); + const double meters = kRadEarthMeters * d; return static_cast(meters); } From 6eef8cde1d37fd53ce83faa50c1a6017dee3ae86 Mon Sep 17 00:00:00 2001 From: Jeff Verkoeyen Date: Sat, 20 Sep 2025 20:44:08 -0400 Subject: [PATCH 8/8] Documentation. --- src/mjolnir/bssbuilder.cc | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/mjolnir/bssbuilder.cc b/src/mjolnir/bssbuilder.cc index aa8e44fae7..c5314887ac 100644 --- a/src/mjolnir/bssbuilder.cc +++ b/src/mjolnir/bssbuilder.cc @@ -185,6 +185,8 @@ std::vector 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