diff --git a/.github/workflows/osrm-backend.yml b/.github/workflows/osrm-backend.yml index b73e9cb69..f93cf0fd4 100644 --- a/.github/workflows/osrm-backend.yml +++ b/.github/workflows/osrm-backend.yml @@ -715,7 +715,7 @@ jobs: run: | pushd ${OSRM_BUILD_DIR} make --jobs=${JOBS} benchmarks - ls -R ../test/data/ + # ls -R ../test/data/ ./src/benchmarks/match-bench ../test/data/ch/monaco.osrm # for i in ./src/benchmark/*-bench ; do echo Running $i ; $i ; done popd diff --git a/include/engine/routing_algorithms/routing_base.hpp b/include/engine/routing_algorithms/routing_base.hpp index 793ac29a8..a416a4429 100644 --- a/include/engine/routing_algorithms/routing_base.hpp +++ b/include/engine/routing_algorithms/routing_base.hpp @@ -353,21 +353,49 @@ double getPathDistance(const DataFacade &facade, const PhantomNode &source_phantom, const PhantomNode &target_phantom) { - double distance = 0.0; - auto prev_coordinate = source_phantom.location; + using util::coordinate_calculation::detail::DEGREE_TO_RAD; + using util::coordinate_calculation::detail::EARTH_RADIUS; + double distance = 0; + double prev_lat = + static_cast(util::toFloating(source_phantom.location.lat)) * DEGREE_TO_RAD; + double prev_lon = + static_cast(util::toFloating(source_phantom.location.lon)) * DEGREE_TO_RAD; + double prev_cos = std::cos(prev_lat); for (const auto &p : unpacked_path) { const auto current_coordinate = facade.GetCoordinateOfNode(p.turn_via_node); - distance += - util::coordinate_calculation::greatCircleDistance(prev_coordinate, current_coordinate); + const double current_lat = + static_cast(util::toFloating(current_coordinate.lat)) * DEGREE_TO_RAD; + const double current_lon = + static_cast(util::toFloating(current_coordinate.lon)) * DEGREE_TO_RAD; + const double current_cos = std::cos(current_lat); - prev_coordinate = current_coordinate; + const double sin_dlon = std::sin((prev_lon - current_lon) / 2.0); + const double sin_dlat = std::sin((prev_lat - current_lat) / 2.0); + + const double aharv = sin_dlat * sin_dlat + prev_cos * current_cos * sin_dlon * sin_dlon; + const double charv = 2. * std::atan2(std::sqrt(aharv), std::sqrt(1.0 - aharv)); + distance += EARTH_RADIUS * charv; + + prev_lat = current_lat; + prev_lon = current_lon; + prev_cos = current_cos; } - distance += - util::coordinate_calculation::greatCircleDistance(prev_coordinate, target_phantom.location); + const double current_lat = + static_cast(util::toFloating(target_phantom.location.lat)) * DEGREE_TO_RAD; + const double current_lon = + static_cast(util::toFloating(target_phantom.location.lon)) * DEGREE_TO_RAD; + const double current_cos = std::cos(current_lat); + + const double sin_dlon = std::sin((prev_lon - current_lon) / 2.0); + const double sin_dlat = std::sin((prev_lat - current_lat) / 2.0); + + const double aharv = sin_dlat * sin_dlat + prev_cos * current_cos * sin_dlon * sin_dlon; + const double charv = 2. * std::atan2(std::sqrt(aharv), std::sqrt(1.0 - aharv)); + distance += EARTH_RADIUS * charv; return distance; }