Changes made

include/engine/routing_algorithms/routing_base_ch.hpp

@@ -36,9 +36,10 @@ bool stallAtNode(const DataFacade<Algorithm> &facade,
             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.WasInsertedGetHeapNode(to);
+            if (toHeapNode)
             {
-                if (query_heap.GetKey(to) + edge_weight < weight)
+                if (toHeapNode->weight + edge_weight < weight)
                 {
                     return true;
                 }
@@ -65,17 +66,19 @@ void relaxOutgoingEdges(const DataFacade<Algorithm> &facade,
             BOOST_ASSERT_MSG(edge_weight > 0, "edge_weight invalid");
             const EdgeWeight to_weight = weight + edge_weight;
 
+            auto toData= heap.WasInsertedGetHeapNode(to);
             // New Node discovered -> Add to Heap + Node Info Storage
-            if (!heap.WasInserted(to))
+            if (!toData)
             {
                 heap.Insert(to, to_weight, node);
             }
             // Found a shorter Path -> Update weight
-            else if (to_weight < heap.GetKey(to))
+            else if (to_weight < toData->weight)
             {
                 // new parent
-                heap.GetData(to).parent = node;
-                heap.DecreaseKey(to, to_weight);
+                toData->data.parent = node;
+                toData->weight=to_weight;
+                heap.DecreaseKey(*toData);
             }
         }
     }
@@ -122,35 +125,36 @@ 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 nodeData = forward_heap.DeleteMinGetHeapNode();
+    const EdgeWeight weight = nodeData.weight;
 
-    if (reverse_heap.WasInserted(node))
+    auto reverseNodeData= reverse_heap.WasInsertedGetHeapNode(nodeData.node);
+    if (reverseNodeData)
     {
-        const EdgeWeight new_weight = reverse_heap.GetKey(node) + weight;
+        const EdgeWeight new_weight = reverseNodeData->weight + 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 && nodeData.data.parent == nodeData.node) ||
+                (force_loop_reverse && reverseNodeData->data.parent == nodeData.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(nodeData.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 == nodeData.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 = nodeData.node;
                                 upper_bound = loop_weight;
                             }
                         }
@@ -161,7 +165,7 @@ void routingStep(const DataFacade<Algorithm> &facade,
             {
                 BOOST_ASSERT(new_weight >= 0);
 
-                middle_node_id = node;
+                middle_node_id = nodeData.node;
                 upper_bound = new_weight;
             }
         }
@@ -177,12 +181,12 @@ void routingStep(const DataFacade<Algorithm> &facade,
     }
 
     // Stalling
-    if (STALLING && stallAtNode<DIRECTION>(facade, node, weight, forward_heap))
+    if (STALLING && stallAtNode<DIRECTION>(facade, nodeData.node, weight, forward_heap))
     {
         return;
     }
 
-    relaxOutgoingEdges<DIRECTION>(facade, node, weight, forward_heap);
+    relaxOutgoingEdges<DIRECTION>(facade, nodeData.node, weight, forward_heap);
 }
 
 template <bool UseDuration>

include/engine/routing_algorithms/routing_base_mld.hpp

@@ -229,7 +229,7 @@ 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 typename SearchEngineData<Algorithm>::QueryHeap::HeapNode& heapNode,
                         const EdgeWeight weight,
                         Args... args)
 {
@@ -237,32 +237,34 @@ void relaxOutgoingEdges(const DataFacade<Algorithm> &facade,
     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))
+                    auto toNodeData= forward_heap.WasInsertedGetHeapNode(to);
+                    if (!toNodeData)
                     {
-                        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))
                     {
-                        forward_heap.GetData(to) = {node, true};
-                        forward_heap.DecreaseKey(to, to_weight);
+                        toNodeData->data = {heapNode.node, true};
+                        toNodeData->weight=to_weight;
+                        forward_heap.DecreaseKey(*toNodeData);
                     }
                 }
                 ++destination;
@@ -271,25 +273,27 @@ 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))
+                    auto toNodeData= forward_heap.WasInsertedGetHeapNode(to);
+                    if (!toNodeData)
                     {
-                        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))
                     {
-                        forward_heap.GetData(to) = {node, true};
-                        forward_heap.DecreaseKey(to, to_weight);
+                        toNodeData->data = {heapNode.node, true};
+                        toNodeData->weight=to_weight;
+                        forward_heap.DecreaseKey(*toNodeData);
                     }
                 }
                 ++source;
@@ -298,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);
 
@@ -311,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;
 
-                if (!forward_heap.WasInserted(to))
+                auto toNodeData= forward_heap.WasInsertedGetHeapNode(to);
+                if (!toNodeData)
                 {
-                    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))
                 {
-                    forward_heap.GetData(to) = {node, false};
-                    forward_heap.DecreaseKey(to, to_weight);
+                    toNodeData->data  = {heapNode.node, false};
+                    toNodeData->weight=to_weight;
+                    forward_heap.DecreaseKey(*toNodeData);
                 }
             }
         }
@@ -342,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))
+    auto reverveNodeData= reverse_heap.WasInsertedGetHeapNode(heapNode.node);
+    if (reverveNodeData)
     {
-        auto reverse_weight = reverse_heap.GetKey(node);
+        auto reverse_weight = reverveNodeData->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 && reverveNodeData->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, weight, args...);
 }
 
 // With (s, middle, t) we trace back the paths middle -> s and middle -> t.

include/util/query_heap.hpp

@@ -2,6 +2,7 @@
 #define OSRM_UTIL_QUERY_HEAP_HPP
 
 #include <boost/assert.hpp>
+#include <boost/optional.hpp>
 #include <boost/heap/d_ary_heap.hpp>
 
 #include <algorithm>
@@ -194,10 +195,27 @@ template <typename NodeID,
           typename IndexStorage = ArrayStorage<NodeID, NodeID>>
 class QueryHeap
 {
+  private:
+
+    using HeapData = std::pair<Weight, Key>;
+    using HeapContainer = boost::heap::d_ary_heap<HeapData,
+        boost::heap::arity<4>,
+        boost::heap::mutable_<true>,
+        boost::heap::compare<std::greater<HeapData>>>;
+    using HeapHandle = typename HeapContainer::handle_type;
+
   public:
     using WeightType = Weight;
     using DataType = Data;
 
+    struct HeapNode
+    {
+        HeapHandle handle;
+        NodeID node;
+        Weight weight;
+        Data data;
+    };
+
     template <typename... StorageArgs> explicit QueryHeap(StorageArgs... args) : node_index(args...)
     {
         Clear();
@@ -230,6 +248,13 @@ class QueryHeap
         return inserted_nodes[index].data;
     }
 
+    HeapNode& getHeapNode(NodeID node)
+    {
+        const auto index = node_index.peek_index(node);
+        BOOST_ASSERT((int)index >= 0 && (int)index < (int)inserted_nodes.size());
+        return inserted_nodes[index];
+    }
+
     Data const &GetData(NodeID node) const
     {
         const auto index = node_index.peek_index(node);
@@ -269,6 +294,28 @@ class QueryHeap
         return inserted_nodes[index].node == node;
     }
 
+    boost::optional<HeapNode&> WasInsertedGetHeapNode(const NodeID node)
+    {
+        const auto index = node_index.peek_index(node);
+        if (index >= static_cast<decltype(index)>(inserted_nodes.size()))
+        {
+            return {};
+        }
+        return inserted_nodes[index];
+    }
+
+    const boost::optional<const HeapNode&> WasInsertedGetHeapNode(const NodeID node) const
+    {
+        const auto index = node_index.peek_index(node);
+        if (index >= static_cast<decltype(index)>(inserted_nodes.size()))
+        {
+            return {};
+        }
+        return inserted_nodes[index];
+    }
+
+
+
     NodeID Min() const
     {
         BOOST_ASSERT(!heap.empty());
@@ -290,6 +337,15 @@ class QueryHeap
         return inserted_nodes[removedIndex].node;
     }
 
+    HeapNode& DeleteMinGetHeapNode()
+    {
+        BOOST_ASSERT(!heap.empty());
+        const Key removedIndex = heap.top().second;
+        heap.pop();
+        inserted_nodes[removedIndex].handle = heap.s_handle_from_iterator(heap.end());
+        return inserted_nodes[removedIndex];
+    }
+
     void DeleteAll()
     {
         auto const none_handle = heap.s_handle_from_iterator(heap.end());
@@ -308,22 +364,13 @@ class QueryHeap
         heap.increase(reference.handle, std::make_pair(weight, index));
     }
 
-  private:
-    using HeapData = std::pair<Weight, Key>;
-    using HeapContainer = boost::heap::d_ary_heap<HeapData,
-                                                  boost::heap::arity<4>,
-                                                  boost::heap::mutable_<true>,
-                                                  boost::heap::compare<std::greater<HeapData>>>;
-    using HeapHandle = typename HeapContainer::handle_type;
-
-    struct HeapNode
+    void DecreaseKey(const HeapNode& heapNode)
     {
-        HeapHandle handle;
-        NodeID node;
-        Weight weight;
-        Data data;
-    };
+        BOOST_ASSERT(!WasRemoved(heapNode.node));
+        heap.increase(heapNode.handle, std::make_pair(heapNode.weight, (*heapNode.handle).second));
+    }
 
+  private:
     std::vector<HeapNode> inserted_nodes;
     HeapContainer heap;
     IndexStorage node_index;

src/contractor/contractor_search.cpp

@@ -32,16 +32,18 @@ void relaxNode(ContractorHeap &heap,
         }
         const EdgeWeight to_weight = node_weight + data.weight;
 
+        const auto& toHeapNode=heap.WasInsertedGetHeapNode(to);
         // New Node discovered -> Add to Heap + Node Info Storage
-        if (!heap.WasInserted(to))
+        if (!toHeapNode)
         {
             heap.Insert(to, to_weight, ContractorHeapData{current_hop, false});
         }
         // Found a shorter Path -> Update weight
-        else if (to_weight < heap.GetKey(to))
+        else if (to_weight < toHeapNode->weight)
         {
-            heap.DecreaseKey(to, to_weight);
-            heap.GetData(to).hop = current_hop;
+            toHeapNode->weight=to_weight;
+            heap.DecreaseKey(*toHeapNode);
+            toHeapNode->data.hop = current_hop;
         }
     }
 }

src/engine/routing_algorithms/alternative_path_ch.cpp

@@ -62,8 +62,8 @@ void alternativeRoutingStep(const DataFacade<Algorithm> &facade,
     QueryHeap &forward_heap = DIRECTION == FORWARD_DIRECTION ? heap1 : heap2;
     QueryHeap &reverse_heap = DIRECTION == FORWARD_DIRECTION ? heap2 : heap1;
 
-    const NodeID node = forward_heap.DeleteMin();
-    const EdgeWeight weight = forward_heap.GetKey(node);
+    auto & heapNode=forward_heap.DeleteMinGetHeapNode();
+    const EdgeWeight weight = heapNode.weight;
 
     const auto scaled_weight =
         static_cast<EdgeWeight>((weight + min_edge_offset) / (1. + VIAPATH_EPSILON));
@@ -74,35 +74,36 @@ void alternativeRoutingStep(const DataFacade<Algorithm> &facade,
         return;
     }
 
-    search_space.emplace_back(forward_heap.GetData(node).parent, node);
+    search_space.emplace_back(heapNode.data.parent, heapNode.node);
 
-    if (reverse_heap.WasInserted(node))
+    const auto& reverseHeapNode=reverse_heap.WasInsertedGetHeapNode(heapNode.node);
+    if (reverseHeapNode)
     {
-        search_space_intersection.emplace_back(node);
-        const EdgeWeight new_weight = reverse_heap.GetKey(node) + weight;
+        search_space_intersection.emplace_back(heapNode.node);
+        const EdgeWeight new_weight = reverseHeapNode->weight + weight;
         if (new_weight < *upper_bound_to_shortest_path_weight)
         {
             if (new_weight >= 0)
             {
-                *middle_node = node;
+                *middle_node = heapNode.node;
                 *upper_bound_to_shortest_path_weight = new_weight;
             }
             else
             {
                 // check whether there is a loop present at the node
-                const auto loop_weight = std::get<0>(getLoopWeight<false>(facade, node));
+                const auto loop_weight = std::get<0>(getLoopWeight<false>(facade, heapNode.node));
                 const EdgeWeight new_weight_with_loop = new_weight + loop_weight;
                 if (loop_weight != INVALID_EDGE_WEIGHT &&
                     new_weight_with_loop <= *upper_bound_to_shortest_path_weight)
                 {
-                    *middle_node = node;
+                    *middle_node = heapNode.node;
                     *upper_bound_to_shortest_path_weight = loop_weight;
                 }
             }
         }
     }
 
-    for (auto edge : facade.GetAdjacentEdgeRange(node))
+    for (auto edge : facade.GetAdjacentEdgeRange(heapNode.node))
     {
         const auto &data = facade.GetEdgeData(edge);
         if (DIRECTION == FORWARD_DIRECTION ? data.forward : data.backward)
@@ -113,18 +114,20 @@ void alternativeRoutingStep(const DataFacade<Algorithm> &facade,
             BOOST_ASSERT(edge_weight > 0);
             const EdgeWeight to_weight = weight + edge_weight;
 
+            const auto& toHeapNode=forward_heap.WasInsertedGetHeapNode(to);
             // New Node discovered -> Add to Heap + Node Info Storage
-            if (!forward_heap.WasInserted(to))
+            if (!toHeapNode)
             {
-                forward_heap.Insert(to, to_weight, node);
+                forward_heap.Insert(to, to_weight, heapNode.node);
             }
             // Found a shorter Path -> Update weight
             else if (to_weight < forward_heap.GetKey(to))
             {
                 // new parent
-                forward_heap.GetData(to).parent = node;
+                toHeapNode->data.parent = heapNode.node;
                 // decreased weight
-                forward_heap.DecreaseKey(to, to_weight);
+                toHeapNode->weight=to_weight;
+                forward_heap.DecreaseKey(*toHeapNode);
             }
         }
     }

src/engine/routing_algorithms/many_to_many_ch.cpp

@@ -74,8 +74,9 @@ void relaxOutgoingEdges(const DataFacade<Algorithm> &facade,
             const auto to_duration = duration + edge_duration;
             const auto to_distance = distance + edge_distance;
 
+            const auto& toHeapNode=query_heap.WasInsertedGetHeapNode(to);
             // New Node discovered -> Add to Heap + Node Info Storage
-            if (!query_heap.WasInserted(to))
+            if (!toHeapNode)
             {
                 query_heap.Insert(to, to_weight, {node, to_duration, to_distance});
             }
@@ -83,8 +84,9 @@ void relaxOutgoingEdges(const DataFacade<Algorithm> &facade,
             else if (std::tie(to_weight, to_duration) <
                      std::tie(query_heap.GetKey(to), query_heap.GetData(to).duration))
             {
-                query_heap.GetData(to) = {node, to_duration, to_distance};
-                query_heap.DecreaseKey(to, to_weight);
+                toHeapNode->data = {node, to_duration, to_distance};
+                toHeapNode->weight=to_weight;
+                query_heap.DecreaseKey(*toHeapNode);
             }
         }
     }
@@ -101,15 +103,15 @@ void forwardRoutingStep(const DataFacade<Algorithm> &facade,
                         std::vector<NodeID> &middle_nodes_table,
                         const PhantomNode &phantom_node)
 {
-    const auto node = query_heap.DeleteMin();
-    const auto source_weight = query_heap.GetKey(node);
-    const auto source_duration = query_heap.GetData(node).duration;
-    const auto source_distance = query_heap.GetData(node).distance;
+    const auto& heapNode=query_heap.DeleteMinGetHeapNode();
+    const auto source_weight = heapNode.weight;
+    const auto source_duration = heapNode.data.duration;
+    const auto source_distance = heapNode.data.distance;
 
     // Check if each encountered node has an entry
     const auto &bucket_list = std::equal_range(search_space_with_buckets.begin(),
                                                search_space_with_buckets.end(),
-                                               node,
+                                               heapNode.node,
                                                NodeBucket::Compare());
     for (const auto &current_bucket : boost::make_iterator_range(bucket_list))
     {
@@ -135,12 +137,12 @@ void forwardRoutingStep(const DataFacade<Algorithm> &facade,
 
         if (new_weight < 0)
         {
-            if (addLoopWeight(facade, node, new_weight, new_duration, new_distance))
+            if (addLoopWeight(facade, heapNode.node, new_weight, new_duration, new_distance))
             {
                 current_weight = std::min(current_weight, new_weight);
                 current_duration = std::min(current_duration, new_duration);
                 current_distance = std::min(current_distance, new_distance);
-                middle_nodes_table[row_index * number_of_targets + column_index] = node;
+                middle_nodes_table[row_index * number_of_targets + column_index] = heapNode.node;
             }
         }
         else if (std::tie(new_weight, new_duration) < std::tie(current_weight, current_duration))
@@ -148,12 +150,12 @@ void forwardRoutingStep(const DataFacade<Algorithm> &facade,
             current_weight = new_weight;
             current_duration = new_duration;
             current_distance = new_distance;
-            middle_nodes_table[row_index * number_of_targets + column_index] = node;
+            middle_nodes_table[row_index * number_of_targets + column_index] = heapNode.node;
         }
     }
 
     relaxOutgoingEdges<FORWARD_DIRECTION>(
-        facade, node, source_weight, source_duration, source_distance, query_heap, phantom_node);
+        facade, heapNode.node, source_weight, source_duration, source_distance, query_heap, phantom_node);
 }
 
 void backwardRoutingStep(const DataFacade<Algorithm> &facade,
@@ -162,18 +164,18 @@ void backwardRoutingStep(const DataFacade<Algorithm> &facade,
                          std::vector<NodeBucket> &search_space_with_buckets,
                          const PhantomNode &phantom_node)
 {
-    const auto node = query_heap.DeleteMin();
-    const auto target_weight = query_heap.GetKey(node);
-    const auto target_duration = query_heap.GetData(node).duration;
-    const auto target_distance = query_heap.GetData(node).distance;
-    const auto parent = query_heap.GetData(node).parent;
+    const auto heapNode=query_heap.DeleteMinGetHeapNode();
+    const auto target_weight = heapNode.weight;
+    const auto target_duration = heapNode.data.duration;
+    const auto target_distance = heapNode.data.distance;
+    const auto parent = heapNode.data.parent;
 
     // Store settled nodes in search space bucket
     search_space_with_buckets.emplace_back(
-        node, parent, column_index, target_weight, target_duration, target_distance);
+        heapNode.node, parent, column_index, target_weight, target_duration, target_distance);
 
     relaxOutgoingEdges<REVERSE_DIRECTION>(
-        facade, node, target_weight, target_duration, target_distance, query_heap, phantom_node);
+        facade, heapNode.node, target_weight, target_duration, target_distance, query_heap, phantom_node);
 }
 
 } // namespace ch

src/engine/routing_algorithms/many_to_many_mld.cpp

@@ -71,19 +71,21 @@ void relaxBorderEdges(const DataFacade<mld::Algorithm> &facade,
             const auto to_distance = distance + node_distance;
 
             // New Node discovered -> Add to Heap + Node Info Storage
-            if (!query_heap.WasInserted(to))
+            const auto& toHeapNode=query_heap.WasInsertedGetHeapNode(to);
+            if (!toHeapNode)
             {
                 query_heap.Insert(to, to_weight, {node, false, to_duration, to_distance});
             }
             // Found a shorter Path -> Update weight and set new parent
             else if (std::tie(to_weight, to_duration, to_distance, node) <
                      std::tie(query_heap.GetKey(to),
-                              query_heap.GetData(to).duration,
-                              query_heap.GetData(to).distance,
-                              query_heap.GetData(to).parent))
+                              toHeapNode->data.duration,
+                              toHeapNode->data.distance,
+                              toHeapNode->data.parent))
             {
-                query_heap.GetData(to) = {node, false, to_duration, to_distance};
-                query_heap.DecreaseKey(to, to_weight);
+                toHeapNode->data = {node, false, to_duration, to_distance};
+                toHeapNode->weight=to_weight;
+                query_heap.DecreaseKey(*toHeapNode);
             }
         }
     }
@@ -91,18 +93,18 @@ void relaxBorderEdges(const DataFacade<mld::Algorithm> &facade,
 
 template <bool DIRECTION, typename... Args>
 void relaxOutgoingEdges(const DataFacade<mld::Algorithm> &facade,
-                        const NodeID node,
+                        const SearchEngineData<mld::Algorithm>::ManyToManyQueryHeap::HeapNode& heapNode,
                         const EdgeWeight weight,
                         const EdgeDuration duration,
                         const EdgeDistance distance,
                         typename SearchEngineData<mld::Algorithm>::ManyToManyQueryHeap &query_heap,
                         Args... args)
 {
-    BOOST_ASSERT(!facade.ExcludeNode(node));
+    BOOST_ASSERT(!facade.ExcludeNode(heapNode.node));
 
     const auto &partition = facade.GetMultiLevelPartition();
 
-    const auto level = getNodeQueryLevel(partition, node, args...);
+    const auto level = getNodeQueryLevel(partition, heapNode.node, args...);
 
     // Break outgoing edges relaxation if node at the restricted level
     if (level == INVALID_LEVEL_ID)
@@ -110,9 +112,9 @@ void relaxOutgoingEdges(const DataFacade<mld::Algorithm> &facade,
 
     const auto &cells = facade.GetCellStorage();
     const auto &metric = facade.GetCellMetric();
-    const auto &node_data = query_heap.GetData(node);
+    const auto node=heapNode.node;
 
-    if (level >= 1 && !node_data.from_clique_arc)
+    if (level >= 1 && !heapNode.data.from_clique_arc)
     {
         const auto &cell = cells.GetCell(metric, level, partition.GetCell(level, node));
         if (DIRECTION == FORWARD_DIRECTION)
@@ -132,18 +134,20 @@ void relaxOutgoingEdges(const DataFacade<mld::Algorithm> &facade,
                     const auto to_weight = weight + shortcut_weight;
                     const auto to_duration = duration + shortcut_durations.front();
                     const auto to_distance = distance + shortcut_distances.front();
-                    if (!query_heap.WasInserted(to))
+                    const auto& toHeapNode=query_heap.WasInsertedGetHeapNode(to);
+                    if (!toHeapNode)
                     {
                         query_heap.Insert(to, to_weight, {node, true, to_duration, to_distance});
                     }
                     else if (std::tie(to_weight, to_duration, to_distance, node) <
                              std::tie(query_heap.GetKey(to),
-                                      query_heap.GetData(to).duration,
-                                      query_heap.GetData(to).distance,
-                                      query_heap.GetData(to).parent))
+                                      toHeapNode->data.duration,
+                                      toHeapNode->data.distance,
+                                      toHeapNode->data.parent))
                     {
-                        query_heap.GetData(to) = {node, true, to_duration, to_distance};
-                        query_heap.DecreaseKey(to, to_weight);
+                        toHeapNode->data = {node, true, to_duration, to_distance};
+                        toHeapNode->weight=to_weight;
+                        query_heap.DecreaseKey(*toHeapNode);
                     }
                 }
                 ++destination;
@@ -170,18 +174,20 @@ void relaxOutgoingEdges(const DataFacade<mld::Algorithm> &facade,
                     const auto to_weight = weight + shortcut_weight;
                     const auto to_duration = duration + shortcut_durations.front();
                     const auto to_distance = distance + shortcut_distances.front();
-                    if (!query_heap.WasInserted(to))
+                    const auto& toHeapNode=query_heap.WasInsertedGetHeapNode(to);
+                    if (!toHeapNode)
                     {
                         query_heap.Insert(to, to_weight, {node, true, to_duration, to_distance});
                     }
                     else if (std::tie(to_weight, to_duration, to_distance, node) <
                              std::tie(query_heap.GetKey(to),
-                                      query_heap.GetData(to).duration,
-                                      query_heap.GetData(to).distance,
-                                      query_heap.GetData(to).parent))
+                                      toHeapNode->data.duration,
+                                      toHeapNode->data.distance,
+                                      toHeapNode->data.parent))
                     {
-                        query_heap.GetData(to) = {node, true, to_duration, to_distance};
-                        query_heap.DecreaseKey(to, to_weight);
+                        toHeapNode->data = {node, true, to_duration, to_distance};
+                        toHeapNode->weight=to_weight;
+                        query_heap.DecreaseKey(*toHeapNode);
                     }
                 }
                 ++source;
@@ -369,17 +375,17 @@ oneToManySearch(SearchEngineData<Algorithm> &engine_working_data,
     while (!query_heap.Empty() && !target_nodes_index.empty())
     {
         // Extract node from the heap
-        const auto node = query_heap.DeleteMin();
-        const auto weight = query_heap.GetKey(node);
-        const auto duration = query_heap.GetData(node).duration;
-        const auto distance = query_heap.GetData(node).distance;
+        const auto& heapNode=query_heap.DeleteMinGetHeapNode();
+        const auto weight = heapNode.weight;
+        const auto duration = heapNode.data.duration;
+        const auto distance = heapNode.data.distance;
 
         // Update values
-        update_values(node, weight, duration, distance);
+        update_values(heapNode.node, weight, duration, distance);
 
         // Relax outgoing edges
         relaxOutgoingEdges<DIRECTION>(facade,
-                                      node,
+                                      heapNode,
                                       weight,
                                       duration,
                                       distance,
@@ -408,15 +414,15 @@ void forwardRoutingStep(const DataFacade<Algorithm> &facade,
                         std::vector<NodeID> &middle_nodes_table,
                         const PhantomNode &phantom_node)
 {
-    const auto node = query_heap.DeleteMin();
-    const auto source_weight = query_heap.GetKey(node);
-    const auto source_duration = query_heap.GetData(node).duration;
-    const auto source_distance = query_heap.GetData(node).distance;
+    const auto& heapNode=query_heap.DeleteMinGetHeapNode();
+    const auto source_weight = heapNode.weight;
+    const auto source_duration = heapNode.data.duration;
+    const auto source_distance = heapNode.data.distance;
 
     // Check if each encountered node has an entry
     const auto &bucket_list = std::equal_range(search_space_with_buckets.begin(),
                                                search_space_with_buckets.end(),
-                                               node,
+                                               heapNode.node,
                                                NodeBucket::Compare());
     for (const auto &current_bucket : boost::make_iterator_range(bucket_list))
     {
@@ -450,12 +456,12 @@ void forwardRoutingStep(const DataFacade<Algorithm> &facade,
             current_weight = new_weight;
             current_duration = new_duration;
             current_distance = new_distance;
-            middle_nodes_table[location] = node;
+            middle_nodes_table[location] = heapNode.node;
         }
     }
 
     relaxOutgoingEdges<DIRECTION>(
-        facade, node, source_weight, source_duration, source_distance, query_heap, phantom_node);
+        facade, heapNode, source_weight, source_duration, source_distance, query_heap, phantom_node);
 }
 
 template <bool DIRECTION>
@@ -465,22 +471,22 @@ void backwardRoutingStep(const DataFacade<Algorithm> &facade,
                          std::vector<NodeBucket> &search_space_with_buckets,
                          const PhantomNode &phantom_node)
 {
-    const auto node = query_heap.DeleteMin();
-    const auto target_weight = query_heap.GetKey(node);
-    const auto target_duration = query_heap.GetData(node).duration;
-    const auto target_distance = query_heap.GetData(node).distance;
-    const auto parent = query_heap.GetData(node).parent;
-    const auto from_clique_arc = query_heap.GetData(node).from_clique_arc;
+    const auto& heapNode=query_heap.DeleteMinGetHeapNode();
+    const auto target_weight = heapNode.weight;
+    const auto target_duration = heapNode.data.duration;
+    const auto target_distance = heapNode.data.distance;
+    const auto parent = heapNode.data.parent;
+    const auto from_clique_arc = heapNode.data.from_clique_arc;
 
     // Store settled nodes in search space bucket
     search_space_with_buckets.emplace_back(
-        node, parent, from_clique_arc, column_idx, target_weight, target_duration, target_distance);
+        heapNode.node, parent, from_clique_arc, column_idx, target_weight, target_duration, target_distance);
 
     const auto &partition = facade.GetMultiLevelPartition();
     const auto maximal_level = partition.GetNumberOfLevels() - 1;
 
     relaxOutgoingEdges<!DIRECTION>(facade,
-                                   node,
+                                   heapNode,
                                    target_weight,
                                    target_duration,
                                    target_distance,