Re-use priority queue in StaticRTree (#6952)

include/util/binary_heap.hpp

@@ -0,0 +1,40 @@
+#pragma once
+#include <algorithm>
+#include <boost/assert.hpp>
+#include <vector>
+
+namespace osrm::util
+{
+
+// in its essence it is std::priority_queue, but with `clear` method
+template <typename T> class BinaryHeap
+{
+  public:
+    bool empty() const { return heap_.empty(); }
+
+    const T &top() const
+    {
+        BOOST_ASSERT(!heap_.empty());
+        return heap_.front();
+    }
+
+    void pop()
+    {
+        BOOST_ASSERT(!heap_.empty());
+        std::pop_heap(heap_.begin(), heap_.end());
+        heap_.pop_back();
+    }
+
+    template <typename... Args> void emplace(Args &&...args)
+    {
+        heap_.emplace_back(std::forward<Args>(args)...);
+        std::push_heap(heap_.begin(), heap_.end());
+    }
+
+    void clear() { heap_.clear(); }
+
+  private:
+    std::vector<T> heap_;
+};
+
+} // namespace osrm::util

include/util/static_rtree.hpp

@@ -2,7 +2,9 @@
 #define STATIC_RTREE_HPP
 
 #include "storage/tar_fwd.hpp"
+#include "osrm/coordinate.hpp"
 #include "util/bearing.hpp"
+#include "util/binary_heap.hpp"
 #include "util/coordinate_calculation.hpp"
 #include "util/deallocating_vector.hpp"
 #include "util/exception.hpp"
@@ -15,8 +17,6 @@
 #include "util/vector_view.hpp"
 #include "util/web_mercator.hpp"
 
-#include "osrm/coordinate.hpp"
-
 #include "storage/shared_memory_ownership.hpp"
 
 #include <boost/assert.hpp>
@@ -554,9 +554,12 @@ class StaticRTree
         auto projected_coordinate = web_mercator::fromWGS84(input_coordinate);
         Coordinate fixed_projected_coordinate{projected_coordinate};
 
+        // we re-use queue for each query to avoid re-allocating memory
+        static thread_local util::BinaryHeap<QueryCandidate> traversal_queue;
+
+        traversal_queue.clear();
         // initialize queue with root element
-        std::priority_queue<QueryCandidate> traversal_queue;
-        traversal_queue.push(QueryCandidate{0, TreeIndex{}});
+        traversal_queue.emplace(QueryCandidate{0, TreeIndex{}});
 
         while (!traversal_queue.empty())
         {
@@ -710,10 +713,11 @@ class StaticRTree
             // distance must be non-negative
             BOOST_ASSERT(0. <= squared_distance);
             BOOST_ASSERT(i < std::numeric_limits<std::uint32_t>::max());
-            traversal_queue.push(QueryCandidate{squared_distance,
-                                                leaf_id,
-                                                static_cast<std::uint32_t>(i),
-                                                Coordinate{projected_nearest}});
+
+            traversal_queue.emplace(QueryCandidate{squared_distance,
+                                                   leaf_id,
+                                                   static_cast<std::uint32_t>(i),
+                                                   Coordinate{projected_nearest}});
         }
     }
 
@@ -742,7 +746,7 @@ class StaticRTree
                 child.minimum_bounding_rectangle.GetMinSquaredDist(
                     fixed_projected_input_coordinate);
 
-            traversal_queue.push(QueryCandidate{
+            traversal_queue.emplace(QueryCandidate{
                 squared_lower_bound_to_element,
                 TreeIndex(parent.level + 1, child_index - m_tree_level_starts[parent.level + 1])});
         }