From a74fe05854051f7b450961c45a9c5b370704666b Mon Sep 17 00:00:00 2001
From: Siarhei Fedartsou <siarhei.fedartsou@gmail.com>
Date: Fri, 21 Jun 2024 21:45:10 +0200
Subject: [PATCH] Re-use priority queue in StaticRTree

---
 include/util/static_rtree.hpp | 31 ++++++++++++++++++++-----------
 1 file changed, 20 insertions(+), 11 deletions(-)

diff --git a/include/util/static_rtree.hpp b/include/util/static_rtree.hpp
index f9d72c144..1fa20b293 100644
--- a/include/util/static_rtree.hpp
+++ b/include/util/static_rtree.hpp
@@ -2,6 +2,7 @@
 #define STATIC_RTREE_HPP
 
 #include "storage/tar_fwd.hpp"
+#include "osrm/coordinate.hpp"
 #include "util/bearing.hpp"
 #include "util/coordinate_calculation.hpp"
 #include "util/deallocating_vector.hpp"
@@ -15,8 +16,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,14 +553,21 @@ class StaticRTree
         auto projected_coordinate = web_mercator::fromWGS84(input_coordinate);
         Coordinate fixed_projected_coordinate{projected_coordinate};
 
+        // we use std::vector<QueryCandidate> with std::push_heap/std::pop_heap instead of
+        // std::priority_queue to be able to re-use allocated memory: std::priority_queue does not
+        // have `clear` method
+        static thread_local std::vector<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_back(QueryCandidate{0, TreeIndex{}});
+        std::push_heap(traversal_queue.begin(), traversal_queue.end());
 
         while (!traversal_queue.empty())
         {
-            QueryCandidate current_query_node = traversal_queue.top();
-            traversal_queue.pop();
+            QueryCandidate current_query_node = traversal_queue.front();
+            std::pop_heap(traversal_queue.begin(), traversal_queue.end());
+            traversal_queue.pop_back();
 
             const TreeIndex &current_tree_index = current_query_node.tree_index;
             if (!current_query_node.is_segment())
@@ -710,10 +716,12 @@ 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_back(QueryCandidate{squared_distance,
+                                                        leaf_id,
+                                                        static_cast<std::uint32_t>(i),
+                                                        Coordinate{projected_nearest}});
+            std::push_heap(traversal_queue.begin(), traversal_queue.end());
         }
     }
 
@@ -742,9 +750,10 @@ class StaticRTree
                 child.minimum_bounding_rectangle.GetMinSquaredDist(
                     fixed_projected_input_coordinate);
 
-            traversal_queue.push(QueryCandidate{
+            traversal_queue.emplace_back(QueryCandidate{
                 squared_lower_bound_to_element,
                 TreeIndex(parent.level + 1, child_index - m_tree_level_starts[parent.level + 1])});
+            std::push_heap(traversal_queue.begin(), traversal_queue.end());
         }
     }