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) diff --git a/src/midgard/pointll.cc b/src/midgard/pointll.cc index 7634e4c8a1..b3c02c62f6 100644 --- a/src/midgard/pointll.cc +++ b/src/midgard/pointll.cc @@ -41,37 +41,61 @@ 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); } - // 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); - } + // 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(1, std::max(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(meters); } /** diff --git a/src/mjolnir/bssbuilder.cc b/src/mjolnir/bssbuilder.cc index 9db91efb24..c5314887ac 100644 --- a/src/mjolnir/bssbuilder.cc +++ b/src/mjolnir/bssbuilder.cc @@ -185,6 +185,10 @@ 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 for (uint32_t i = 0; i < local_tile.header()->nodecount(); ++i) { const NodeInfo* node = local_tile.node(i); @@ -210,7 +214,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 +223,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; 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";