diff --git a/include/engine/routing_algorithms/routing_base.hpp b/include/engine/routing_algorithms/routing_base.hpp index a416a4429..793ac29a8 100644 --- a/include/engine/routing_algorithms/routing_base.hpp +++ b/include/engine/routing_algorithms/routing_base.hpp @@ -353,49 +353,21 @@ double getPathDistance(const DataFacade &facade, const PhantomNode &source_phantom, const PhantomNode &target_phantom) { - using util::coordinate_calculation::detail::DEGREE_TO_RAD; - using util::coordinate_calculation::detail::EARTH_RADIUS; + double distance = 0.0; + auto prev_coordinate = source_phantom.location; - 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); - 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); + distance += + util::coordinate_calculation::greatCircleDistance(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; + prev_coordinate = current_coordinate; } - 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; + distance += + util::coordinate_calculation::greatCircleDistance(prev_coordinate, target_phantom.location); return distance; }