1#ifndef DONUT_LOOSE_QUADTREE_HPP
2#define DONUT_LOOSE_QUADTREE_HPP
37 using TreeIndex = std::uint32_t;
38 using QuadrantIndexArray = std::array<TreeIndex, 4>;
41 QuadrantIndexArray subQuadrantIndices{};
42 TreeIndex parentIndex = 0;
43 std::optional<T> element{};
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;
54 constexpr Iterator() noexcept = default;
56 [[nodiscard]] constexpr reference operator*()
const {
62 [[nodiscard]]
constexpr pointer operator->()
const {
66 [[nodiscard]]
constexpr bool operator==(
const Iterator& other)
const noexcept {
67 return element == other.element;
70 Iterator& operator++() {
75 Iterator operator++(
int) {
84 Iterator(std::optional<T>* element, TreeIndex treeIndex) noexcept
86 , treeIndex(treeIndex) {}
88 std::optional<T>* element =
nullptr;
89 TreeIndex treeIndex{};
108 reset(worldBoundingBox, typicalBoxSize);
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);
130 halfRootSize = minimumQuadrantSize;
131 while (halfRootSize < worldMaxExtent) {
132 halfRootSize *= 2.0f;
171 template <
typename... Args>
179 const vec2 aabbDiagonal = elementBoundingBox.
max - elementBoundingBox.
min;
180 const vec2 aabbCenter = elementBoundingBox.
min + aabbDiagonal * 0.5f;
183 const float aabbSize = max(aabbDiagonal.x, aabbDiagonal.y);
190 float quadrantSize = halfRootSize;
191 vec2 center = rootCenter;
192 TreeIndex treeIndex = 0;
193 while (quadrantSize >= aabbSize && quadrantSize >= minimumQuadrantSize) {
194 quadrantSize *= 0.5f;
197 const std::size_t quadrantIndexArrayIndex = chooseQuadrant(aabbCenter, center, quadrantSize);
200 if (TreeIndex& quadrantIndex = tree[treeIndex].subQuadrantIndices[quadrantIndexArrayIndex]) {
202 treeIndex = quadrantIndex;
206 if (firstFreeIndex != 0) {
208 Quadrant& quadrant = tree[firstFreeIndex];
210 quadrant.parentIndex = treeIndex;
212 quadrantIndex = firstFreeIndex;
214 treeIndex = firstFreeIndex;
216 firstFreeIndex = std::exchange(quadrant.subQuadrantIndices.front(), 0);
220 const TreeIndex newQuadrantIndex =
static_cast<TreeIndex
>(tree.size());
222 Quadrant& newQuadrant = tree.emplace_back();
224 newQuadrant.parentIndex = treeIndex;
227 tree[treeIndex].subQuadrantIndices[quadrantIndexArrayIndex] = newQuadrantIndex;
229 treeIndex = newQuadrantIndex;
236 std::optional<T>& element = tree[treeIndex].element;
238 return {
iterator{&element, treeIndex},
false};
240 element.emplace(std::forward<Args>(args)...);
241 return {
iterator{&element, treeIndex},
true};
273 return emplace(elementBoundingBox, value);
301 return emplace(elementBoundingBox, std::move(value));
324 return *
emplace(elementBoundingBox).first;
336 pos.element->reset();
337 cleanup(pos.treeIndex);
387 template <
typename Callback,
typename Predicate>
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);
396 callback(looseBounds, element);
399 std::forward<Predicate>(predicate));
438 template <
typename Callback>
490 template <
typename Callback,
typename Predicate>
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>;
496 if constexpr (CALLBACK_RETURNS_BOOL) {
497 if (callback(looseBounds, *node.element)) {
501 callback(looseBounds, *node.element);
504 if constexpr (CALLBACK_RETURNS_BOOL) {
508 std::forward<Predicate>(predicate));
547 template <
typename Callback>
549 return traverseElementNodes(std::forward<Callback>(callback), [](
const Box<2, float>&) ->
bool {
return true; });
593 template <
typename Callback,
typename Predicate>
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>;
599 if constexpr (CALLBACK_RETURNS_BOOL) {
600 if (callback(*node.element)) {
604 callback(*node.element);
607 if constexpr (CALLBACK_RETURNS_BOOL) {
611 std::forward<Predicate>(predicate));
645 template <
typename Callback>
647 return traverseElements(std::forward<Callback>(callback), [](
const Box<2, float>&) ->
bool {
return true; });
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); });
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); });
743 template <
typename Callback>
745 return traverseElements(std::forward<Callback>(callback), [&box](
const Box<2, float>& looseBounds) ->
bool {
return intersects(looseBounds, box); });
768 return traverseElements([](
const T&) ->
bool {
return true; }, [&box](
const Box<2, float>& looseBounds) ->
bool {
return intersects(looseBounds, box); });
772 struct IterationState {
778 [[nodiscard]]
static constexpr std::size_t chooseQuadrant(vec2 aabbCenter, vec2& center,
float halfQuadrantSize) {
780 if (aabbCenter.x < center.x) {
781 center.x -= halfQuadrantSize;
782 if (aabbCenter.y < center.y) {
783 center.y -= halfQuadrantSize;
786 center.y += halfQuadrantSize;
790 center.x += halfQuadrantSize;
791 if (aabbCenter.y < center.y) {
792 center.y -= halfQuadrantSize;
795 center.y += halfQuadrantSize;
802 template <
typename Callback>
803 static constexpr void forEachActiveQuadrant(
const QuadrantIndexArray& subQuadrantIndices, vec2 center,
float halfQuadrantSize,
804 Callback&& callback) {
805 if (
const TreeIndex quadrantIndex = subQuadrantIndices[0]) {
806 callback(quadrantIndex, vec2{center.x - halfQuadrantSize, center.y - halfQuadrantSize});
808 if (
const TreeIndex quadrantIndex = subQuadrantIndices[1]) {
809 callback(quadrantIndex, vec2{center.x - halfQuadrantSize, center.y + halfQuadrantSize});
811 if (
const TreeIndex quadrantIndex = subQuadrantIndices[2]) {
812 callback(quadrantIndex, vec2{center.x + halfQuadrantSize, center.y - halfQuadrantSize});
814 if (
const TreeIndex quadrantIndex = subQuadrantIndices[3]) {
815 callback(quadrantIndex, vec2{center.x + halfQuadrantSize, center.y + halfQuadrantSize});
819 template <
typename Callback,
typename Predicate>
820 constexpr auto traverseNodesImpl(Callback&& callback, Predicate&& predicate)
const {
821 constexpr bool CALLBACK_RETURNS_BOOL = std::is_convertible_v<std::invoke_result_t<Callback, const Box<2, float>&,
const Quadrant&>,
bool>;
823 iterationStack.clear();
824 iterationStack.push_back(IterationState{
825 .center = rootCenter,
826 .quadrantSize = halfRootSize,
830 const auto [center, quadrantSize, treeIndex] = iterationStack.back();
831 iterationStack.pop_back();
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)) {
841 callback(looseBounds, node);
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},
851 if (predicate(looseBounds)) {
852 iterationStack.push_back(IterationState{
853 .center = quadrantCenter,
854 .quadrantSize = halfQuadrantSize,
855 .treeIndex = quadrantIndex,
859 }
while (!iterationStack.empty());
861 if constexpr (CALLBACK_RETURNS_BOOL) {
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) {
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) {
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{};
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