diff --git a/src/libslic3r/MutablePriorityQueue.hpp b/src/libslic3r/MutablePriorityQueue.hpp index b20bf60ea..e34e03080 100644 --- a/src/libslic3r/MutablePriorityQueue.hpp +++ b/src/libslic3r/MutablePriorityQueue.hpp @@ -7,6 +7,7 @@ template(index_setter)), m_less_predicate(std::forward(less_predicate)) @@ -139,10 +140,10 @@ inline void MutablePriorityQueue +struct SkipHeapAddressing +{ +public: + static const constexpr std::size_t block_size = blocking; + static const constexpr std::size_t block_mask = block_size - 1; + static_assert((block_size & block_mask) == 0U, "block size must be 2^n for some integer n"); + + static inline std::size_t child_of(std::size_t node_no) noexcept { + if (! is_block_leaf(node_no)) + // If not a leaf, then it is sufficient to just traverse down inside a miniheap. + // The following line is equivalent to, but quicker than + // return block_base(node_no) + 2 * block_offset(node_no); + return node_no + block_offset(node_no); + // Otherwise skip to a root of a child miniheap. + return (block_base(node_no) + 1 + child_no(node_no) * 2) * block_size + 1; + } + + static inline std::size_t parent_of(std::size_t node_no) noexcept { + auto const node_root = block_base(node_no); // 16 + if (! is_block_root(node_no)) + // If not a block (miniheap) root, then it is sufficient to just traverse up inside a miniheap. + return node_root + block_offset(node_no) / 2; + // Otherwise skipping from a root of one miniheap into leaf of another miniheap. + // Address of a parent miniheap block. One miniheap branches at (block_size / 2) leaves to (block_size) miniheaps. + auto const parent_base = block_base(node_root / block_size - 1); // 0 + // Index of a leaf of a parent miniheap, which is a parent of node_no. + auto const child = ((node_no - block_size) / block_size - parent_base) / 2; + return + // Address of a parent miniheap + parent_base + + // Address of a leaf of a parent miniheap + block_size / 2 + child; // 30 + } + + // Leafs are stored inside the second half of a block. + static inline bool is_block_leaf(std::size_t node_no) noexcept { return (node_no & (block_size >> 1)) != 0U; } + // Unused space aka padding to facilitate quick addressing. + static inline bool is_padding (std::size_t node_no) noexcept { return block_offset(node_no) == 0U; } +// Following methods are internal, but made public for unit tests. +//private: + // Address is a root of a block (of a miniheap). + static inline bool is_block_root(std::size_t node_no) noexcept { return block_offset(node_no) == 1U; } + // Offset inside a block (inside a miniheap). + static inline std::size_t block_offset (std::size_t node_no) noexcept { return node_no & block_mask; } + // Base address of a block (a miniheap). + static inline std::size_t block_base (std::size_t node_no) noexcept { return node_no & ~block_mask; } + // Index of a leaf. + static inline std::size_t child_no (std::size_t node_no) noexcept { assert(is_block_leaf(node_no)); return node_no & (block_mask >> 1); } +}; + +// Cache friendly variant of MutablePriorityQueue, implemented as a binary heap of binary miniheaps, +// building upon SkipHeapAddressing. +template +class MutableSkipHeapPriorityQueue +{ +public: + using address = SkipHeapAddressing; + + // It is recommended to use make_miniheap_mutable_priority_queue() for construction. + MutableSkipHeapPriorityQueue(IndexSetter &&index_setter, LessPredicate &&less_predicate) : + m_index_setter(std::forward(index_setter)), + m_less_predicate(std::forward(less_predicate)) + {} + ~MutableSkipHeapPriorityQueue() { clear(); } + + void clear(); + // Reserve one unused element per miniheap. + void reserve(size_t cnt) { m_heap.reserve(cnt + ((cnt + (address::block_size - 1)) / (address::block_size - 1))); } + void push(const T &item); + void push(T &&item); + void pop(); + T& top() { return m_heap[1]; } + void remove(size_t idx); + void update(size_t idx) { assert(! address::is_padding(idx)); T item = m_heap[idx]; remove(idx); push(item); } + // There is one padding element storead at each miniheap, thus lower the number of elements by the number of miniheaps. + size_t size() const noexcept { return m_heap.size() - (m_heap.size() + address::block_size - 1) / address::block_size; } + bool empty() const { return m_heap.empty(); } + +protected: + void update_heap_up(size_t top, size_t bottom); + void update_heap_down(size_t top, size_t bottom); + void pop_back() noexcept { + assert(m_heap.size() > 1); + assert(! address::is_padding(m_heap.size() - 1)); + m_heap.pop_back(); + if (address::is_padding(m_heap.size() - 1)) + m_heap.pop_back(); + } + +private: + std::vector m_heap; + IndexSetter m_index_setter; + LessPredicate m_less_predicate; +}; + +template +MutableSkipHeapPriorityQueue + make_miniheap_mutable_priority_queue(IndexSetter &&index_setter, LessPredicate &&less_predicate) +{ + return MutableSkipHeapPriorityQueue( + std::forward(index_setter), std::forward(less_predicate)); +} + +template +inline void MutableSkipHeapPriorityQueue::clear() +{ +#ifdef NDEBUG + // Only mark as removed from the queue in release mode, if configured so. + if (ResetIndexWhenRemoved) +#endif /* NDEBUG */ + { + for (size_t idx = 0; idx < m_heap.size(); ++ idx) + // Mark as removed from the queue. + if (! address::is_padding(idx)) + m_index_setter(m_heap[idx], std::numeric_limits::max()); + } + m_heap.clear(); +} + +template +inline void MutableSkipHeapPriorityQueue::push(const T &item) +{ + if (address::is_padding(m_heap.size())) + m_heap.emplace_back(T()); + size_t idx = m_heap.size(); + m_heap.emplace_back(item); + m_index_setter(m_heap.back(), idx); + update_heap_up(1, idx); +} + +template +inline void MutableSkipHeapPriorityQueue::push(T &&item) +{ + if (address::is_padding(m_heap.size())) + m_heap.emplace_back(T()); + size_t idx = m_heap.size(); + m_heap.emplace_back(std::move(item)); + m_index_setter(m_heap.back(), idx); + update_heap_up(1, idx); +} + +template +inline void MutableSkipHeapPriorityQueue::pop() +{ + assert(! m_heap.empty()); +#ifdef NDEBUG + // Only mark as removed from the queue in release mode, if configured so. + if (ResetIndexWhenRemoved) +#endif /* NDEBUG */ + { + // Mark as removed from the queue. + m_index_setter(m_heap.front(), std::numeric_limits::max()); + } + // Zero'th element is padding, thus non-empty queue must have at least two elements. + if (m_heap.size() > 2) { + m_heap[1] = m_heap.back(); + this->pop_back(); + m_index_setter(m_heap[1], 1); + update_heap_down(1, m_heap.size() - 1); + } else + m_heap.clear(); +} + +template +inline void MutableSkipHeapPriorityQueue::remove(size_t idx) +{ + assert(idx < m_heap.size()); + assert(! address::is_padding(idx)); +#ifdef NDEBUG + // Only mark as removed from the queue in release mode, if configured so. + if (ResetIndexWhenRemoved) +#endif /* NDEBUG */ + { + // Mark as removed from the queue. + m_index_setter(m_heap[idx], std::numeric_limits::max()); + } + if (idx + 1 == m_heap.size()) { + this->pop_back(); + return; + } + m_heap[idx] = m_heap.back(); + m_index_setter(m_heap[idx], idx); + this->pop_back(); + update_heap_down(idx, m_heap.size() - 1); + update_heap_up(1, idx); +} + +template +inline void MutableSkipHeapPriorityQueue::update_heap_up(size_t top, size_t bottom) +{ + assert(! address::is_padding(top)); + assert(! address::is_padding(bottom)); + size_t childIdx = bottom; + T *child = &m_heap[childIdx]; + for (;;) { + size_t parentIdx = address::parent_of(childIdx); + if (childIdx == 1 || parentIdx < top) + break; + T *parent = &m_heap[parentIdx]; + // switch nodes + if (! m_less_predicate(*parent, *child)) { + T tmp = *parent; + m_index_setter(tmp, childIdx); + m_index_setter(*child, parentIdx); + m_heap[parentIdx] = *child; + m_heap[childIdx] = tmp; + } + // shift up + childIdx = parentIdx; + child = parent; + } +} + +template +inline void MutableSkipHeapPriorityQueue::update_heap_down(size_t top, size_t bottom) +{ + assert(! address::is_padding(top)); + assert(! address::is_padding(bottom)); + size_t parentIdx = top; + T *parent = &m_heap[parentIdx]; + for (;;) { + size_t childIdx = address::child_of(parentIdx); + if (childIdx > bottom) + break; + T *child = &m_heap[childIdx]; + size_t child2Idx = childIdx + (address::is_block_leaf(parentIdx) ? address::block_size : 1); + if (child2Idx <= bottom) { + T *child2 = &m_heap[child2Idx]; + if (! m_less_predicate(*child, *child2)) { + child = child2; + childIdx = child2Idx; + } + } + if (m_less_predicate(*parent, *child)) + return; + // switch nodes + T tmp = *parent; + m_index_setter(tmp, childIdx); + m_index_setter(*child, parentIdx); + m_heap[parentIdx] = *child; + m_heap[childIdx] = tmp; + // shift down + parentIdx = childIdx; + parent = child; + } +} + #endif /* slic3r_MutablePriorityQueue_hpp_ */ diff --git a/tests/libslic3r/CMakeLists.txt b/tests/libslic3r/CMakeLists.txt index 5a4203ea0..575878cf2 100644 --- a/tests/libslic3r/CMakeLists.txt +++ b/tests/libslic3r/CMakeLists.txt @@ -12,6 +12,7 @@ add_executable(${_TEST_NAME}_tests test_placeholder_parser.cpp test_polygon.cpp test_mutable_polygon.cpp + test_mutable_priority_queue.cpp test_stl.cpp test_meshsimplify.cpp test_meshboolean.cpp diff --git a/tests/libslic3r/test_mutable_priority_queue.cpp b/tests/libslic3r/test_mutable_priority_queue.cpp new file mode 100644 index 000000000..83ac28793 --- /dev/null +++ b/tests/libslic3r/test_mutable_priority_queue.cpp @@ -0,0 +1,330 @@ +#include + +#include + +#include "libslic3r/MutablePriorityQueue.hpp" + +// based on https://raw.githubusercontent.com/rollbear/prio_queue/master/self_test.cpp +// original source Copyright Björn Fahller 2015, Boost Software License, Version 1.0, http://www.boost.org/LICENSE_1_0.txt + +TEST_CASE("Skip addressing", "[MutableSkipHeapPriorityQueue]") { + using skip_addressing = SkipHeapAddressing<8>; + SECTION("block root") { + REQUIRE(skip_addressing::is_block_root(1)); + REQUIRE(skip_addressing::is_block_root(9)); + REQUIRE(skip_addressing::is_block_root(17)); + REQUIRE(skip_addressing::is_block_root(73)); + REQUIRE(! skip_addressing::is_block_root(2)); + REQUIRE(! skip_addressing::is_block_root(3)); + REQUIRE(! skip_addressing::is_block_root(4)); + REQUIRE(! skip_addressing::is_block_root(7)); + REQUIRE(! skip_addressing::is_block_root(31)); + } + SECTION("block leaf") { + REQUIRE(! skip_addressing::is_block_leaf(1)); + REQUIRE(! skip_addressing::is_block_leaf(2)); + REQUIRE(! skip_addressing::is_block_leaf(3)); + REQUIRE(skip_addressing::is_block_leaf(4)); + REQUIRE(skip_addressing::is_block_leaf(5)); + REQUIRE(skip_addressing::is_block_leaf(6)); + REQUIRE(skip_addressing::is_block_leaf(7)); + REQUIRE(skip_addressing::is_block_leaf(28)); + REQUIRE(skip_addressing::is_block_leaf(29)); + REQUIRE(skip_addressing::is_block_leaf(30)); + REQUIRE(! skip_addressing::is_block_leaf(257)); + REQUIRE(skip_addressing::is_block_leaf(255)); + } + SECTION("Obtaining child") { + REQUIRE(skip_addressing::child_of(1) == 2); + REQUIRE(skip_addressing::child_of(2) == 4); + REQUIRE(skip_addressing::child_of(3) == 6); + REQUIRE(skip_addressing::child_of(4) == 9); + REQUIRE(skip_addressing::child_of(31) == 249); + } + SECTION("Obtaining parent") { + REQUIRE(skip_addressing::parent_of(2) == 1); + REQUIRE(skip_addressing::parent_of(3) == 1); + REQUIRE(skip_addressing::parent_of(6) == 3); + REQUIRE(skip_addressing::parent_of(7) == 3); + REQUIRE(skip_addressing::parent_of(9) == 4); + REQUIRE(skip_addressing::parent_of(17) == 4); + REQUIRE(skip_addressing::parent_of(33) == 5); + REQUIRE(skip_addressing::parent_of(29) == 26); + REQUIRE(skip_addressing::parent_of(1097) == 140); + } +} + +template +static auto make_test_priority_queue() +{ + return make_miniheap_mutable_priority_queue, block_size, false>( + [](std::pair &v, size_t idx){ v.second = idx; }, + [](std::pair &l, std::pair &r){ return l.first < r.first; }); +} + +TEST_CASE("Mutable priority queue - basic tests", "[MutableSkipHeapPriorityQueue]") { + SECTION("a default constructed queue is empty") { + auto q = make_test_priority_queue(); + REQUIRE(q.empty()); + REQUIRE(q.size() == 0); + } + SECTION("an empty queue is not empty when one element is inserted") { + auto q = make_test_priority_queue(); + q.push(std::make_pair(1, 0U)); + REQUIRE(!q.empty()); + REQUIRE(q.size() == 1); + } + SECTION("a queue with one element has it on top") { + auto q = make_test_priority_queue(); + q.push(std::make_pair(8, 0U)); + REQUIRE(q.top().first == 8); + } + SECTION("a queue with one element becomes empty when popped") { + auto q = make_test_priority_queue(); + q.push(std::make_pair(9, 0U)); + q.pop(); + REQUIRE(q.empty()); + REQUIRE(q.size() == 0); + } + SECTION("insert sorted stays sorted") { + auto q = make_test_priority_queue(); + for (auto i : { 1, 2, 3, 4, 5, 6, 7, 8 }) + q.push(std::make_pair(i, 0U)); + REQUIRE(q.top().first == 1); + q.pop(); + REQUIRE(q.top().first == 2); + q.pop(); + REQUIRE(q.top().first == 3); + q.pop(); + REQUIRE(q.top().first == 4); + q.pop(); + REQUIRE(q.top().first == 5); + q.pop(); + REQUIRE(q.top().first == 6); + q.pop(); + REQUIRE(q.top().first == 7); + q.pop(); + REQUIRE(q.top().first == 8); + q.pop(); + REQUIRE(q.empty()); + } + SECTION("randomly inserted elements are popped sorted") { + auto q = make_test_priority_queue(); + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_int_distribution<> dist(1, 100000); + int n[36000]; + for (auto& i : n) { + i = dist(gen); + q.push(std::make_pair(i, 0U)); + } + + REQUIRE(!q.empty()); + REQUIRE(q.size() == 36000); + std::sort(std::begin(n), std::end(n)); + for (auto i : n) { + REQUIRE(q.top().first == i); + q.pop(); + } + REQUIRE(q.empty()); + } +} + +TEST_CASE("Mutable priority queue - reshedule first", "[MutableSkipHeapPriorityQueue]") { + SECTION("reschedule top with highest prio leaves order unchanged") { + auto q = make_miniheap_mutable_priority_queue, size_t>, 4, false>( + [](std::pair, size_t>& v, size_t idx) { v.second = idx; }, + [](std::pair, size_t>& l, std::pair, size_t>& r) { return l.first.first < r.first.first; }); + + // 0 1 2 3 4 5 6 7 8 + int nums[] = { 32, 1, 88, 16, 9, 11, 3, 22, 23 }; + for (auto &i : nums) + q.push(std::make_pair(std::make_pair(i, &i), 0U)); + REQUIRE(q.top().first.first == 1); + REQUIRE(q.top().first.second == &nums[1]); + REQUIRE(*q.top().first.second == 1); + + // Update the top element. + q.top().first.first = 2; + q.update(1); + + REQUIRE(q.top().first.first == 2); + REQUIRE(q.top().first.second == &nums[1]); + q.pop(); + REQUIRE(q.top().first.first == 3); + REQUIRE(q.top().first.second == &nums[6]); + q.pop(); + REQUIRE(q.top().first.first == 9); + REQUIRE(q.top().first.second == &nums[4]); + q.pop(); + REQUIRE(q.top().first.first == 11); + REQUIRE(q.top().first.second == &nums[5]); + q.pop(); + REQUIRE(q.top().first.first == 16); + REQUIRE(q.top().first.second == &nums[3]); + q.pop(); + REQUIRE(q.top().first.first == 22); + REQUIRE(q.top().first.second == &nums[7]); + q.pop(); + REQUIRE(q.top().first.first == 23); + REQUIRE(q.top().first.second == &nums[8]); + q.pop(); + REQUIRE(q.top().first.first == 32); + REQUIRE(q.top().first.second == &nums[0]); + q.pop(); + REQUIRE(q.top().first.first == 88); + REQUIRE(q.top().first.second == &nums[2]); + q.pop(); + REQUIRE(q.empty()); + } + SECTION("reschedule to mid range moves element to correct place") { + auto q = make_miniheap_mutable_priority_queue, size_t>, 4, false>( + [](std::pair, size_t>& v, size_t idx) { v.second = idx; }, + [](std::pair, size_t>& l, std::pair, size_t>& r) { return l.first.first < r.first.first; }); + + // 0 1 2 3 4 5 6 7 8 + int nums[] = { 32, 1, 88, 16, 9, 11, 3, 22, 23 }; + for (auto& i : nums) + q.push(std::make_pair(std::make_pair(i, &i), 0U)); + REQUIRE(q.top().first.first == 1); + REQUIRE(q.top().first.second == &nums[1]); + REQUIRE(*q.top().first.second == 1); + + // Update the top element. + q.top().first.first = 12; + q.update(1); + + REQUIRE(q.top().first.first == 3); + REQUIRE(q.top().first.second == &nums[6]); + q.pop(); + REQUIRE(q.top().first.first == 9); + REQUIRE(q.top().first.second == &nums[4]); + q.pop(); + REQUIRE(q.top().first.first == 11); + REQUIRE(q.top().first.second == &nums[5]); + q.pop(); + REQUIRE(q.top().first.first == 12); + REQUIRE(q.top().first.second == &nums[1]); + q.pop(); + REQUIRE(q.top().first.first == 16); + REQUIRE(q.top().first.second == &nums[3]); + q.pop(); + REQUIRE(q.top().first.first == 22); + REQUIRE(q.top().first.second == &nums[7]); + q.pop(); + REQUIRE(q.top().first.first == 23); + REQUIRE(q.top().first.second == &nums[8]); + q.pop(); + REQUIRE(q.top().first.first == 32); + REQUIRE(q.top().first.second == &nums[0]); + q.pop(); + REQUIRE(q.top().first.first == 88); + REQUIRE(q.top().first.second == &nums[2]); + q.pop(); + REQUIRE(q.empty()); + } + SECTION("reschedule to last moves element to correct place", "heap") + { + auto q = make_miniheap_mutable_priority_queue, size_t>, 4, false>( + [](std::pair, size_t>& v, size_t idx) { v.second = idx; }, + [](std::pair, size_t>& l, std::pair, size_t>& r) { return l.first.first < r.first.first; }); + + // 0 1 2 3 4 5 6 7 8 + int nums[] = { 32, 1, 88, 16, 9, 11, 3, 22, 23 }; + for (auto& i : nums) + q.push(std::make_pair(std::make_pair(i, &i), 0U)); + REQUIRE(q.top().first.first == 1); + REQUIRE(q.top().first.second == &nums[1]); + REQUIRE(*q.top().first.second == 1); + + // Update the top element. + q.top().first.first = 89; + q.update(1); + + REQUIRE(q.top().first.first == 3); + REQUIRE(q.top().first.second == &nums[6]); + q.pop(); + REQUIRE(q.top().first.first == 9); + REQUIRE(q.top().first.second == &nums[4]); + q.pop(); + REQUIRE(q.top().first.first == 11); + REQUIRE(q.top().first.second == &nums[5]); + q.pop(); + REQUIRE(q.top().first.first == 16); + REQUIRE(q.top().first.second == &nums[3]); + q.pop(); + REQUIRE(q.top().first.first == 22); + REQUIRE(q.top().first.second == &nums[7]); + q.pop(); + REQUIRE(q.top().first.first == 23); + REQUIRE(q.top().first.second == &nums[8]); + q.pop(); + REQUIRE(q.top().first.first == 32); + REQUIRE(q.top().first.second == &nums[0]); + q.pop(); + REQUIRE(q.top().first.first == 88); + REQUIRE(q.top().first.second == &nums[2]); + q.pop(); + REQUIRE(q.top().first.first == 89); + REQUIRE(q.top().first.second == &nums[1]); + q.pop(); + REQUIRE(q.empty()); + } + SECTION("reschedule top of 2 elements to last") { + auto q = make_test_priority_queue<8>(); + q.push(std::make_pair(1, 0U)); + q.push(std::make_pair(2, 0U)); + REQUIRE(q.top().first == 1); + // Update the top element. + q.top().first = 3; + q.update(1); + REQUIRE(q.top().first == 2); + } + SECTION("reschedule top of 3 elements left to 2nd") { + auto q = make_test_priority_queue<8>(); + q.push(std::make_pair(1, 0U)); + q.push(std::make_pair(2, 0U)); + q.push(std::make_pair(4, 0U)); + REQUIRE(q.top().first == 1); + // Update the top element. + q.top().first = 3; + q.update(1); + REQUIRE(q.top().first == 2); + } + SECTION("reschedule top of 3 elements right to 2nd") { + auto q = make_test_priority_queue<8>(); + q.push(std::make_pair(1, 0U)); + q.push(std::make_pair(4, 0U)); + q.push(std::make_pair(2, 0U)); + REQUIRE(q.top().first == 1); + // Update the top element. + q.top().first = 3; + q.update(1); + REQUIRE(q.top().first == 2); + } + SECTION("reschedule top random gives same resultas pop/push") { + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_int_distribution dist(1, 100000); + + auto pq = make_test_priority_queue<8>(); + std::priority_queue, std::greater<>> stdq; + + for (size_t outer = 0; outer < 100; ++ outer) { + int num = gen(); + pq.push(std::make_pair(num, 0U)); + stdq.push(num); + for (size_t inner = 0; inner < 100; ++ inner) { + int newval = gen(); + // Update the top element. + pq.top().first = newval; + pq.update(1); + stdq.pop(); + stdq.push(newval); + auto n = pq.top().first; + auto sn = stdq.top(); + REQUIRE(sn == n); + } + } + } +}