1 #ifndef DONUT_LOOSE_QUADTREE_HPP
2 #define DONUT_LOOSE_QUADTREE_HPP
15 #include <type_traits>
38 using TreeIndex = std::uint32_t;
39 using QuadrantIndexArray = std::array<TreeIndex, 4>;
42 QuadrantIndexArray subQuadrantIndices{};
43 TreeIndex parentIndex = 0;
44 std::optional<T> element{};
50 using reference = std::conditional_t<Const, LooseQuadtree::const_reference, LooseQuadtree::reference>;
51 using pointer = std::conditional_t<Const, LooseQuadtree::const_pointer, LooseQuadtree::pointer>;
55 constexpr Iterator() noexcept = default;
57 [[nodiscard]] constexpr
reference operator*()
const {
63 [[nodiscard]] constexpr
pointer operator->()
const {
67 [[nodiscard]] constexpr
bool operator==(
const Iterator& other)
const noexcept {
68 return element == other.element;
71 Iterator& operator++() {
76 Iterator operator++(
int) {
85 Iterator(std::optional<T>* element, TreeIndex treeIndex) noexcept
87 , treeIndex(treeIndex) {}
89 std::optional<T>* element =
nullptr;
90 TreeIndex treeIndex{};
109 reset(worldBoundingBox, typicalBoxSize);
126 minimumQuadrantSize = max(typicalBoxSize.x, typicalBoxSize.y);
127 rootCenter = (worldBoundingBox.min + worldBoundingBox.max) * 0.5f;
128 const vec2 worldMaxExtents = max(worldBoundingBox.max - rootCenter, rootCenter - worldBoundingBox.min);
129 const float worldMaxExtent = max(worldMaxExtents.x, worldMaxExtents.y);
131 halfRootSize = minimumQuadrantSize;
132 while (halfRootSize < worldMaxExtent) {
133 halfRootSize *= 2.0f;
172 template <
typename... Args>
180 const vec2 aabbDiagonal = elementBoundingBox.
max - elementBoundingBox.
min;
181 const vec2 aabbCenter = elementBoundingBox.
min + aabbDiagonal * 0.5f;
184 const float aabbSize = max(aabbDiagonal.x, aabbDiagonal.y);
191 float quadrantSize = halfRootSize;
192 vec2 center = rootCenter;
193 TreeIndex treeIndex = 0;
194 while (quadrantSize >= aabbSize && quadrantSize >= minimumQuadrantSize) {
195 quadrantSize *= 0.5f;
198 const std::size_t quadrantIndexArrayIndex = chooseQuadrant(aabbCenter, center, quadrantSize);
201 if (TreeIndex& quadrantIndex = tree[treeIndex].subQuadrantIndices[quadrantIndexArrayIndex]) {
203 treeIndex = quadrantIndex;
207 if (firstFreeIndex != 0) {
209 Quadrant& quadrant = tree[firstFreeIndex];
211 quadrant.parentIndex = treeIndex;
213 quadrantIndex = firstFreeIndex;
215 treeIndex = firstFreeIndex;
217 firstFreeIndex = std::exchange(quadrant.subQuadrantIndices.front(), 0);
221 const TreeIndex newQuadrantIndex =
static_cast<TreeIndex
>(tree.size());
223 Quadrant& newQuadrant = tree.emplace_back();
225 newQuadrant.parentIndex = treeIndex;
228 tree[treeIndex].subQuadrantIndices[quadrantIndexArrayIndex] = newQuadrantIndex;
230 treeIndex = newQuadrantIndex;
237 std::optional<T>& element = tree[treeIndex].element;
239 return {
iterator{&element, treeIndex},
false};
241 element.emplace(std::forward<Args>(args)...);
242 return {
iterator{&element, treeIndex},
true};
274 return emplace(elementBoundingBox, value);
302 return emplace(elementBoundingBox, std::move(value));
325 return *
emplace(elementBoundingBox).first;
337 pos.element->reset();
338 cleanup(pos.treeIndex);
388 template <
typename Callback,
typename Predicate>
390 return traverseNodesImpl(
391 [callback = std::forward<Callback>(callback)](
const Box<2, float>& looseBounds,
const Quadrant& node)
mutable {
392 constexpr
bool CALLBACK_RETURNS_BOOL = std::is_convertible_v<std::invoke_result_t<Callback, const Box<2, float>&,
const T*
const&>,
bool>;
393 const T*
const element = (node.element) ? &*node.element :
nullptr;
394 if constexpr (CALLBACK_RETURNS_BOOL) {
395 return callback(looseBounds, element);
397 callback(looseBounds, element);
400 std::forward<Predicate>(predicate));
439 template <
typename Callback>
491 template <
typename Callback,
typename Predicate>
493 return traverseNodesImpl(
494 [callback = std::forward<Callback>(callback)](
const Box<2, float>& looseBounds,
const Quadrant& node)
mutable {
495 constexpr
bool CALLBACK_RETURNS_BOOL = std::is_convertible_v<std::invoke_result_t<Callback, const Box<2, float>&,
const T&>,
bool>;
497 if constexpr (CALLBACK_RETURNS_BOOL) {
498 if (callback(looseBounds, *node.element)) {
502 callback(looseBounds, *node.element);
505 if constexpr (CALLBACK_RETURNS_BOOL) {
509 std::forward<Predicate>(predicate));
548 template <
typename Callback>
550 return traverseElementNodes(std::forward<Callback>(callback), [](
const Box<2, float>&) ->
bool {
return true; });
594 template <
typename Callback,
typename Predicate>
596 return traverseNodesImpl(
597 [callback = std::forward<Callback>(callback)](
const Box<2, float>&,
const Quadrant& node)
mutable {
598 constexpr
bool CALLBACK_RETURNS_BOOL = std::is_convertible_v<std::invoke_result_t<Callback, const T&>,
bool>;
600 if constexpr (CALLBACK_RETURNS_BOOL) {
601 if (callback(*node.element)) {
605 callback(*node.element);
608 if constexpr (CALLBACK_RETURNS_BOOL) {
612 std::forward<Predicate>(predicate));
646 template <
typename Callback>
648 return traverseElements(std::forward<Callback>(callback), [](
const Box<2, float>&) ->
bool {
return true; });
684 template <
typename Callback>
685 auto test(vec2 point, Callback&& callback)
const {
686 return traverseElements(std::forward<Callback>(callback), [&point](
const Box<2, float>& looseBounds) ->
bool {
return looseBounds.
contains(point); });
707 [[nodiscard]]
bool test(vec2 point)
const noexcept {
708 return traverseElements([](
const T&) ->
bool {
return true; }, [&point](
const Box<2, float>& looseBounds) ->
bool {
return looseBounds.contains(point); });
744 template <
typename Callback>
746 return traverseElements(std::forward<Callback>(callback), [&box](
const Box<2, float>& looseBounds) ->
bool {
return intersects(looseBounds, box); });
769 return traverseElements([](
const T&) ->
bool {
return true; }, [&box](
const Box<2, float>& looseBounds) ->
bool {
return intersects(looseBounds, box); });
773 struct IterationState {
779 [[nodiscard]]
static constexpr std::size_t chooseQuadrant(vec2 aabbCenter, vec2& center,
float halfQuadrantSize) {
781 if (aabbCenter.x < center.x) {
782 center.x -= halfQuadrantSize;
783 if (aabbCenter.y < center.y) {
784 center.y -= halfQuadrantSize;
787 center.y += halfQuadrantSize;
791 center.x += halfQuadrantSize;
792 if (aabbCenter.y < center.y) {
793 center.y -= halfQuadrantSize;
796 center.y += halfQuadrantSize;
803 template <
typename Callback>
804 static constexpr
void forEachActiveQuadrant(
const QuadrantIndexArray& subQuadrantIndices, vec2 center,
float halfQuadrantSize,
805 Callback&& callback) {
806 if (
const TreeIndex quadrantIndex = subQuadrantIndices[0]) {
807 callback(quadrantIndex, vec2{center.x - halfQuadrantSize, center.y - halfQuadrantSize});
809 if (
const TreeIndex quadrantIndex = subQuadrantIndices[1]) {
810 callback(quadrantIndex, vec2{center.x - halfQuadrantSize, center.y + halfQuadrantSize});
812 if (
const TreeIndex quadrantIndex = subQuadrantIndices[2]) {
813 callback(quadrantIndex, vec2{center.x + halfQuadrantSize, center.y - halfQuadrantSize});
815 if (
const TreeIndex quadrantIndex = subQuadrantIndices[3]) {
816 callback(quadrantIndex, vec2{center.x + halfQuadrantSize, center.y + halfQuadrantSize});
820 template <
typename Callback,
typename Predicate>
821 constexpr
auto traverseNodesImpl(Callback&& callback, Predicate&& predicate)
const {
822 constexpr
bool CALLBACK_RETURNS_BOOL = std::is_convertible_v<std::invoke_result_t<Callback, const Box<2, float>&,
const Quadrant&>,
bool>;
824 iterationStack.clear();
825 iterationStack.push_back(IterationState{
826 .center = rootCenter,
827 .quadrantSize = halfRootSize,
831 const auto [center, quadrantSize, treeIndex] = iterationStack.back();
832 iterationStack.pop_back();
834 const float size = quadrantSize * 2.0f;
835 const Box<2, float> looseBounds{center - vec2{size}, center + vec2{size}};
836 const Quadrant& node = tree[treeIndex];
837 if constexpr (CALLBACK_RETURNS_BOOL) {
838 if (callback(looseBounds, node)) {
842 callback(looseBounds, node);
845 const float halfQuadrantSize = quadrantSize * 0.5f;
846 forEachActiveQuadrant(node.subQuadrantIndices, center, halfQuadrantSize,
847 [
this, &predicate, quadrantSize = quadrantSize, halfQuadrantSize](TreeIndex quadrantIndex, vec2 quadrantCenter) {
848 const Box<2, float> looseBounds{quadrantCenter - vec2{quadrantSize}, quadrantCenter + vec2{quadrantSize}};
849 if (predicate(looseBounds)) {
850 iterationStack.push_back(IterationState{
851 .center = quadrantCenter,
852 .quadrantSize = halfQuadrantSize,
853 .treeIndex = quadrantIndex,
857 }
while (!iterationStack.empty());
859 if constexpr (CALLBACK_RETURNS_BOOL) {
864 void cleanup(TreeIndex treeIndex) noexcept {
865 Quadrant* node = &tree[treeIndex];
866 while (!node->element &&
867 std::all_of(node->subQuadrantIndices.begin(), node->subQuadrantIndices.end(), [](TreeIndex quadrantIndex) ->
bool { return quadrantIndex == 0; })) {
868 if (treeIndex == 0) {
872 node->subQuadrantIndices.front() = firstFreeIndex;
873 firstFreeIndex = treeIndex;
874 treeIndex = node->parentIndex;
875 node = &tree[treeIndex];
876 for (TreeIndex& quadrantIndex : node->subQuadrantIndices) {
877 if (quadrantIndex == firstFreeIndex) {
885 std::vector<Quadrant> tree{};
886 float minimumQuadrantSize = 0.0f;
887 float halfRootSize = 0.0f;
888 vec2 rootCenter{0.0f, 0.0f};
889 TreeIndex firstFreeIndex = 0;
890 mutable std::vector<IterationState> iterationStack{};
Quadtree-based space subdivision container, optimized for intersection queries between 2D axis-aligne...
Definition: LooseQuadtree.hpp:28
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:685
std::pair< iterator, bool > emplace(const Box< 2, float > &elementBoundingBox, Args &&... args)
Try to construct a new element in the tree.
Definition: LooseQuadtree.hpp:173
T value_type
Definition: LooseQuadtree.hpp:30
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:745
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:492
std::size_t size_type
Definition: LooseQuadtree.hpp:35
void erase(const_iterator pos) noexcept
Remove an element from the tree.
Definition: LooseQuadtree.hpp:335
std::pair< iterator, bool > insert(const Box< 2, float > &elementBoundingBox, T &&value)
Try to move an element into the tree.
Definition: LooseQuadtree.hpp:301
std::pair< iterator, bool > insert(const Box< 2, float > &elementBoundingBox, const T &value)
Try to copy an element into the tree.
Definition: LooseQuadtree.hpp:273
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:440
T & reference
Definition: LooseQuadtree.hpp:31
Iterator< false > iterator
Definition: LooseQuadtree.hpp:94
bool test(vec2 point) const noexcept
Check if it is possible that some element in the tree contains a given point.
Definition: LooseQuadtree.hpp:707
constexpr auto traverseElements(Callback &&callback, Predicate &&predicate) const
Execute a callback function for each element in the tree.
Definition: LooseQuadtree.hpp:595
LooseQuadtree(const Box< 2, float > &worldBoundingBox, vec2 typicalBoxSize) noexcept
Construct an empty tree.
Definition: LooseQuadtree.hpp:108
typename std::iterator_traits< iterator >::difference_type difference_type
Definition: LooseQuadtree.hpp:96
Iterator< true > const_iterator
Definition: LooseQuadtree.hpp:95
constexpr auto traverseElementNodes(Callback &&callback) const
Execute a callback function for each active node of the tree that has an element.
Definition: LooseQuadtree.hpp:549
void clear() noexcept
Erase all inserted elements from the tree.
Definition: LooseQuadtree.hpp:143
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:768
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:389
T * pointer
Definition: LooseQuadtree.hpp:33
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:324
constexpr auto traverseElements(Callback &&callback) const
Execute a callback function for each element in the tree.
Definition: LooseQuadtree.hpp:647
const T & const_reference
Definition: LooseQuadtree.hpp:32
const T * const_pointer
Definition: LooseQuadtree.hpp:34
void reset(const Box< 2, float > &worldBoundingBox, vec2 typicalBoxSize) noexcept
Reset the tree to an empty state with new world parameters.
Definition: LooseQuadtree.hpp:124
Definition: Application.hpp:9
constexpr bool operator==(Monostate, Monostate) noexcept
Compare two monostates for equality.
Definition: Variant.hpp:246
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