Merge pull request #5894 from xlaussel/avoid_samelookup_in_heap_map

Avoid samelookup in heap map

Author: akashihi

include/engine/routing_algorithms/routing_base_ch.hpp

@@ -24,21 +24,21 @@ namespace ch
 // Stalling
 template <bool DIRECTION, typename HeapT>
 bool stallAtNode(const DataFacade<Algorithm> &facade,
-                 const NodeID node,
-                 const EdgeWeight weight,
+                 const typename HeapT::HeapNode &heapNode,
                  const HeapT &query_heap)
 {
-    for (auto edge : facade.GetAdjacentEdgeRange(node))
+    for (auto edge : facade.GetAdjacentEdgeRange(heapNode.node))
     {
         const auto &data = facade.GetEdgeData(edge);
         if (DIRECTION == REVERSE_DIRECTION ? data.forward : data.backward)
         {
             const NodeID to = facade.GetTarget(edge);
             const EdgeWeight edge_weight = data.weight;
             BOOST_ASSERT_MSG(edge_weight > 0, "edge_weight invalid");
-            if (query_heap.WasInserted(to))
+            const auto toHeapNode = query_heap.GetHeapNodeIfWasInserted(to);
+            if (toHeapNode)
             {
-                if (query_heap.GetKey(to) + edge_weight < weight)
+                if (toHeapNode->weight + edge_weight < heapNode.weight)
                 {
                     return true;
                 }
@@ -50,11 +50,10 @@ bool stallAtNode(const DataFacade<Algorithm> &facade,
 
 template <bool DIRECTION>
 void relaxOutgoingEdges(const DataFacade<Algorithm> &facade,
-                        const NodeID node,
-                        const EdgeWeight weight,
+                        const SearchEngineData<Algorithm>::QueryHeap::HeapNode &heapNode,
                         SearchEngineData<Algorithm>::QueryHeap &heap)
 {
-    for (const auto edge : facade.GetAdjacentEdgeRange(node))
+    for (const auto edge : facade.GetAdjacentEdgeRange(heapNode.node))
     {
         const auto &data = facade.GetEdgeData(edge);
         if (DIRECTION == FORWARD_DIRECTION ? data.forward : data.backward)
@@ -63,19 +62,21 @@ void relaxOutgoingEdges(const DataFacade<Algorithm> &facade,
             const EdgeWeight edge_weight = data.weight;
 
             BOOST_ASSERT_MSG(edge_weight > 0, "edge_weight invalid");
-            const EdgeWeight to_weight = weight + edge_weight;
+            const EdgeWeight to_weight = heapNode.weight + edge_weight;
 
+            const auto toHeapNode = heap.GetHeapNodeIfWasInserted(to);
             // New Node discovered -> Add to Heap + Node Info Storage
-            if (!heap.WasInserted(to))
+            if (!toHeapNode)
             {
-                heap.Insert(to, to_weight, node);
+                heap.Insert(to, to_weight, heapNode.node);
             }
             // Found a shorter Path -> Update weight
-            else if (to_weight < heap.GetKey(to))
+            else if (to_weight < toHeapNode->weight)
             {
                 // new parent
-                heap.GetData(to).parent = node;
-                heap.DecreaseKey(to, to_weight);
+                toHeapNode->data.parent = heapNode.node;
+                toHeapNode->weight = to_weight;
+                heap.DecreaseKey(*toHeapNode);
             }
         }
     }
@@ -122,35 +123,35 @@ void routingStep(const DataFacade<Algorithm> &facade,
                  const bool force_loop_forward,
                  const bool force_loop_reverse)
 {
-    const NodeID node = forward_heap.DeleteMin();
-    const EdgeWeight weight = forward_heap.GetKey(node);
+    auto heapNode = forward_heap.DeleteMinGetHeapNode();
+    const auto reverseHeapNode = reverse_heap.GetHeapNodeIfWasInserted(heapNode.node);
 
-    if (reverse_heap.WasInserted(node))
+    if (reverseHeapNode)
     {
-        const EdgeWeight new_weight = reverse_heap.GetKey(node) + weight;
+        const EdgeWeight new_weight = reverseHeapNode->weight + heapNode.weight;
         if (new_weight < upper_bound)
         {
             // if loops are forced, they are so at the source
-            if ((force_loop_forward && forward_heap.GetData(node).parent == node) ||
-                (force_loop_reverse && reverse_heap.GetData(node).parent == node) ||
+            if ((force_loop_forward && heapNode.data.parent == heapNode.node) ||
+                (force_loop_reverse && reverseHeapNode->data.parent == heapNode.node) ||
                 // in this case we are looking at a bi-directional way where the source
                 // and target phantom are on the same edge based node
                 new_weight < 0)
             {
                 // check whether there is a loop present at the node
-                for (const auto edge : facade.GetAdjacentEdgeRange(node))
+                for (const auto edge : facade.GetAdjacentEdgeRange(heapNode.node))
                 {
                     const auto &data = facade.GetEdgeData(edge);
                     if (DIRECTION == FORWARD_DIRECTION ? data.forward : data.backward)
                     {
                         const NodeID to = facade.GetTarget(edge);
-                        if (to == node)
+                        if (to == heapNode.node)
                         {
                             const EdgeWeight edge_weight = data.weight;
                             const EdgeWeight loop_weight = new_weight + edge_weight;
                             if (loop_weight >= 0 && loop_weight < upper_bound)
                             {
-                                middle_node_id = node;
+                                middle_node_id = heapNode.node;
                                 upper_bound = loop_weight;
                             }
                         }
@@ -161,7 +162,7 @@ void routingStep(const DataFacade<Algorithm> &facade,
             {
                 BOOST_ASSERT(new_weight >= 0);
 
-                middle_node_id = node;
+                middle_node_id = heapNode.node;
                 upper_bound = new_weight;
             }
         }
@@ -170,19 +171,19 @@ void routingStep(const DataFacade<Algorithm> &facade,
     // make sure we don't terminate too early if we initialize the weight
     // for the nodes in the forward heap with the forward/reverse offset
     BOOST_ASSERT(min_edge_offset <= 0);
-    if (weight + min_edge_offset > upper_bound)
+    if (heapNode.weight + min_edge_offset > upper_bound)
     {
         forward_heap.DeleteAll();
         return;
     }
 
     // Stalling
-    if (STALLING && stallAtNode<DIRECTION>(facade, node, weight, forward_heap))
+    if (STALLING && stallAtNode<DIRECTION>(facade, heapNode, forward_heap))
     {
         return;
     }
 
-    relaxOutgoingEdges<DIRECTION>(facade, node, weight, forward_heap);
+    relaxOutgoingEdges<DIRECTION>(facade, heapNode, forward_heap);
 }
 
 template <bool UseDuration>

include/engine/routing_algorithms/routing_base_mld.hpp

@@ -228,40 +228,42 @@ retrievePackedPathFromHeap(const SearchEngineData<Algorithm>::QueryHeap &forward
 template <bool DIRECTION, typename Algorithm, typename... Args>
 void relaxOutgoingEdges(const DataFacade<Algorithm> &facade,
                         typename SearchEngineData<Algorithm>::QueryHeap &forward_heap,
-                        const NodeID node,
-                        const EdgeWeight weight,
+                        const typename SearchEngineData<Algorithm>::QueryHeap::HeapNode &heapNode,
                         Args... args)
 {
     const auto &partition = facade.GetMultiLevelPartition();
     const auto &cells = facade.GetCellStorage();
     const auto &metric = facade.GetCellMetric();
 
-    const auto level = getNodeQueryLevel(partition, node, args...);
+    const auto level = getNodeQueryLevel(partition, heapNode.node, args...);
 
-    if (level >= 1 && !forward_heap.GetData(node).from_clique_arc)
+    if (level >= 1 && !heapNode.data.from_clique_arc)
     {
         if (DIRECTION == FORWARD_DIRECTION)
         {
             // Shortcuts in forward direction
-            const auto &cell = cells.GetCell(metric, level, partition.GetCell(level, node));
+            const auto &cell =
+                cells.GetCell(metric, level, partition.GetCell(level, heapNode.node));
             auto destination = cell.GetDestinationNodes().begin();
-            for (auto shortcut_weight : cell.GetOutWeight(node))
+            for (auto shortcut_weight : cell.GetOutWeight(heapNode.node))
             {
                 BOOST_ASSERT(destination != cell.GetDestinationNodes().end());
                 const NodeID to = *destination;
 
-                if (shortcut_weight != INVALID_EDGE_WEIGHT && node != to)
+                if (shortcut_weight != INVALID_EDGE_WEIGHT && heapNode.node != to)
                 {
-                    const EdgeWeight to_weight = weight + shortcut_weight;
-                    BOOST_ASSERT(to_weight >= weight);
-                    if (!forward_heap.WasInserted(to))
+                    const EdgeWeight to_weight = heapNode.weight + shortcut_weight;
+                    BOOST_ASSERT(to_weight >= heapNode.weight);
+                    const auto toHeapNode = forward_heap.GetHeapNodeIfWasInserted(to);
+                    if (!toHeapNode)
                     {
-                        forward_heap.Insert(to, to_weight, {node, true});
+                        forward_heap.Insert(to, to_weight, {heapNode.node, true});
                     }
-                    else if (to_weight < forward_heap.GetKey(to))
+                    else if (to_weight < toHeapNode->weight)
                     {
-                        forward_heap.GetData(to) = {node, true};
-                        forward_heap.DecreaseKey(to, to_weight);
+                        toHeapNode->data = {heapNode.node, true};
+                        toHeapNode->weight = to_weight;
+                        forward_heap.DecreaseKey(*toHeapNode);
                     }
                 }
                 ++destination;
@@ -270,25 +272,28 @@ void relaxOutgoingEdges(const DataFacade<Algorithm> &facade,
         else
         {
             // Shortcuts in backward direction
-            const auto &cell = cells.GetCell(metric, level, partition.GetCell(level, node));
+            const auto &cell =
+                cells.GetCell(metric, level, partition.GetCell(level, heapNode.node));
             auto source = cell.GetSourceNodes().begin();
-            for (auto shortcut_weight : cell.GetInWeight(node))
+            for (auto shortcut_weight : cell.GetInWeight(heapNode.node))
             {
                 BOOST_ASSERT(source != cell.GetSourceNodes().end());
                 const NodeID to = *source;
 
-                if (shortcut_weight != INVALID_EDGE_WEIGHT && node != to)
+                if (shortcut_weight != INVALID_EDGE_WEIGHT && heapNode.node != to)
                 {
-                    const EdgeWeight to_weight = weight + shortcut_weight;
-                    BOOST_ASSERT(to_weight >= weight);
-                    if (!forward_heap.WasInserted(to))
+                    const EdgeWeight to_weight = heapNode.weight + shortcut_weight;
+                    BOOST_ASSERT(to_weight >= heapNode.weight);
+                    const auto toHeapNode = forward_heap.GetHeapNodeIfWasInserted(to);
+                    if (!toHeapNode)
                     {
-                        forward_heap.Insert(to, to_weight, {node, true});
+                        forward_heap.Insert(to, to_weight, {heapNode.node, true});
                     }
-                    else if (to_weight < forward_heap.GetKey(to))
+                    else if (to_weight < toHeapNode->weight)
                     {
-                        forward_heap.GetData(to) = {node, true};
-                        forward_heap.DecreaseKey(to, to_weight);
+                        toHeapNode->data = {heapNode.node, true};
+                        toHeapNode->weight = to_weight;
+                        forward_heap.DecreaseKey(*toHeapNode);
                     }
                 }
                 ++source;
@@ -297,7 +302,7 @@ void relaxOutgoingEdges(const DataFacade<Algorithm> &facade,
     }
 
     // Boundary edges
-    for (const auto edge : facade.GetBorderEdgeRange(level, node))
+    for (const auto edge : facade.GetBorderEdgeRange(level, heapNode.node))
     {
         const auto &edge_data = facade.GetEdgeData(edge);
 
@@ -310,21 +315,23 @@ void relaxOutgoingEdges(const DataFacade<Algorithm> &facade,
                 checkParentCellRestriction(partition.GetCell(level + 1, to), args...))
             {
                 const auto node_weight =
-                    facade.GetNodeWeight(DIRECTION == FORWARD_DIRECTION ? node : to);
+                    facade.GetNodeWeight(DIRECTION == FORWARD_DIRECTION ? heapNode.node : to);
                 const auto turn_penalty = facade.GetWeightPenaltyForEdgeID(edge_data.turn_id);
 
                 // TODO: BOOST_ASSERT(edge_data.weight == node_weight + turn_penalty);
 
-                const EdgeWeight to_weight = weight + node_weight + turn_penalty;
+                const EdgeWeight to_weight = heapNode.weight + node_weight + turn_penalty;
 
-                if (!forward_heap.WasInserted(to))
+                const auto toHeapNode = forward_heap.GetHeapNodeIfWasInserted(to);
+                if (!toHeapNode)
                 {
-                    forward_heap.Insert(to, to_weight, {node, false});
+                    forward_heap.Insert(to, to_weight, {heapNode.node, false});
                 }
-                else if (to_weight < forward_heap.GetKey(to))
+                else if (to_weight < toHeapNode->weight)
                 {
-                    forward_heap.GetData(to) = {node, false};
-                    forward_heap.DecreaseKey(to, to_weight);
+                    toHeapNode->data = {heapNode.node, false};
+                    toHeapNode->weight = to_weight;
+                    forward_heap.DecreaseKey(*toHeapNode);
                 }
             }
         }
@@ -341,34 +348,35 @@ void routingStep(const DataFacade<Algorithm> &facade,
                  const bool force_loop_reverse,
                  Args... args)
 {
-    const auto node = forward_heap.DeleteMin();
-    const auto weight = forward_heap.GetKey(node);
+    const auto heapNode = forward_heap.DeleteMinGetHeapNode();
+    const auto weight = heapNode.weight;
 
-    BOOST_ASSERT(!facade.ExcludeNode(node));
+    BOOST_ASSERT(!facade.ExcludeNode(heapNode.node));
 
     // Upper bound for the path source -> target with
     // weight(source -> node) = weight weight(to -> target) ≤ reverse_weight
     // is weight + reverse_weight
     // More tighter upper bound requires additional condition reverse_heap.WasRemoved(to)
     // with weight(to -> target) = reverse_weight and all weights ≥ 0
-    if (reverse_heap.WasInserted(node))
+    const auto reverseHeapNode = reverse_heap.GetHeapNodeIfWasInserted(heapNode.node);
+    if (reverseHeapNode)
     {
-        auto reverse_weight = reverse_heap.GetKey(node);
+        auto reverse_weight = reverseHeapNode->weight;
         auto path_weight = weight + reverse_weight;
 
         // MLD uses loops forcing only to prune single node paths in forward and/or
         // backward direction (there is no need to force loops in MLD but in CH)
-        if (!(force_loop_forward && forward_heap.GetData(node).parent == node) &&
-            !(force_loop_reverse && reverse_heap.GetData(node).parent == node) &&
+        if (!(force_loop_forward && heapNode.data.parent == heapNode.node) &&
+            !(force_loop_reverse && reverseHeapNode->data.parent == heapNode.node) &&
             (path_weight >= 0) && (path_weight < path_upper_bound))
         {
-            middle_node = node;
+            middle_node = heapNode.node;
             path_upper_bound = path_weight;
         }
     }
 
     // Relax outgoing edges from node
-    relaxOutgoingEdges<DIRECTION>(facade, forward_heap, node, weight, args...);
+    relaxOutgoingEdges<DIRECTION>(facade, forward_heap, heapNode, args...);
 }
 
 // With (s, middle, t) we trace back the paths middle -> s and middle -> t.