Adapt shortest_path and viaroute to core search

include/engine/routing_algorithms/map_matching.hpp

@@ -123,9 +123,13 @@ class MapMatching final : public BasicRoutingInterface<DataFacadeT, MapMatching<
 
         engine_working_data.InitializeOrClearFirstThreadLocalStorage(
             super::facade->GetNumberOfNodes());
+        engine_working_data.InitializeOrClearSecondThreadLocalStorage(
+            super::facade->GetNumberOfNodes());
 
         QueryHeap &forward_heap = *(engine_working_data.forward_heap_1);
         QueryHeap &reverse_heap = *(engine_working_data.reverse_heap_1);
+        QueryHeap &forward_core_heap = *(engine_working_data.forward_heap_2);
+        QueryHeap &reverse_core_heap = *(engine_working_data.reverse_heap_2);
 
         std::size_t breakage_begin = map_matching::INVALID_STATE;
         std::vector<std::size_t> split_points;
@@ -221,9 +225,23 @@ class MapMatching final : public BasicRoutingInterface<DataFacadeT, MapMatching<
                     reverse_heap.Clear();
 
                     // get distance diff between loc1/2 and locs/s_prime
-                    const auto network_distance = super::get_network_distance(
+                    double network_distance;
-                        forward_heap, reverse_heap, prev_unbroken_timestamps_list[s].phantom_node,
+                    if (super::facade->GetCoreSize() > 0)
+                    {
+                        forward_core_heap.Clear();
+                        reverse_core_heap.Clear();
+                        network_distance = super::GetNetworkDistanceWithCore(
+                            forward_heap, reverse_heap, forward_core_heap, reverse_core_heap,
+                            prev_unbroken_timestamps_list[s].phantom_node,
                             current_timestamps_list[s_prime].phantom_node);
+                    }
+                    else
+                    {
+                        network_distance = super::GetNetworkDistance(
+                            forward_heap, reverse_heap,
+                            prev_unbroken_timestamps_list[s].phantom_node,
+                            current_timestamps_list[s_prime].phantom_node);
+                    }
 
                     const auto d_t = std::abs(network_distance - haversine_distance);
 

include/engine/routing_algorithms/routing_base.hpp

@@ -727,20 +727,59 @@ template <class DataFacadeT, class Derived> class BasicRoutingInterface
         }
     }
 
+    bool NeedsLoopForward(const PhantomNode &source_phantom,
+                          const PhantomNode &target_phantom) const
+    {
+        return source_phantom.forward_node_id == target_phantom.forward_node_id &&
+               source_phantom.GetForwardWeightPlusOffset() >
+                   target_phantom.GetForwardWeightPlusOffset();
+    }
+
+    bool NeedsLoopBackwards(const PhantomNode &source_phantom,
+                            const PhantomNode &target_phantom) const
+    {
+        return source_phantom.reverse_node_id == target_phantom.reverse_node_id &&
+               source_phantom.GetReverseWeightPlusOffset() >
+                   target_phantom.GetReverseWeightPlusOffset();
+    }
+
+    double GetPathDistance(const std::vector<NodeID> &packed_path,
+                           const PhantomNode &source_phantom,
+                           const PhantomNode &target_phantom) const
+    {
+        std::vector<PathData> unpacked_path;
+        PhantomNodes nodes;
+        nodes.source_phantom = source_phantom;
+        nodes.target_phantom = target_phantom;
+        UnpackPath(packed_path.begin(), packed_path.end(), nodes, unpacked_path);
+
+        util::FixedPointCoordinate previous_coordinate = source_phantom.location;
+        util::FixedPointCoordinate current_coordinate;
+        double distance = 0;
+        for (const auto &p : unpacked_path)
+        {
+            current_coordinate = facade->GetCoordinateOfNode(p.node);
+            distance += util::coordinate_calculation::haversineDistance(previous_coordinate,
+                                                                        current_coordinate);
+            previous_coordinate = current_coordinate;
+        }
+        distance += util::coordinate_calculation::haversineDistance(previous_coordinate,
+                                                                    target_phantom.location);
+        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
-    double get_network_distance(SearchEngineData::QueryHeap &forward_heap,
+    double GetNetworkDistanceWithCore(SearchEngineData::QueryHeap &forward_heap,
                                       SearchEngineData::QueryHeap &reverse_heap,
+                                      SearchEngineData::QueryHeap &forward_core_heap,
+                                      SearchEngineData::QueryHeap &reverse_core_heap,
                                       const PhantomNode &source_phantom,
                                       const PhantomNode &target_phantom) const
     {
         BOOST_ASSERT(forward_heap.Empty());
         BOOST_ASSERT(reverse_heap.Empty());
-        EdgeWeight upper_bound = INVALID_EDGE_WEIGHT;
-        NodeID middle_node = SPECIAL_NODEID;
-        EdgeWeight edge_offset = std::min(0, -source_phantom.GetForwardWeightPlusOffset());
-        edge_offset = std::min(edge_offset, -source_phantom.GetReverseWeightPlusOffset());
 
         if (source_phantom.forward_node_id != SPECIAL_NODEID)
         {
@@ -768,61 +807,73 @@ template <class DataFacadeT, class Derived> class BasicRoutingInterface
                                 target_phantom.reverse_node_id);
         }
 
-        // search from s and t till new_min/(1+epsilon) > length_of_shortest_path
+        const bool constexpr DO_NOT_FORCE_LOOPS =
-        const constexpr bool STALLING_ENABLED = true;
+            false; // prevents forcing of loops, since offsets are set correctly
-        const constexpr bool DO_NOT_FORCE_LOOPS = false;
+
-        while (0 < (forward_heap.Size() + reverse_heap.Size()))
+        int duration = INVALID_EDGE_WEIGHT;
-        {
+        std::vector<NodeID> packed_path;
-            if (0 < forward_heap.Size())
+        SearchWithCore(forward_heap, reverse_heap, forward_core_heap, reverse_core_heap, duration,
-            {
+                       packed_path, DO_NOT_FORCE_LOOPS, DO_NOT_FORCE_LOOPS);
-                RoutingStep(forward_heap, reverse_heap, middle_node, upper_bound, edge_offset, true,
-                            STALLING_ENABLED, DO_NOT_FORCE_LOOPS, DO_NOT_FORCE_LOOPS);
-            }
-            if (0 < reverse_heap.Size())
-            {
-                RoutingStep(reverse_heap, forward_heap, middle_node, upper_bound, edge_offset,
-                            false, STALLING_ENABLED, DO_NOT_FORCE_LOOPS, DO_NOT_FORCE_LOOPS);
-            }
-        }
 
         double distance = std::numeric_limits<double>::max();
-        if (upper_bound != INVALID_EDGE_WEIGHT)
+        if (duration != INVALID_EDGE_WEIGHT)
         {
-            std::vector<NodeID> packed_leg;
+            return GetPathDistance(packed_path, source_phantom, target_phantom);
-            if (upper_bound != forward_heap.GetKey(middle_node) + reverse_heap.GetKey(middle_node))
-            {
-                // self loop
-                BOOST_ASSERT(forward_heap.GetData(middle_node).parent == middle_node &&
-                             reverse_heap.GetData(middle_node).parent == middle_node);
-                packed_leg.push_back(middle_node);
-                packed_leg.push_back(middle_node);
-            }
-            else
-            {
-                RetrievePackedPathFromHeap(forward_heap, reverse_heap, middle_node, packed_leg);
-            }
-
-            std::vector<PathData> unpacked_path;
-            PhantomNodes nodes;
-            nodes.source_phantom = source_phantom;
-            nodes.target_phantom = target_phantom;
-            UnpackPath(packed_leg.begin(), packed_leg.end(), nodes, unpacked_path);
-
-            util::FixedPointCoordinate previous_coordinate = source_phantom.location;
-            util::FixedPointCoordinate current_coordinate;
-            distance = 0;
-            for (const auto &p : unpacked_path)
-            {
-                current_coordinate = facade->GetCoordinateOfNode(p.node);
-                distance += util::coordinate_calculation::haversineDistance(previous_coordinate,
-                                                                            current_coordinate);
-                previous_coordinate = current_coordinate;
-            }
-            distance += util::coordinate_calculation::haversineDistance(previous_coordinate,
-                                                                        target_phantom.location);
         }
         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
+    double GetNetworkDistance(SearchEngineData::QueryHeap &forward_heap,
+                              SearchEngineData::QueryHeap &reverse_heap,
+                              const PhantomNode &source_phantom,
+                              const PhantomNode &target_phantom) const
+    {
+        BOOST_ASSERT(forward_heap.Empty());
+        BOOST_ASSERT(reverse_heap.Empty());
+
+        if (source_phantom.forward_node_id != SPECIAL_NODEID)
+        {
+            forward_heap.Insert(source_phantom.forward_node_id,
+                                -source_phantom.GetForwardWeightPlusOffset(),
+                                source_phantom.forward_node_id);
+        }
+        if (source_phantom.reverse_node_id != SPECIAL_NODEID)
+        {
+            forward_heap.Insert(source_phantom.reverse_node_id,
+                                -source_phantom.GetReverseWeightPlusOffset(),
+                                source_phantom.reverse_node_id);
+        }
+
+        if (target_phantom.forward_node_id != SPECIAL_NODEID)
+        {
+            reverse_heap.Insert(target_phantom.forward_node_id,
+                                target_phantom.GetForwardWeightPlusOffset(),
+                                target_phantom.forward_node_id);
+        }
+        if (target_phantom.reverse_node_id != SPECIAL_NODEID)
+        {
+            reverse_heap.Insert(target_phantom.reverse_node_id,
+                                target_phantom.GetReverseWeightPlusOffset(),
+                                target_phantom.reverse_node_id);
+        }
+
+        const bool constexpr DO_NOT_FORCE_LOOPS =
+            false; // prevents forcing of loops, since offsets are set correctly
+
+        int duration = INVALID_EDGE_WEIGHT;
+        std::vector<NodeID> packed_path;
+        Search(forward_heap, reverse_heap, duration, packed_path, DO_NOT_FORCE_LOOPS, DO_NOT_FORCE_LOOPS);
+
+        if (duration == INVALID_EDGE_WEIGHT)
+        {
+            return std::numeric_limits<double>::max();
+        }
+
+        return GetPathDistance(packed_path, source_phantom, target_phantom);
+    }
 };
 }
 }

include/engine/routing_algorithms/shortest_path.hpp

@@ -24,8 +24,6 @@ class ShortestPathRouting final
     using super = BasicRoutingInterface<DataFacadeT, ShortestPathRouting<DataFacadeT>>;
     using QueryHeap = SearchEngineData::QueryHeap;
     SearchEngineData &engine_working_data;
-    const static constexpr bool FORWARD_DIRECTION = true;
-    const static constexpr bool REVERSE_DIRECTION = false;
     const static constexpr bool DO_NOT_FORCE_LOOP = false;
 
   public:
@@ -36,23 +34,12 @@ class ShortestPathRouting final
 
     ~ShortestPathRouting() {}
 
-    inline bool
-    forceLoop(bool forward, const PhantomNode &source_phantom, const PhantomNode &target_phantom) const
-    {
-        if (forward)
-            return source_phantom.forward_node_id == target_phantom.forward_node_id &&
-                   source_phantom.GetForwardWeightPlusOffset() >
-                       target_phantom.GetForwardWeightPlusOffset();
-        else
-            return source_phantom.reverse_node_id == target_phantom.reverse_node_id &&
-                   source_phantom.GetReverseWeightPlusOffset() >
-                       target_phantom.GetReverseWeightPlusOffset();
-    };
-
     // allows a uturn at the target_phantom
     // searches source forward/reverse -> target forward/reverse
     void SearchWithUTurn(QueryHeap &forward_heap,
                          QueryHeap &reverse_heap,
+                         QueryHeap &forward_core_heap,
+                         QueryHeap &reverse_core_heap,
                          const bool search_from_forward_node,
                          const bool search_from_reverse_node,
                          const bool search_to_forward_node,
@@ -95,9 +82,24 @@ class ShortestPathRouting final
 
         BOOST_ASSERT(forward_heap.Size() > 0);
         BOOST_ASSERT(reverse_heap.Size() > 0);
+
+        if (super::facade->GetCoreSize() > 0)
+        {
+            forward_core_heap.Clear();
+            reverse_core_heap.Clear();
+            BOOST_ASSERT(forward_core_heap.Size() == 0);
+            BOOST_ASSERT(reverse_core_heap.Size() == 0);
+            super::SearchWithCore(forward_heap, reverse_heap, forward_core_heap, reverse_core_heap,
+                                  new_total_distance, leg_packed_path,
+                                  super::NeedsLoopForward(source_phantom, target_phantom),
+                                  super::NeedsLoopBackwards(source_phantom, target_phantom));
+        }
+        else
+        {
             super::Search(forward_heap, reverse_heap, new_total_distance, leg_packed_path,
-                      forceLoop(FORWARD_DIRECTION, source_phantom, target_phantom),
+                          super::NeedsLoopForward(source_phantom, target_phantom),
-                      forceLoop(REVERSE_DIRECTION, source_phantom, target_phantom));
+                          super::NeedsLoopBackwards(source_phantom, target_phantom));
+        }
     }
 
     // searches shortest path between:
@@ -105,6 +107,8 @@ class ShortestPathRouting final
     // source forward/reverse -> target reverse
     void Search(QueryHeap &forward_heap,
                 QueryHeap &reverse_heap,
+                QueryHeap &forward_core_heap,
+                QueryHeap &reverse_core_heap,
                 const bool search_from_forward_node,
                 const bool search_from_reverse_node,
                 const bool search_to_forward_node,
@@ -142,9 +146,25 @@ class ShortestPathRouting final
             }
             BOOST_ASSERT(forward_heap.Size() > 0);
             BOOST_ASSERT(reverse_heap.Size() > 0);
-            super::Search(
+
-                forward_heap, reverse_heap, new_total_distance_to_forward, leg_packed_path_forward,
+            if (super::facade->GetCoreSize() > 0)
-                forceLoop(FORWARD_DIRECTION, source_phantom, target_phantom), DO_NOT_FORCE_LOOP);
+            {
+                forward_core_heap.Clear();
+                reverse_core_heap.Clear();
+                BOOST_ASSERT(forward_core_heap.Size() == 0);
+                BOOST_ASSERT(reverse_core_heap.Size() == 0);
+                super::SearchWithCore(
+                    forward_heap, reverse_heap, forward_core_heap, reverse_core_heap,
+                    new_total_distance_to_forward, leg_packed_path_forward,
+                    super::NeedsLoopForward(source_phantom, target_phantom), DO_NOT_FORCE_LOOP);
+            }
+            else
+            {
+                super::Search(forward_heap, reverse_heap, new_total_distance_to_forward,
+                              leg_packed_path_forward,
+                              super::NeedsLoopForward(source_phantom, target_phantom),
+                              DO_NOT_FORCE_LOOP);
+            }
         }
 
         if (search_to_reverse_node)
@@ -170,9 +190,23 @@ class ShortestPathRouting final
             }
             BOOST_ASSERT(forward_heap.Size() > 0);
             BOOST_ASSERT(reverse_heap.Size() > 0);
+            if (super::facade->GetCoreSize() > 0)
+            {
+                forward_core_heap.Clear();
+                reverse_core_heap.Clear();
+                BOOST_ASSERT(forward_core_heap.Size() == 0);
+                BOOST_ASSERT(reverse_core_heap.Size() == 0);
+                super::SearchWithCore(forward_heap, reverse_heap, forward_core_heap,
+                                      reverse_core_heap, new_total_distance_to_reverse,
+                                      leg_packed_path_reverse, DO_NOT_FORCE_LOOP,
+                                      super::NeedsLoopBackwards(source_phantom, target_phantom));
+            }
+            else
+            {
                 super::Search(forward_heap, reverse_heap, new_total_distance_to_reverse,
                               leg_packed_path_reverse, DO_NOT_FORCE_LOOP,
-                          forceLoop(REVERSE_DIRECTION, source_phantom, target_phantom));
+                              super::NeedsLoopBackwards(source_phantom, target_phantom));
+            }
         }
     }
 
@@ -209,9 +243,13 @@ class ShortestPathRouting final
         BOOST_ASSERT(uturn_indicators.size() == phantom_nodes_vector.size() + 1);
         engine_working_data.InitializeOrClearFirstThreadLocalStorage(
             super::facade->GetNumberOfNodes());
+        engine_working_data.InitializeOrClearSecondThreadLocalStorage(
+            super::facade->GetNumberOfNodes());
 
         QueryHeap &forward_heap = *(engine_working_data.forward_heap_1);
         QueryHeap &reverse_heap = *(engine_working_data.reverse_heap_1);
+        QueryHeap &forward_core_heap = *(engine_working_data.forward_heap_2);
+        QueryHeap &reverse_core_heap = *(engine_working_data.reverse_heap_2);
 
         int total_distance_to_forward = 0;
         int total_distance_to_reverse = 0;
@@ -259,7 +297,8 @@ class ShortestPathRouting final
             {
                 if (allow_u_turn_at_via)
                 {
-                    SearchWithUTurn(forward_heap, reverse_heap, search_from_forward_node,
+                    SearchWithUTurn(forward_heap, reverse_heap, forward_core_heap,
+                                    reverse_core_heap, search_from_forward_node,
                                     search_from_reverse_node, search_to_forward_node,
                                     search_to_reverse_node, source_phantom, target_phantom,
                                     total_distance_to_forward, total_distance_to_reverse,
@@ -281,12 +320,12 @@ class ShortestPathRouting final
                 }
                 else
                 {
-                    Search(forward_heap, reverse_heap, search_from_forward_node,
+                    Search(forward_heap, reverse_heap, forward_core_heap, reverse_core_heap,
-                           search_from_reverse_node, search_to_forward_node, search_to_reverse_node,
+                           search_from_forward_node, search_from_reverse_node,
-                           source_phantom, target_phantom, total_distance_to_forward,
+                           search_to_forward_node, search_to_reverse_node, source_phantom,
-                           total_distance_to_reverse, new_total_distance_to_forward,
+                           target_phantom, total_distance_to_forward, total_distance_to_reverse,
-                           new_total_distance_to_reverse, packed_leg_to_forward,
+                           new_total_distance_to_forward, new_total_distance_to_reverse,
-                           packed_leg_to_reverse);
+                           packed_leg_to_forward, packed_leg_to_reverse);
                 }
             }