From 1aa8cc3b65fa68cf9b7ecdb329a5cda1249d1ce5 Mon Sep 17 00:00:00 2001 From: Michael Krasnyk Date: Fri, 31 Mar 2017 20:00:30 +0200 Subject: [PATCH] make getPathDistance algorithm-independent --- .../routing_algorithms/routing_base.hpp | 51 ++++++++++++ .../routing_algorithms/routing_base_ch.hpp | 5 -- .../routing_algorithms/routing_base_ch.cpp | 82 +++++-------------- 3 files changed, 70 insertions(+), 68 deletions(-) diff --git a/include/engine/routing_algorithms/routing_base.hpp b/include/engine/routing_algorithms/routing_base.hpp index c509204f2..1ca880f90 100644 --- a/include/engine/routing_algorithms/routing_base.hpp +++ b/include/engine/routing_algorithms/routing_base.hpp @@ -310,6 +310,57 @@ void annotatePath(const FacadeT &facade, } } +template +double getPathDistance(const datafacade::ContiguousInternalMemoryDataFacade &facade, + const std::vector unpacked_path, + 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; + double prev_lat = static_cast(toFloating(source_phantom.location.lat)) * DEGREE_TO_RAD; + double prev_lon = static_cast(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(toFloating(current_coordinate.lat)) * DEGREE_TO_RAD; + const double current_lon = + static_cast(toFloating(current_coordinate.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; + + prev_lat = current_lat; + prev_lon = current_lon; + prev_cos = current_cos; + } + + const double current_lat = + static_cast(toFloating(target_phantom.location.lat)) * DEGREE_TO_RAD; + const double current_lon = + static_cast(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; +} + } // namespace routing_algorithms } // namespace engine } // namespace osrm diff --git a/include/engine/routing_algorithms/routing_base_ch.hpp b/include/engine/routing_algorithms/routing_base_ch.hpp index ae5993f5a..e8b3d76c1 100644 --- a/include/engine/routing_algorithms/routing_base_ch.hpp +++ b/include/engine/routing_algorithms/routing_base_ch.hpp @@ -382,11 +382,6 @@ inline void search(const datafacade::ContiguousInternalMemoryDataFacade &facade, - const std::vector &packed_path, - const PhantomNode &source_phantom, - const PhantomNode &target_phantom); - // Requires the heaps for be empty // If heaps should be adjusted to be initialized outside of this function, // the addition of force_loop parameters might be required diff --git a/src/engine/routing_algorithms/routing_base_ch.cpp b/src/engine/routing_algorithms/routing_base_ch.cpp index df30c2528..f7c533c09 100644 --- a/src/engine/routing_algorithms/routing_base_ch.cpp +++ b/src/engine/routing_algorithms/routing_base_ch.cpp @@ -139,62 +139,6 @@ void search(const datafacade::ContiguousInternalMemoryDataFacade &fac } } -double getPathDistance(const datafacade::ContiguousInternalMemoryDataFacade &facade, - const std::vector &packed_path, - const PhantomNode &source_phantom, - const PhantomNode &target_phantom) -{ - std::vector unpacked_path; - PhantomNodes nodes; - nodes.source_phantom = source_phantom; - nodes.target_phantom = target_phantom; - unpackPath(facade, packed_path.begin(), packed_path.end(), nodes, unpacked_path); - - using util::coordinate_calculation::detail::DEGREE_TO_RAD; - using util::coordinate_calculation::detail::EARTH_RADIUS; - - double distance = 0; - double prev_lat = static_cast(toFloating(source_phantom.location.lat)) * DEGREE_TO_RAD; - double prev_lon = static_cast(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(toFloating(current_coordinate.lat)) * DEGREE_TO_RAD; - const double current_lon = - static_cast(toFloating(current_coordinate.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; - - prev_lat = current_lat; - prev_lon = current_lon; - prev_cos = current_cos; - } - - const double current_lat = - static_cast(toFloating(target_phantom.location.lat)) * DEGREE_TO_RAD; - const double current_lon = - static_cast(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; -} - // Requires the heaps for be empty // If heaps should be adjusted to be initialized outside of this function, // the addition of force_loop parameters might be required @@ -250,7 +194,14 @@ double getNetworkDistance(const datafacade::ContiguousInternalMemoryDataFacade::max(); } - return getPathDistance(facade, packed_path, source_phantom, target_phantom); + std::vector unpacked_path; + unpackPath(facade, + packed_path.begin(), + packed_path.end(), + {source_phantom, target_phantom}, + unpacked_path); + + return getPathDistance(facade, unpacked_path, source_phantom, target_phantom); } } // namespace ch @@ -474,12 +425,17 @@ double getNetworkDistance(const datafacade::ContiguousInternalMemoryDataFacade::max(); - if (weight != INVALID_EDGE_WEIGHT) - { - return ch::getPathDistance(facade, packed_path, source_phantom, target_phantom); - } - return distance; + if (weight == INVALID_EDGE_WEIGHT) + return std::numeric_limits::max(); + + std::vector unpacked_path; + ch::unpackPath(facade, + packed_path.begin(), + packed_path.end(), + {source_phantom, target_phantom}, + unpacked_path); + + return getPathDistance(facade, unpacked_path, source_phantom, target_phantom); } } // namespace corech