make getPathDistance algorithm-independent

include/engine/routing_algorithms/routing_base.hpp

@@ -310,6 +310,57 @@ void annotatePath(const FacadeT &facade,
     }
 }
 
+template<typename Algorithm>
+double getPathDistance(const datafacade::ContiguousInternalMemoryDataFacade<Algorithm> &facade,
+                       const std::vector<PathData> 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<double>(toFloating(source_phantom.location.lat)) * DEGREE_TO_RAD;
+    double prev_lon = static_cast<double>(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<double>(toFloating(current_coordinate.lat)) * DEGREE_TO_RAD;
+        const double current_lon =
+            static_cast<double>(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<double>(toFloating(target_phantom.location.lat)) * DEGREE_TO_RAD;
+    const double current_lon =
+        static_cast<double>(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

include/engine/routing_algorithms/routing_base_ch.hpp

@@ -382,11 +382,6 @@ inline void search(const datafacade::ContiguousInternalMemoryDataFacade<Algorith
            duration_upper_bound);
 }
 
-double getPathDistance(const datafacade::ContiguousInternalMemoryDataFacade<ch::Algorithm> &facade,
-                       const std::vector<NodeID> &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