Revert "Improve speed of Map Matching" (#5196)

* Revert "Update changelog"

This reverts commit 9b779c704f.

* Revert "Fix formating"

This reverts commit 5bd7d04fe3.

* Revert "Fix bug in computation of distance offset for phantom node"

This reverts commit 0f78f7b2cc.

* Revert "Adjust text cases for flightly different matching due to rounding"

This reverts commit 8473be69d2.

* Revert "Round network distance to deci-meter to retain previous behavior"

This reverts commit c0124f7d77.

* Revert "Preserve heap state in map matching"

This reverts commit b630b4e32a.

* Revert "Use distance functions from many to many"

This reverts commit 89fabc1b9c.

* Revert "Use FCC algorithm for map matching distance calculation"

This reverts commit a649a8a5cf.

CHANGELOG.md

@@ -6,8 +6,6 @@
       - ADDED: Node bindings can return pre-rendered JSON buffer. [#5189](https://github.com/Project-OSRM/osrm-backend/pull/5189)
     - Bugfixes:
       - FIXED: collapsing of ExitRoundabout instructions [#5114](https://github.com/Project-OSRM/osrm-backend/issues/5114)
-      - FIXED: fix osrm-routed gdb not work issue [#5156](https://github.com/Project-OSRM/osrm-backend/issues/5156)
-      - FIXED: negative distances in table plugin annotation [#5106](https://github.com/Project-OSRM/osrm-backend/issues/5106)
     - Misc:
       - CHANGED: Support up to 512 named shared memory regions [#5185](https://github.com/Project-OSRM/osrm-backend/pull/5185)
 

features/testbot/matching.feature

@@ -792,4 +792,4 @@ Feature: Basic Map Matching
         When I match I should get
           | trace | geometry             | a:distance | a:duration | a:weight | duration |
           | 2345  | 1.00018,1,1.000315,1 | 15.013264  | 1.5        | 1.5      | 1.5      |
-          | 4321  | 1.00027,1,1.000135,1 | 15.013264  | 1.5        | 1.5      | 1.5      |
+          | 4321  | 1.00027,1,1.000135,1 | 15.013264  | 1.5        | 1.5      | 1.5      |

include/engine/geospatial_query.hpp

@@ -449,7 +449,6 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
         const auto reverse_durations = datafacade.GetUncompressedReverseDurations(geometry_id);
 
         const auto forward_geometry = datafacade.GetUncompressedForwardGeometry(geometry_id);
-        const auto reverse_geometry = datafacade.GetUncompressedReverseGeometry(geometry_id);
 
         const auto forward_weight_offset =
             std::accumulate(forward_weights.begin(),
@@ -480,19 +479,19 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
             datafacade.GetCoordinateOfNode(forward_geometry(data.fwd_segment_position)),
             point_on_segment);
 
-        const auto rev_segment_position = reverse_weights.size() - data.fwd_segment_position - 1;
+        const auto reverse_weight_offset =
-
+            std::accumulate(reverse_weights.begin(),
-        const auto reverse_weight_offset = std::accumulate(
+                            reverse_weights.end() - data.fwd_segment_position - 1,
-            reverse_weights.begin(), reverse_weights.begin() + rev_segment_position, EdgeWeight{0});
+                            EdgeWeight{0});
 
         const auto reverse_duration_offset =
             std::accumulate(reverse_durations.begin(),
-                            reverse_durations.begin() + rev_segment_position,
+                            reverse_durations.end() - data.fwd_segment_position - 1,
                             EdgeDuration{0});
 
         EdgeDistance reverse_distance_offset = 0;
-        for (auto current = reverse_geometry.begin();
+        for (auto current = forward_geometry.begin();
-             current < reverse_geometry.begin() + rev_segment_position;
+             current < forward_geometry.end() - data.fwd_segment_position - 2;
              ++current)
         {
             reverse_distance_offset += util::coordinate_calculation::fccApproximateDistance(
@@ -500,11 +499,13 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
                 datafacade.GetCoordinateOfNode(*std::next(current)));
         }
 
-        EdgeWeight reverse_weight = reverse_weights[rev_segment_position];
+        EdgeWeight reverse_weight =
-        EdgeDuration reverse_duration = reverse_durations[rev_segment_position];
+            reverse_weights[reverse_weights.size() - data.fwd_segment_position - 1];
+        EdgeDuration reverse_duration =
+            reverse_durations[reverse_durations.size() - data.fwd_segment_position - 1];
         EdgeDistance reverse_distance = util::coordinate_calculation::fccApproximateDistance(
             point_on_segment,
-            datafacade.GetCoordinateOfNode(reverse_geometry(rev_segment_position)));
+            datafacade.GetCoordinateOfNode(forward_geometry(data.fwd_segment_position + 1)));
 
         ratio = std::min(1.0, std::max(0.0, ratio));
         if (data.forward_segment_id.id != SPECIAL_SEGMENTID)
@@ -692,7 +693,7 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
     const CoordinateList &coordinates;
     DataFacadeT &datafacade;
 };
-} // namespace engine
+}
-} // namespace osrm
+}
 
 #endif

include/engine/routing_algorithms/routing_base.hpp

@@ -44,19 +44,50 @@ bool needsLoopBackwards(const PhantomNode &source_phantom, const PhantomNode &ta
 bool needsLoopForward(const PhantomNodes &phantoms);
 bool needsLoopBackwards(const PhantomNodes &phantoms);
 
-namespace detail
+template <typename Heap>
+void insertNodesInHeaps(Heap &forward_heap, Heap &reverse_heap, const PhantomNodes &nodes)
 {
-template <typename Algorithm>
+    const auto &source = nodes.source_phantom;
-void insertSourceInHeap(typename SearchEngineData<Algorithm>::ManyToManyQueryHeap &heap,
+    if (source.IsValidForwardSource())
-                        const PhantomNode &phantom_node)
+    {
+        forward_heap.Insert(source.forward_segment_id.id,
+                            -source.GetForwardWeightPlusOffset(),
+                            source.forward_segment_id.id);
+    }
+
+    if (source.IsValidReverseSource())
+    {
+        forward_heap.Insert(source.reverse_segment_id.id,
+                            -source.GetReverseWeightPlusOffset(),
+                            source.reverse_segment_id.id);
+    }
+
+    const auto &target = nodes.target_phantom;
+    if (target.IsValidForwardTarget())
+    {
+        reverse_heap.Insert(target.forward_segment_id.id,
+                            target.GetForwardWeightPlusOffset(),
+                            target.forward_segment_id.id);
+    }
+
+    if (target.IsValidReverseTarget())
+    {
+        reverse_heap.Insert(target.reverse_segment_id.id,
+                            target.GetReverseWeightPlusOffset(),
+                            target.reverse_segment_id.id);
+    }
+}
+
+template <typename ManyToManyQueryHeap>
+void insertSourceInHeap(ManyToManyQueryHeap &heap, const PhantomNode &phantom_node)
 {
-    if (phantom_node.IsValidForwardTarget())
+    if (phantom_node.IsValidForwardSource())
     {
         heap.Insert(phantom_node.forward_segment_id.id,
                     -phantom_node.GetForwardWeightPlusOffset(),
                     {phantom_node.forward_segment_id.id, -phantom_node.GetForwardDuration()});
     }
-    if (phantom_node.IsValidReverseTarget())
+    if (phantom_node.IsValidReverseSource())
     {
         heap.Insert(phantom_node.reverse_segment_id.id,
                     -phantom_node.GetReverseWeightPlusOffset(),
@@ -64,9 +95,8 @@ void insertSourceInHeap(typename SearchEngineData<Algorithm>::ManyToManyQueryHea
     }
 }
 
-template <typename Algorithm>
+template <typename ManyToManyQueryHeap>
-void insertTargetInHeap(typename SearchEngineData<Algorithm>::ManyToManyQueryHeap &heap,
+void insertTargetInHeap(ManyToManyQueryHeap &heap, const PhantomNode &phantom_node)
-                        const PhantomNode &phantom_node)
 {
     if (phantom_node.IsValidForwardTarget())
     {
@@ -82,109 +112,6 @@ void insertTargetInHeap(typename SearchEngineData<Algorithm>::ManyToManyQueryHea
     }
 }
 
-template <typename Algorithm>
-void insertSourceInHeap(typename SearchEngineData<Algorithm>::QueryHeap &heap,
-                        const PhantomNode &phantom_node)
-{
-    if (phantom_node.IsValidForwardSource())
-    {
-        heap.Insert(phantom_node.forward_segment_id.id,
-                    -phantom_node.GetForwardWeightPlusOffset(),
-                    phantom_node.forward_segment_id.id);
-    }
-    if (phantom_node.IsValidReverseSource())
-    {
-        heap.Insert(phantom_node.reverse_segment_id.id,
-                    -phantom_node.GetReverseWeightPlusOffset(),
-                    phantom_node.reverse_segment_id.id);
-    }
-}
-
-template <typename Algorithm>
-void insertTargetInHeap(typename SearchEngineData<Algorithm>::QueryHeap &heap,
-                        const PhantomNode &phantom_node)
-{
-    if (phantom_node.IsValidForwardTarget())
-    {
-        heap.Insert(phantom_node.forward_segment_id.id,
-                    phantom_node.GetForwardWeightPlusOffset(),
-                    phantom_node.forward_segment_id.id);
-    }
-    if (phantom_node.IsValidReverseTarget())
-    {
-        heap.Insert(phantom_node.reverse_segment_id.id,
-                    phantom_node.GetReverseWeightPlusOffset(),
-                    phantom_node.reverse_segment_id.id);
-    }
-}
-} // namespace detail
-
-inline void insertTargetInHeap(typename SearchEngineData<mld::Algorithm>::ManyToManyQueryHeap &heap,
-                               const PhantomNode &phantom_node)
-{
-    detail::insertTargetInHeap<mld::Algorithm>(heap, phantom_node);
-}
-inline void insertTargetInHeap(typename SearchEngineData<ch::Algorithm>::ManyToManyQueryHeap &heap,
-                               const PhantomNode &phantom_node)
-{
-    detail::insertTargetInHeap<ch::Algorithm>(heap, phantom_node);
-}
-inline void insertTargetInHeap(typename SearchEngineData<mld::Algorithm>::QueryHeap &heap,
-                               const PhantomNode &phantom_node)
-{
-    detail::insertTargetInHeap<mld::Algorithm>(heap, phantom_node);
-}
-inline void insertTargetInHeap(typename SearchEngineData<ch::Algorithm>::QueryHeap &heap,
-                               const PhantomNode &phantom_node)
-{
-    detail::insertTargetInHeap<ch::Algorithm>(heap, phantom_node);
-}
-inline void insertSourceInHeap(typename SearchEngineData<mld::Algorithm>::ManyToManyQueryHeap &heap,
-                               const PhantomNode &phantom_node)
-{
-    detail::insertSourceInHeap<mld::Algorithm>(heap, phantom_node);
-}
-inline void insertSourceInHeap(typename SearchEngineData<ch::Algorithm>::ManyToManyQueryHeap &heap,
-                               const PhantomNode &phantom_node)
-{
-    detail::insertSourceInHeap<ch::Algorithm>(heap, phantom_node);
-}
-inline void insertSourceInHeap(typename SearchEngineData<mld::Algorithm>::QueryHeap &heap,
-                               const PhantomNode &phantom_node)
-{
-    detail::insertSourceInHeap<mld::Algorithm>(heap, phantom_node);
-}
-inline void insertSourceInHeap(typename SearchEngineData<ch::Algorithm>::QueryHeap &heap,
-                               const PhantomNode &phantom_node)
-{
-    detail::insertSourceInHeap<ch::Algorithm>(heap, phantom_node);
-}
-
-template <typename Heap>
-void insertNodesInHeaps(Heap &forward_heap, Heap &reverse_heap, const PhantomNodes &nodes)
-{
-    insertSourceInHeap(forward_heap, nodes.source_phantom);
-    insertTargetInHeap(reverse_heap, nodes.target_phantom);
-}
-
-template <typename Algorithm>
-void insertSourceInHeap(typename SearchEngineData<Algorithm>::ManyToManyQueryHeap &heap,
-                        const PhantomNode &phantom_node)
-{
-    if (phantom_node.IsValidForwardSource())
-    {
-        heap.Insert(phantom_node.forward_segment_id.id,
-                    -phantom_node.GetForwardWeightPlusOffset(),
-                    {phantom_node.forward_segment_id.id, -phantom_node.GetForwardDuration()});
-    }
-    if (phantom_node.IsValidReverseSource())
-    {
-        heap.Insert(phantom_node.reverse_segment_id.id,
-                    -phantom_node.GetReverseWeightPlusOffset(),
-                    {phantom_node.reverse_segment_id.id, -phantom_node.GetReverseDuration()});
-    }
-}
-
 template <typename FacadeT>
 void annotatePath(const FacadeT &facade,
                   const PhantomNodes &phantom_node_pair,
@@ -394,10 +321,58 @@ void annotatePath(const FacadeT &facade,
     }
 }
 
-EdgeDistance adjustPathDistanceToPhantomNodes(const std::vector<NodeID> &path,
+template <typename Algorithm>
-                                              const PhantomNode &source_phantom,
+double getPathDistance(const DataFacade<Algorithm> &facade,
-                                              const PhantomNode &target_phantom,
+                       const std::vector<PathData> unpacked_path,
-                                              const EdgeDistance distance);
+                       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>(util::toFloating(source_phantom.location.lat)) * DEGREE_TO_RAD;
+    double prev_lon =
+        static_cast<double>(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<double>(util::toFloating(current_coordinate.lat)) * DEGREE_TO_RAD;
+        const double current_lon =
+            static_cast<double>(util::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>(util::toFloating(target_phantom.location.lat)) * DEGREE_TO_RAD;
+    const double current_lon =
+        static_cast<double>(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;
+}
 
 template <typename AlgorithmT>
 InternalRouteResult extractRoute(const DataFacade<AlgorithmT> &facade,

include/engine/routing_algorithms/routing_base_mld.hpp

@@ -97,6 +97,7 @@ inline LevelID getNodeQueryLevel(const MultiLevelPartition &partition,
                                  const std::vector<std::size_t> &phantom_indices)
 {
     auto min_level = [&partition, node](const PhantomNode &phantom_node) {
+
         const auto &forward_segment = phantom_node.forward_segment_id;
         const auto forward_level =
             forward_segment.enabled ? partition.GetHighestDifferentLevel(node, forward_segment.id)
@@ -119,7 +120,7 @@ inline LevelID getNodeQueryLevel(const MultiLevelPartition &partition,
     }
     return result;
 }
-} // namespace
+}
 
 // Heaps only record for each node its predecessor ("parent") on the shortest path.
 // For re-constructing the actual path we need to trace back all parent "pointers".
@@ -390,27 +391,21 @@ UnpackedPath search(SearchEngineData<Algorithm> &engine_working_data,
                     EdgeWeight weight_upper_bound,
                     Args... args)
 {
-    if (forward_heap.Empty() && reverse_heap.Empty())
+    if (forward_heap.Empty() || reverse_heap.Empty())
     {
         return std::make_tuple(INVALID_EDGE_WEIGHT, std::vector<NodeID>(), std::vector<EdgeID>());
     }
 
     const auto &partition = facade.GetMultiLevelPartition();
 
-    BOOST_ASSERT(forward_heap.Empty() || forward_heap.MinKey() < INVALID_EDGE_WEIGHT);
+    BOOST_ASSERT(!forward_heap.Empty() && forward_heap.MinKey() < INVALID_EDGE_WEIGHT);
-    BOOST_ASSERT(reverse_heap.Empty() || reverse_heap.MinKey() < INVALID_EDGE_WEIGHT);
+    BOOST_ASSERT(!reverse_heap.Empty() && reverse_heap.MinKey() < INVALID_EDGE_WEIGHT);
 
     // run two-Target Dijkstra routing step.
     NodeID middle = SPECIAL_NODEID;
     EdgeWeight weight = weight_upper_bound;
-
+    EdgeWeight forward_heap_min = forward_heap.MinKey();
-    EdgeWeight forward_heap_min = 0;
+    EdgeWeight reverse_heap_min = reverse_heap.MinKey();
-    if (!forward_heap.Empty())
-        forward_heap_min = forward_heap.MinKey();
-    EdgeWeight reverse_heap_min = 0;
-    if (!reverse_heap.Empty())
-        reverse_heap_min = reverse_heap.MinKey();
-
     while (forward_heap.Size() + reverse_heap.Size() > 0 &&
            forward_heap_min + reverse_heap_min < weight)
     {
@@ -662,7 +657,11 @@ double getNetworkDistance(SearchEngineData<Algorithm> &engine_working_data,
                           const PhantomNode &target_phantom,
                           EdgeWeight weight_upper_bound = INVALID_EDGE_WEIGHT)
 {
+    forward_heap.Clear();
+    reverse_heap.Clear();
+
     const PhantomNodes phantom_nodes{source_phantom, target_phantom};
+    insertNodesInHeaps(forward_heap, reverse_heap, phantom_nodes);
 
     EdgeWeight weight = INVALID_EDGE_WEIGHT;
     std::vector<NodeID> unpacked_nodes;
@@ -681,22 +680,11 @@ double getNetworkDistance(SearchEngineData<Algorithm> &engine_working_data,
         return std::numeric_limits<double>::max();
     }
 
-    EdgeDistance distance = 0;
+    std::vector<PathData> unpacked_path;
 
-    if (!unpacked_nodes.empty())
+    annotatePath(facade, phantom_nodes, unpacked_nodes, unpacked_edges, unpacked_path);
-    {
-        distance = std::accumulate(unpacked_nodes.begin(),
-                                   std::prev(unpacked_nodes.end()),
-                                   EdgeDistance{0},
-                                   [&](const EdgeDistance distance, const auto node_id) {
-                                       return distance + computeEdgeDistance(facade, node_id);
-                                   });
-    }
 
-    distance = adjustPathDistanceToPhantomNodes(
+    return getPathDistance(facade, unpacked_path, source_phantom, target_phantom);
-        unpacked_nodes, phantom_nodes.source_phantom, phantom_nodes.target_phantom, distance);
-
-    return distance;
 }
 
 } // namespace mld

include/util/coordinate_calculation.hpp

@@ -23,6 +23,9 @@ namespace detail
 {
 const constexpr double DEGREE_TO_RAD = 0.017453292519943295769236907684886;
 const constexpr double RAD_TO_DEGREE = 1. / DEGREE_TO_RAD;
+// earth radius varies between 6,356.750-6,378.135 km (3,949.901-3,963.189mi)
+// The IUGG value for the equatorial radius is 6378.137 km (3963.19 miles)
+const constexpr long double EARTH_RADIUS = 6372797.560856;
 
 inline double degToRad(const double degree)
 {

src/engine/routing_algorithms/many_to_many_ch.cpp

@@ -240,12 +240,74 @@ void calculateDistances(typename SearchEngineData<ch::Algorithm>::ManyToManyQuer
         }
         if (!packed_leg.empty())
         {
-            EdgeDistance annotation =
+            auto annotation =
                 ch::calculateEBGNodeAnnotations(facade, packed_leg.begin(), packed_leg.end());
-            annotation = adjustPathDistanceToPhantomNodes(
-                packed_leg, source_phantom, target_phantom, annotation);
 
             distances_table[row_index * number_of_targets + column_index] = annotation;
+
+            // check the direction of travel to figure out how to calculate the offset to/from
+            // the source/target
+            if (source_phantom.forward_segment_id.id == packed_leg.front())
+            {
+                //       ............       <-- calculateEGBAnnotation returns distance from 0 to 3
+                //       -->s               <-- subtract offset to start at source
+                //          .........       <-- want this distance as result
+                // entry 0---1---2---3---   <-- 3 is exit node
+                EdgeDistance offset = source_phantom.GetForwardDistance();
+                distances_table[row_index * number_of_targets + column_index] -= offset;
+            }
+            else if (source_phantom.reverse_segment_id.id == packed_leg.front())
+            {
+                //       ............    <-- calculateEGBAnnotation returns distance from 0 to 3
+                //          s<-------    <-- subtract offset to start at source
+                //       ...             <-- want this distance
+                // entry 0---1---2---3   <-- 3 is exit node
+                EdgeDistance offset = source_phantom.GetReverseDistance();
+                distances_table[row_index * number_of_targets + column_index] -= offset;
+            }
+            if (target_phantom.forward_segment_id.id == packed_leg.back())
+            {
+                //       ............       <-- calculateEGBAnnotation returns distance from 0 to 3
+                //                   ++>t   <-- add offset to get to target
+                //       ................   <-- want this distance as result
+                // entry 0---1---2---3---   <-- 3 is exit node
+                EdgeDistance offset = target_phantom.GetForwardDistance();
+                distances_table[row_index * number_of_targets + column_index] += offset;
+            }
+            else if (target_phantom.reverse_segment_id.id == packed_leg.back())
+            {
+                //       ............       <-- calculateEGBAnnotation returns distance from 0 to 3
+                //                   <++t   <-- add offset to get from target
+                //       ................   <-- want this distance as result
+                // entry 0---1---2---3---   <-- 3 is exit node
+                EdgeDistance offset = target_phantom.GetReverseDistance();
+                distances_table[row_index * number_of_targets + column_index] += offset;
+            }
+        }
+        else
+        {
+            // there is no shortcut to unpack. source and target are on the same EBG Node.
+            // if the offset of the target is greater than the offset of the source, subtract it
+            if (target_phantom.GetForwardDistance() > source_phantom.GetForwardDistance())
+            {
+                //       --------->t        <-- offsets
+                //       ->s                <-- subtract source offset from target offset
+                //         .........        <-- want this distance as result
+                // entry 0---1---2---3---   <-- 3 is exit node
+                EdgeDistance offset =
+                    target_phantom.GetForwardDistance() - source_phantom.GetForwardDistance();
+                distances_table[row_index * number_of_targets + column_index] = offset;
+            }
+            else
+            {
+                //               s<---      <-- offsets
+                //         t<---------      <-- subtract source offset from target offset
+                //         ......           <-- want this distance as result
+                // entry 0---1---2---3---   <-- 3 is exit node
+                EdgeDistance offset =
+                    target_phantom.GetReverseDistance() - source_phantom.GetReverseDistance();
+                distances_table[row_index * number_of_targets + column_index] = offset;
+            }
         }
         packed_leg.clear();
     }

src/engine/routing_algorithms/map_matching.cpp

@@ -227,9 +227,6 @@ SubMatchingList mapMatching(SearchEngineData<Algorithm> &engine_working_data,
                 {
                     continue;
                 }
-                forward_heap.Clear();
-                const auto &source_phantom = prev_unbroken_timestamps_list[s].phantom_node;
-                insertSourceInHeap(forward_heap, source_phantom);
 
                 for (const auto s_prime : util::irange<std::size_t>(0UL, current_viterbi.size()))
                 {
@@ -240,19 +237,14 @@ SubMatchingList mapMatching(SearchEngineData<Algorithm> &engine_working_data,
                         continue;
                     }
 
-                    reverse_heap.Clear();
+                    double network_distance =
-                    const auto &target_phantom = current_timestamps_list[s_prime].phantom_node;
+                        getNetworkDistance(engine_working_data,
-                    insertTargetInHeap(reverse_heap, target_phantom);
+                                           facade,
-
+                                           forward_heap,
-                    double network_distance = getNetworkDistance(engine_working_data,
+                                           reverse_heap,
-                                                                 facade,
+                                           prev_unbroken_timestamps_list[s].phantom_node,
-                                                                 forward_heap,
+                                           current_timestamps_list[s_prime].phantom_node,
-                                                                 reverse_heap,
+                                           weight_upper_bound);
-                                                                 source_phantom,
-                                                                 target_phantom,
-                                                                 weight_upper_bound);
-
-                    network_distance = std::round(network_distance * 10) / 10;
 
                     // get distance diff between loc1/2 and locs/s_prime
                     const auto d_t = std::abs(network_distance - haversine_distance);

src/engine/routing_algorithms/routing_base.cpp

@@ -33,79 +33,6 @@ bool needsLoopBackwards(const PhantomNodes &phantoms)
     return needsLoopBackwards(phantoms.source_phantom, phantoms.target_phantom);
 }
 
-EdgeDistance adjustPathDistanceToPhantomNodes(const std::vector<NodeID> &path,
-                                              const PhantomNode &source_phantom,
-                                              const PhantomNode &target_phantom,
-                                              const EdgeDistance uncorrected_distance)
-{
-    EdgeDistance distance = uncorrected_distance;
-    if (!path.empty())
-    {
-
-        // check the direction of travel to figure out how to calculate the offset to/from
-        // the source/target
-        if (source_phantom.forward_segment_id.id == path.front())
-        {
-            //       ............       <-- calculateEGBAnnotation returns distance from 0 to 3
-            //       -->s               <-- subtract offset to start at source
-            //          .........       <-- want this distance as result
-            // entry 0---1---2---3---   <-- 3 is exit node
-            distance -= source_phantom.GetForwardDistance();
-        }
-        else if (source_phantom.reverse_segment_id.id == path.front())
-        {
-            //       ............    <-- calculateEGBAnnotation returns distance from 0 to 3
-            //          s<-------    <-- subtract offset to start at source
-            //       ...             <-- want this distance
-            // entry 0---1---2---3   <-- 3 is exit node
-            distance -= source_phantom.GetReverseDistance();
-        }
-        if (target_phantom.forward_segment_id.id == path.back())
-        {
-            //       ............       <-- calculateEGBAnnotation returns distance from 0 to 3
-            //                   ++>t   <-- add offset to get to target
-            //       ................   <-- want this distance as result
-            // entry 0---1---2---3---   <-- 3 is exit node
-            distance += target_phantom.GetForwardDistance();
-        }
-        else if (target_phantom.reverse_segment_id.id == path.back())
-        {
-            //       ............       <-- calculateEGBAnnotation returns distance from 0 to 3
-            //                   <++t   <-- add offset to get from target
-            //       ................   <-- want this distance as result
-            // entry 0---1---2---3---   <-- 3 is exit node
-            distance += target_phantom.GetReverseDistance();
-        }
-    }
-    else
-    {
-        // there is no shortcut to unpack. source and target are on the same EBG Node.
-        // if the offset of the target is greater than the offset of the source, subtract it
-        if (target_phantom.GetForwardDistance() > source_phantom.GetForwardDistance())
-        {
-            //       --------->t        <-- offsets
-            //       ->s                <-- subtract source offset from target offset
-            //         .........        <-- want this distance as result
-            // entry 0---1---2---3---   <-- 3 is exit node
-            distance = target_phantom.GetForwardDistance() - source_phantom.GetForwardDistance();
-        }
-        else
-        {
-            //               s<---      <-- offsets
-            //         t<---------      <-- subtract source offset from target offset
-            //         ......           <-- want this distance as result
-            // entry 0---1---2---3---   <-- 3 is exit node
-            distance = target_phantom.GetReverseDistance() - source_phantom.GetReverseDistance();
-        }
-    }
-
-    BOOST_ASSERT_MSG(distance >= 0 || distance > -1.0f,
-                     "Distance correction generated negative number");
-    // guard against underflow errors caused by rounding
-    distance = std::max(EdgeDistance{0}, distance);
-    return distance;
-}
-
 } // namespace routing_algorithms
 } // namespace engine
 } // namespace osrm

src/engine/routing_algorithms/routing_base_ch.cpp

@@ -100,7 +100,7 @@ void search(SearchEngineData<Algorithm> & /*engine_working_data*/,
             const PhantomNodes & /*phantom_nodes*/,
             const EdgeWeight weight_upper_bound)
 {
-    if (forward_heap.Empty() && reverse_heap.Empty())
+    if (forward_heap.Empty() || reverse_heap.Empty())
     {
         weight = INVALID_EDGE_WEIGHT;
         return;
@@ -110,14 +110,10 @@ void search(SearchEngineData<Algorithm> & /*engine_working_data*/,
     weight = weight_upper_bound;
 
     // get offset to account for offsets on phantom nodes on compressed edges
-    EdgeWeight min_edge_offset = 0;
+    const auto min_edge_offset = std::min(0, forward_heap.MinKey());
-    if (forward_heap.Size() > 0)
+    BOOST_ASSERT(min_edge_offset <= 0);
-    {
-        min_edge_offset = std::min(min_edge_offset, forward_heap.MinKey());
-        BOOST_ASSERT(min_edge_offset <= 0);
-    }
     // we only every insert negative offsets for nodes in the forward heap
-    BOOST_ASSERT(reverse_heap.Empty() || reverse_heap.MinKey() >= 0);
+    BOOST_ASSERT(reverse_heap.MinKey() >= 0);
 
     // run two-Target Dijkstra routing step.
     while (0 < (forward_heap.Size() + reverse_heap.Size()))
@@ -180,6 +176,11 @@ double getNetworkDistance(SearchEngineData<Algorithm> &engine_working_data,
                           const PhantomNode &target_phantom,
                           EdgeWeight weight_upper_bound)
 {
+    forward_heap.Clear();
+    reverse_heap.Clear();
+
+    insertNodesInHeaps(forward_heap, reverse_heap, {source_phantom, target_phantom});
+
     EdgeWeight weight = INVALID_EDGE_WEIGHT;
     std::vector<NodeID> packed_path;
     search(engine_working_data,
@@ -198,31 +199,14 @@ double getNetworkDistance(SearchEngineData<Algorithm> &engine_working_data,
         return std::numeric_limits<double>::max();
     }
 
-    EdgeDistance distance = 0;
+    std::vector<PathData> unpacked_path;
+    unpackPath(facade,
+               packed_path.begin(),
+               packed_path.end(),
+               {source_phantom, target_phantom},
+               unpacked_path);
 
-    std::vector<NodeID> unpacked_nodes;
+    return getPathDistance(facade, unpacked_path, source_phantom, target_phantom);
-    unpacked_nodes.reserve(packed_path.size());
-    if (!packed_path.empty())
-    {
-        unpacked_nodes.push_back(packed_path.front());
-        unpackPath(
-            facade, packed_path.begin(), packed_path.end(), [&](const auto &edge, const auto &) {
-                BOOST_ASSERT(edge.first == unpacked_nodes.back());
-                unpacked_nodes.push_back(edge.second);
-            });
-
-        distance = std::accumulate(unpacked_nodes.begin(),
-                                   std::prev(unpacked_nodes.end()),
-                                   EdgeDistance{0},
-                                   [&](const EdgeDistance distance, const auto node_id) {
-                                       return distance + computeEdgeDistance(facade, node_id);
-                                   });
-    }
-
-    distance =
-        adjustPathDistanceToPhantomNodes(unpacked_nodes, source_phantom, target_phantom, distance);
-
-    return distance;
 }
 } // namespace ch
 

src/util/coordinate_calculation.cpp

@@ -22,11 +22,6 @@ namespace coordinate_calculation
 
 namespace
 {
-
-// earth radius varies between 6,356.750-6,378.135 km (3,949.901-3,963.189mi)
-// The IUGG value for the equatorial radius is 6378.137 km (3963.19 miles)
-const constexpr double EARTH_RADIUS = 6372797.560856;
-
 class CheapRulerContainer
 {
   public:
@@ -117,7 +112,7 @@ double haversineDistance(const Coordinate coordinate_1, const Coordinate coordin
     const double aharv = std::pow(std::sin(dlat / 2.0), 2.0) +
                          std::cos(dlat1) * std::cos(dlat2) * std::pow(std::sin(dlong / 2.), 2);
     const double charv = 2. * std::atan2(std::sqrt(aharv), std::sqrt(1.0 - aharv));
-    return EARTH_RADIUS * charv;
+    return detail::EARTH_RADIUS * charv;
 }
 
 double greatCircleDistance(const Coordinate coordinate_1, const Coordinate coordinate_2)
@@ -138,7 +133,7 @@ double greatCircleDistance(const Coordinate coordinate_1, const Coordinate coord
 
     const double x_value = (float_lon2 - float_lon1) * std::cos((float_lat1 + float_lat2) / 2.0);
     const double y_value = float_lat2 - float_lat1;
-    return std::hypot(x_value, y_value) * EARTH_RADIUS;
+    return std::hypot(x_value, y_value) * detail::EARTH_RADIUS;
 }
 
 double perpendicularDistance(const Coordinate segment_source,