libdonut 2.3.6
Application framework for cross-platform game development in C++20
Loading...
Searching...
No Matches
LooseQuadtree.hpp
Go to the documentation of this file.
1#ifndef DONUT_LOOSE_QUADTREE_HPP
2#define DONUT_LOOSE_QUADTREE_HPP
3
4#include <donut/math.hpp>
5#include <donut/shapes.hpp>
6
7#include <algorithm> // std::all_of
8#include <array> // std::array
9#include <cassert> // assert
10#include <cstddef> // std::size_t, std::ptrdiff_t
11#include <cstdint> // std::uint..._t
12#include <iterator> // std::iterator_traits
13#include <optional> // std::optional
14#include <type_traits> // std::conditional_t, std::is_convertible_v, std::invoke_result_t
15#include <utility> // std::pair, std::forward, std::move, std::exchange
16#include <vector> // std::vector
17
18namespace donut {
19
26template <typename T>
28public:
29 using value_type = T;
30 using reference = T&;
31 using const_reference = const T&;
32 using pointer = T*;
33 using const_pointer = const T*;
34 using size_type = std::size_t;
35
36private:
37 using TreeIndex = std::uint32_t;
38 using QuadrantIndexArray = std::array<TreeIndex, 4>;
39
40 struct Quadrant {
41 QuadrantIndexArray subQuadrantIndices{};
42 TreeIndex parentIndex = 0;
43 std::optional<T> element{};
44 };
45
46 template <bool Const>
47 class Iterator {
48 public:
49 using reference = std::conditional_t<Const, LooseQuadtree::const_reference, LooseQuadtree::reference>;
50 using pointer = std::conditional_t<Const, LooseQuadtree::const_pointer, LooseQuadtree::pointer>;
51 using difference_type = std::ptrdiff_t;
52 using value_type = LooseQuadtree::value_type;
53
54 constexpr Iterator() noexcept = default;
55
56 [[nodiscard]] constexpr reference operator*() const {
57 assert(element);
58 assert(*element);
59 return **element;
60 }
61
62 [[nodiscard]] constexpr pointer operator->() const {
63 return &**this;
64 }
65
66 [[nodiscard]] constexpr bool operator==(const Iterator& other) const noexcept {
67 return element == other.element;
68 }
69
70 Iterator& operator++() {
71 ++element;
72 return *this;
73 }
74
75 Iterator operator++(int) {
76 Iterator old = *this;
77 ++*this;
78 return old;
79 }
80
81 private:
82 friend LooseQuadtree;
83
84 Iterator(std::optional<T>* element, TreeIndex treeIndex) noexcept
85 : element(element)
86 , treeIndex(treeIndex) {}
87
88 std::optional<T>* element = nullptr;
89 TreeIndex treeIndex{};
90 };
91
92public:
93 using iterator = Iterator<false>;
94 using const_iterator = Iterator<true>;
95 using difference_type = typename std::iterator_traits<iterator>::difference_type;
96
107 LooseQuadtree(const Box<2, float>& worldBoundingBox, vec2 typicalBoxSize) noexcept {
108 reset(worldBoundingBox, typicalBoxSize);
109 }
110
123 void reset(const Box<2, float>& worldBoundingBox, vec2 typicalBoxSize) noexcept {
124 clear();
125 minimumQuadrantSize = max(typicalBoxSize.x, typicalBoxSize.y);
126 rootCenter = (worldBoundingBox.min + worldBoundingBox.max) * 0.5f;
127 const vec2 worldMaxExtents = max(worldBoundingBox.max - rootCenter, rootCenter - worldBoundingBox.min);
128 const float worldMaxExtent = max(worldMaxExtents.x, worldMaxExtents.y);
129 // Double the root size until it fits the entire world.
130 halfRootSize = minimumQuadrantSize;
131 while (halfRootSize < worldMaxExtent) {
132 halfRootSize *= 2.0f;
133 }
134 }
135
142 void clear() noexcept {
143 tree.clear();
144 firstFreeIndex = 0;
145 }
146
171 template <typename... Args>
172 std::pair<iterator, bool> emplace(const Box<2, float>& elementBoundingBox, Args&&... args) {
173 // Make sure the tree has a root.
174 if (tree.empty()) {
175 tree.emplace_back();
176 }
177
178 // Find the center of the AABB.
179 const vec2 aabbDiagonal = elementBoundingBox.max - elementBoundingBox.min;
180 const vec2 aabbCenter = elementBoundingBox.min + aabbDiagonal * 0.5f;
181
182 // Find the largest extent of the AABB.
183 const float aabbSize = max(aabbDiagonal.x, aabbDiagonal.y);
184
185 // Start at the root of the tree and search for the smallest quadrant that contains the entire AABB within its loose bounds.
186 // The loose bounds is a box around the quadrant that is twice as big as the quadrant in every direction and shares the same center.
187 // Since the loose bounds of adjacent quadrants overlap, it could happen that the AABB is contained within multiple quadrants' loose bounds at the same time.
188 // In that case, the closest quadrant, i.e. the one which contains the center of the AABB, is chosen.
189 // The loop stops going lower in the tree when the AABB can no longer fit in a smaller quadrant, or when we reach the minimum quadrant size.
190 float quadrantSize = halfRootSize;
191 vec2 center = rootCenter;
192 TreeIndex treeIndex = 0;
193 while (quadrantSize >= aabbSize && quadrantSize >= minimumQuadrantSize) {
194 quadrantSize *= 0.5f;
195
196 // Determine which quadrant the AABB belongs to. This updates the center.
197 const std::size_t quadrantIndexArrayIndex = chooseQuadrant(aabbCenter, center, quadrantSize);
198
199 // Go to the quadrant.
200 if (TreeIndex& quadrantIndex = tree[treeIndex].subQuadrantIndices[quadrantIndexArrayIndex]) {
201 // The quadrant already exists in the tree. Go directly to it.
202 treeIndex = quadrantIndex;
203 } else {
204 // The quadrant does not exist in the tree yet.
205 // Acquire a space in the tree for the new quadrant.
206 if (firstFreeIndex != 0) {
207 // Re-use the first free quadrant.
208 Quadrant& quadrant = tree[firstFreeIndex];
209 // Set the new quadrant's parent index.
210 quadrant.parentIndex = treeIndex;
211 // Update the quadrant index to point to the new quadrant.
212 quadrantIndex = firstFreeIndex;
213 // Go to the new quadrant.
214 treeIndex = firstFreeIndex;
215 // Update the free index. The first sub-quadrant index in the quadrant leads to the next free quadrant.
216 firstFreeIndex = std::exchange(quadrant.subQuadrantIndices.front(), 0);
217 } else {
218 // No free quadrants available for re-use. We need to allocate a new quadrant.
219 // Save the new index. Don't change quadrantIndex before emplace_back() since the allocation might throw.
220 const TreeIndex newQuadrantIndex = static_cast<TreeIndex>(tree.size());
221 // Allocate the new quadrant.
222 Quadrant& newQuadrant = tree.emplace_back();
223 // Set the new quadrant's parent index.
224 newQuadrant.parentIndex = treeIndex;
225 // Update the quadrant index to point to the new quadrant.
226 // Don't access through the quadrantIndex reference since it might have been invalidated by emplace_back().
227 tree[treeIndex].subQuadrantIndices[quadrantIndexArrayIndex] = newQuadrantIndex;
228 // Go to the new quadrant.
229 treeIndex = newQuadrantIndex;
230 }
231 }
232 }
233
234 // Try to insert the new element into the selected quadrant.
235 try {
236 std::optional<T>& element = tree[treeIndex].element;
237 if (element) {
238 return {iterator{&element, treeIndex}, false};
239 }
240 element.emplace(std::forward<Args>(args)...);
241 return {iterator{&element, treeIndex}, true};
242 } catch (...) {
243 cleanup(treeIndex);
244 throw;
245 }
246 }
247
272 std::pair<iterator, bool> insert(const Box<2, float>& elementBoundingBox, const T& value) {
273 return emplace(elementBoundingBox, value);
274 }
275
300 std::pair<iterator, bool> insert(const Box<2, float>& elementBoundingBox, T&& value) {
301 return emplace(elementBoundingBox, std::move(value));
302 }
303
323 [[nodiscard]] T& operator[](const Box<2, float>& elementBoundingBox) {
324 return *emplace(elementBoundingBox).first;
325 }
326
334 void erase(const_iterator pos) noexcept {
335 assert(pos.element);
336 pos.element->reset();
337 cleanup(pos.treeIndex);
338 }
339
387 template <typename Callback, typename Predicate>
388 constexpr auto traverseActiveNodes(Callback&& callback, Predicate&& predicate) const { // NOLINT(cppcoreguidelines-missing-std-forward)
389 return traverseNodesImpl(
390 [callback = std::forward<Callback>(callback)](const Box<2, float>& looseBounds, const Quadrant& node) mutable {
391 constexpr bool CALLBACK_RETURNS_BOOL = std::is_convertible_v<std::invoke_result_t<Callback, const Box<2, float>&, const T* const&>, bool>;
392 const T* const element = (node.element) ? &*node.element : nullptr;
393 if constexpr (CALLBACK_RETURNS_BOOL) {
394 return callback(looseBounds, element);
395 } else {
396 callback(looseBounds, element);
397 }
398 },
399 std::forward<Predicate>(predicate));
400 }
401
438 template <typename Callback>
439 constexpr auto traverseActiveNodes(Callback&& callback) const {
440 return traverseActiveNodes(std::forward<Callback>(callback), [](const Box<2, float>&) -> bool { return true; });
441 }
442
490 template <typename Callback, typename Predicate>
491 constexpr auto traverseElementNodes(Callback&& callback, Predicate&& predicate) const { // NOLINT(cppcoreguidelines-missing-std-forward)
492 return traverseNodesImpl(
493 [callback = std::forward<Callback>(callback)](const Box<2, float>& looseBounds, const Quadrant& node) mutable {
494 constexpr bool CALLBACK_RETURNS_BOOL = std::is_convertible_v<std::invoke_result_t<Callback, const Box<2, float>&, const T&>, bool>;
495 if (node.element) {
496 if constexpr (CALLBACK_RETURNS_BOOL) {
497 if (callback(looseBounds, *node.element)) {
498 return true; // Callback requested early return.
499 }
500 } else {
501 callback(looseBounds, *node.element);
502 }
503 }
504 if constexpr (CALLBACK_RETURNS_BOOL) {
505 return false;
506 }
507 },
508 std::forward<Predicate>(predicate));
509 }
510
547 template <typename Callback>
548 constexpr auto traverseElementNodes(Callback&& callback) const {
549 return traverseElementNodes(std::forward<Callback>(callback), [](const Box<2, float>&) -> bool { return true; });
550 }
551
593 template <typename Callback, typename Predicate>
594 constexpr auto traverseElements(Callback&& callback, Predicate&& predicate) const { // NOLINT(cppcoreguidelines-missing-std-forward)
595 return traverseNodesImpl(
596 [callback = std::forward<Callback>(callback)](const Box<2, float>&, const Quadrant& node) mutable {
597 constexpr bool CALLBACK_RETURNS_BOOL = std::is_convertible_v<std::invoke_result_t<Callback, const T&>, bool>;
598 if (node.element) {
599 if constexpr (CALLBACK_RETURNS_BOOL) {
600 if (callback(*node.element)) {
601 return true; // Callback requested early return.
602 }
603 } else {
604 callback(*node.element);
605 }
606 }
607 if constexpr (CALLBACK_RETURNS_BOOL) {
608 return false;
609 }
610 },
611 std::forward<Predicate>(predicate));
612 }
613
645 template <typename Callback>
646 constexpr auto traverseElements(Callback&& callback) const {
647 return traverseElements(std::forward<Callback>(callback), [](const Box<2, float>&) -> bool { return true; });
648 }
649
683 template <typename Callback>
684 auto test(vec2 point, Callback&& callback) const {
685 return traverseElements(std::forward<Callback>(callback), [&point](const Box<2, float>& looseBounds) -> bool { return looseBounds.contains(point); });
686 }
687
706 [[nodiscard]] bool test(vec2 point) const noexcept {
707 return traverseElements([](const T&) -> bool { return true; }, [&point](const Box<2, float>& looseBounds) -> bool { return looseBounds.contains(point); });
708 }
709
743 template <typename Callback>
744 auto test(const Box<2, float>& box, Callback&& callback) const {
745 return traverseElements(std::forward<Callback>(callback), [&box](const Box<2, float>& looseBounds) -> bool { return intersects(looseBounds, box); });
746 }
747
767 [[nodiscard]] bool test(const Box<2, float>& box) const noexcept {
768 return traverseElements([](const T&) -> bool { return true; }, [&box](const Box<2, float>& looseBounds) -> bool { return intersects(looseBounds, box); });
769 }
770
771private:
772 struct IterationState {
773 vec2 center;
774 float quadrantSize;
775 TreeIndex treeIndex;
776 };
777
778 [[nodiscard]] static constexpr std::size_t chooseQuadrant(vec2 aabbCenter, vec2& center, float halfQuadrantSize) {
779 std::size_t index{};
780 if (aabbCenter.x < center.x) {
781 center.x -= halfQuadrantSize;
782 if (aabbCenter.y < center.y) {
783 center.y -= halfQuadrantSize;
784 index = 0;
785 } else {
786 center.y += halfQuadrantSize;
787 index = 1;
788 }
789 } else {
790 center.x += halfQuadrantSize;
791 if (aabbCenter.y < center.y) {
792 center.y -= halfQuadrantSize;
793 index = 2;
794 } else {
795 center.y += halfQuadrantSize;
796 index = 3;
797 }
798 }
799 return index;
800 }
801
802 template <typename Callback>
803 static constexpr void forEachActiveQuadrant(const QuadrantIndexArray& subQuadrantIndices, vec2 center, float halfQuadrantSize,
804 Callback&& callback) { // NOLINT(cppcoreguidelines-missing-std-forward)
805 if (const TreeIndex quadrantIndex = subQuadrantIndices[0]) {
806 callback(quadrantIndex, vec2{center.x - halfQuadrantSize, center.y - halfQuadrantSize});
807 }
808 if (const TreeIndex quadrantIndex = subQuadrantIndices[1]) {
809 callback(quadrantIndex, vec2{center.x - halfQuadrantSize, center.y + halfQuadrantSize});
810 }
811 if (const TreeIndex quadrantIndex = subQuadrantIndices[2]) {
812 callback(quadrantIndex, vec2{center.x + halfQuadrantSize, center.y - halfQuadrantSize});
813 }
814 if (const TreeIndex quadrantIndex = subQuadrantIndices[3]) {
815 callback(quadrantIndex, vec2{center.x + halfQuadrantSize, center.y + halfQuadrantSize});
816 }
817 }
818
819 template <typename Callback, typename Predicate>
820 constexpr auto traverseNodesImpl(Callback&& callback, Predicate&& predicate) const { // NOLINT(cppcoreguidelines-missing-std-forward)
821 constexpr bool CALLBACK_RETURNS_BOOL = std::is_convertible_v<std::invoke_result_t<Callback, const Box<2, float>&, const Quadrant&>, bool>;
822 if (!tree.empty()) {
823 iterationStack.clear();
824 iterationStack.push_back(IterationState{
825 .center = rootCenter,
826 .quadrantSize = halfRootSize,
827 .treeIndex = 0,
828 });
829 do {
830 const auto [center, quadrantSize, treeIndex] = iterationStack.back();
831 iterationStack.pop_back();
832
833 const float size = quadrantSize * 2.0f;
834 const Box<2, float> looseBounds{center - vec2{size}, center + vec2{size}};
835 const Quadrant& node = tree[treeIndex];
836 if constexpr (CALLBACK_RETURNS_BOOL) {
837 if (callback(looseBounds, node)) {
838 return true; // Callback requested early return.
839 }
840 } else {
841 callback(looseBounds, node);
842 }
843
844 const float halfQuadrantSize = quadrantSize * 0.5f;
845 forEachActiveQuadrant(node.subQuadrantIndices, center, halfQuadrantSize,
846 [this, &predicate, quadrantSize = quadrantSize, halfQuadrantSize](TreeIndex quadrantIndex, vec2 quadrantCenter) {
847 const Box<2, float> looseBounds{
848 .min = quadrantCenter - vec2{quadrantSize},
849 .max = quadrantCenter + vec2{quadrantSize},
850 };
851 if (predicate(looseBounds)) {
852 iterationStack.push_back(IterationState{
853 .center = quadrantCenter,
854 .quadrantSize = halfQuadrantSize,
855 .treeIndex = quadrantIndex,
856 });
857 }
858 });
859 } while (!iterationStack.empty());
860 }
861 if constexpr (CALLBACK_RETURNS_BOOL) {
862 return false;
863 }
864 }
865
866 void cleanup(TreeIndex treeIndex) noexcept {
867 Quadrant* node = &tree[treeIndex];
868 while (!node->element &&
869 std::all_of(node->subQuadrantIndices.begin(), node->subQuadrantIndices.end(), [](TreeIndex quadrantIndex) -> bool { return quadrantIndex == 0; })) {
870 if (treeIndex == 0) {
871 clear();
872 break;
873 }
874 node->subQuadrantIndices.front() = firstFreeIndex;
875 firstFreeIndex = treeIndex;
876 treeIndex = node->parentIndex;
877 node = &tree[treeIndex];
878 for (TreeIndex& quadrantIndex : node->subQuadrantIndices) {
879 if (quadrantIndex == firstFreeIndex) {
880 quadrantIndex = 0;
881 break;
882 }
883 }
884 }
885 }
886
887 std::vector<Quadrant> tree{};
888 float minimumQuadrantSize = 0.0f;
889 float halfRootSize = 0.0f;
890 vec2 rootCenter{0.0f, 0.0f};
891 TreeIndex firstFreeIndex = 0;
892 mutable std::vector<IterationState> iterationStack{};
893};
894
895} // namespace donut
896
897#endif
Quadtree-based space subdivision container, optimized for intersection queries between 2D axis-aligne...
Definition LooseQuadtree.hpp:27
auto test(vec2 point, Callback &&callback) const
Execute a callback function for each element in the tree that might contain a given point.
Definition LooseQuadtree.hpp:684
T value_type
Definition LooseQuadtree.hpp:29
auto test(const Box< 2, float > &box, Callback &&callback) const
Execute a callback function for each element in the tree that might be intersecting with a given axis...
Definition LooseQuadtree.hpp:744
constexpr auto traverseElementNodes(Callback &&callback, Predicate &&predicate) const
Execute a callback function for each active node of the tree that has an element.
Definition LooseQuadtree.hpp:491
std::size_t size_type
Definition LooseQuadtree.hpp:34
void erase(const_iterator pos) noexcept
Remove an element from the tree.
Definition LooseQuadtree.hpp:334
std::pair< iterator, bool > insert(const Box< 2, float > &elementBoundingBox, T &&value)
Try to move an element into the tree.
Definition LooseQuadtree.hpp:300
T & operator[](const Box< 2, float > &elementBoundingBox)
Try to default-construct a new element in the tree and get a reference to it.
Definition LooseQuadtree.hpp:323
constexpr auto traverseActiveNodes(Callback &&callback) const
Execute a callback function for each active node of the tree, including empty branch nodes without an...
Definition LooseQuadtree.hpp:439
T & reference
Definition LooseQuadtree.hpp:30
Iterator< false > iterator
Definition LooseQuadtree.hpp:93
bool test(vec2 point) const noexcept
Check if it is possible that some element in the tree contains a given point.
Definition LooseQuadtree.hpp:706
constexpr auto traverseElements(Callback &&callback, Predicate &&predicate) const
Execute a callback function for each element in the tree.
Definition LooseQuadtree.hpp:594
LooseQuadtree(const Box< 2, float > &worldBoundingBox, vec2 typicalBoxSize) noexcept
Construct an empty tree.
Definition LooseQuadtree.hpp:107
typename std::iterator_traits< iterator >::difference_type difference_type
Definition LooseQuadtree.hpp:95
Iterator< true > const_iterator
Definition LooseQuadtree.hpp:94
constexpr auto traverseElementNodes(Callback &&callback) const
Execute a callback function for each active node of the tree that has an element.
Definition LooseQuadtree.hpp:548
std::pair< iterator, bool > insert(const Box< 2, float > &elementBoundingBox, const T &value)
Try to copy an element into the tree.
Definition LooseQuadtree.hpp:272
void clear() noexcept
Erase all inserted elements from the tree.
Definition LooseQuadtree.hpp:142
std::pair< iterator, bool > emplace(const Box< 2, float > &elementBoundingBox, Args &&... args)
Try to construct a new element in the tree.
Definition LooseQuadtree.hpp:172
bool test(const Box< 2, float > &box) const noexcept
Check if it is possible that some element in the tree is intersecting with a given axis-aligned box.
Definition LooseQuadtree.hpp:767
constexpr auto traverseActiveNodes(Callback &&callback, Predicate &&predicate) const
Execute a callback function for each active node of the tree, including empty branch nodes without an...
Definition LooseQuadtree.hpp:388
T * pointer
Definition LooseQuadtree.hpp:32
constexpr auto traverseElements(Callback &&callback) const
Execute a callback function for each element in the tree.
Definition LooseQuadtree.hpp:646
const T & const_reference
Definition LooseQuadtree.hpp:31
const T * const_pointer
Definition LooseQuadtree.hpp:33
void reset(const Box< 2, float > &worldBoundingBox, vec2 typicalBoxSize) noexcept
Reset the tree to an empty state with new world parameters.
Definition LooseQuadtree.hpp:123
Definition Application.hpp:9
constexpr bool operator==(Monostate, Monostate) noexcept
Compare two monostates for equality.
Definition Variant.hpp:245
constexpr bool intersects(const Sphere< L, T > &a, const Sphere< L, T > &b) noexcept
Check if two spheres intersect.
Definition shapes.hpp:243
Generic axis-aligned box shape with minimum and maximum extents.
Definition shapes.hpp:110
constexpr bool contains(const Point< L, T > &point) const noexcept
Check if a given point is contained within the extents of this box.
Definition shapes.hpp:603
Point< L, T > max
Position with the maximum coordinates of the box extents on each coordinate axis.
Definition shapes.hpp:112
Point< L, T > min
Position with the minimum coordinates of the box extents on each coordinate axis.
Definition shapes.hpp:111