Add util::BinaryHeap

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

@@ -4,6 +4,7 @@
 #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"
@@ -553,21 +554,17 @@ 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
+        // we re-use queue for each query to avoid re-allocating memory
-        // std::priority_queue to be able to re-use allocated memory: std::priority_queue does not
+        static thread_local util::BinaryHeap<QueryCandidate> traversal_queue;
-        // have `clear` method
-        static thread_local std::vector<QueryCandidate> traversal_queue;
 
         traversal_queue.clear();
         // initialize queue with root element
-        traversal_queue.emplace_back(QueryCandidate{0, TreeIndex{}});
+        traversal_queue.emplace(QueryCandidate{0, TreeIndex{}});
-        std::push_heap(traversal_queue.begin(), traversal_queue.end());
 
         while (!traversal_queue.empty())
         {
-            QueryCandidate current_query_node = traversal_queue.front();
+            QueryCandidate current_query_node = traversal_queue.top();
-            std::pop_heap(traversal_queue.begin(), traversal_queue.end());
+            traversal_queue.pop();
-            traversal_queue.pop_back();
 
             const TreeIndex &current_tree_index = current_query_node.tree_index;
             if (!current_query_node.is_segment())
@@ -717,11 +714,10 @@ class StaticRTree
             BOOST_ASSERT(0. <= squared_distance);
             BOOST_ASSERT(i < std::numeric_limits<std::uint32_t>::max());
 
-            traversal_queue.emplace_back(QueryCandidate{squared_distance,
+            traversal_queue.emplace(QueryCandidate{squared_distance,
                                                    leaf_id,
                                                    static_cast<std::uint32_t>(i),
                                                    Coordinate{projected_nearest}});
-            std::push_heap(traversal_queue.begin(), traversal_queue.end());
         }
     }
 
@@ -750,10 +746,9 @@ class StaticRTree
                 child.minimum_bounding_rectangle.GetMinSquaredDist(
                     fixed_projected_input_coordinate);
 
-            traversal_queue.emplace_back(QueryCandidate{
+            traversal_queue.emplace(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());
         }
     }
 

unit_tests/util/binary_heap.cpp

@@ -0,0 +1,132 @@
+#include "util/binary_heap.hpp"
+
+#include <boost/test/unit_test.hpp>
+#include <string>
+
+BOOST_AUTO_TEST_SUITE(binary_heap_test)
+
+BOOST_AUTO_TEST_CASE(empty_heap)
+{
+    osrm::util::BinaryHeap<int> heap;
+    BOOST_CHECK(heap.empty());
+}
+
+BOOST_AUTO_TEST_CASE(push_and_top)
+{
+    osrm::util::BinaryHeap<int> heap;
+    heap.emplace(5);
+    BOOST_CHECK_EQUAL(heap.top(), 5);
+    BOOST_CHECK(!heap.empty());
+}
+
+BOOST_AUTO_TEST_CASE(push_multiple_and_order)
+{
+    osrm::util::BinaryHeap<int> heap;
+    heap.emplace(5);
+    heap.emplace(10);
+    heap.emplace(3);
+    heap.emplace(8);
+    BOOST_CHECK_EQUAL(heap.top(), 10);
+}
+
+BOOST_AUTO_TEST_CASE(pop_and_order)
+{
+    osrm::util::BinaryHeap<int> heap;
+    heap.emplace(5);
+    heap.emplace(10);
+    heap.emplace(3);
+    heap.emplace(8);
+
+    BOOST_CHECK_EQUAL(heap.top(), 10);
+    heap.pop();
+    BOOST_CHECK_EQUAL(heap.top(), 8);
+    heap.pop();
+    BOOST_CHECK_EQUAL(heap.top(), 5);
+    heap.pop();
+    BOOST_CHECK_EQUAL(heap.top(), 3);
+    heap.pop();
+    BOOST_CHECK(heap.empty());
+}
+
+BOOST_AUTO_TEST_CASE(clear_heap)
+{
+    osrm::util::BinaryHeap<int> heap;
+    heap.emplace(5);
+    heap.emplace(10);
+    heap.clear();
+    BOOST_CHECK(heap.empty());
+}
+
+BOOST_AUTO_TEST_CASE(emplace_with_custom_type)
+{
+    struct CustomType
+    {
+        int value;
+        CustomType(int v) : value(v) {}
+        bool operator<(const CustomType &other) const { return value < other.value; }
+    };
+
+    osrm::util::BinaryHeap<CustomType> heap;
+    heap.emplace(5);
+    heap.emplace(10);
+    heap.emplace(3);
+    BOOST_CHECK_EQUAL(heap.top().value, 10);
+}
+
+BOOST_AUTO_TEST_CASE(large_number_of_elements)
+{
+    osrm::util::BinaryHeap<int> heap;
+    for (int i = 0; i < 1000; ++i)
+    {
+        heap.emplace(i);
+    }
+    BOOST_CHECK_EQUAL(heap.top(), 999);
+
+    for (int i = 999; i >= 0; --i)
+    {
+        BOOST_CHECK_EQUAL(heap.top(), i);
+        heap.pop();
+    }
+    BOOST_CHECK(heap.empty());
+}
+
+BOOST_AUTO_TEST_CASE(duplicate_values)
+{
+    osrm::util::BinaryHeap<int> heap;
+    heap.emplace(5);
+    heap.emplace(5);
+    heap.emplace(5);
+
+    BOOST_CHECK_EQUAL(heap.top(), 5);
+    heap.pop();
+    BOOST_CHECK_EQUAL(heap.top(), 5);
+    heap.pop();
+    BOOST_CHECK_EQUAL(heap.top(), 5);
+    heap.pop();
+    BOOST_CHECK(heap.empty());
+}
+
+BOOST_AUTO_TEST_CASE(string_type)
+{
+    osrm::util::BinaryHeap<std::string> heap;
+    heap.emplace("apple");
+    heap.emplace("banana");
+    heap.emplace("cherry");
+
+    BOOST_CHECK_EQUAL(heap.top(), "cherry");
+    heap.pop();
+    BOOST_CHECK_EQUAL(heap.top(), "banana");
+    heap.pop();
+    BOOST_CHECK_EQUAL(heap.top(), "apple");
+}
+
+BOOST_AUTO_TEST_CASE(emplace_after_clear)
+{
+    osrm::util::BinaryHeap<int> heap;
+    heap.emplace(5);
+    heap.clear();
+    heap.emplace(10);
+    BOOST_CHECK_EQUAL(heap.top(), 10);
+}
+
+BOOST_AUTO_TEST_SUITE_END()