diff --git a/include/J3ML/Geometry/QuadTree.h b/include/J3ML/Geometry/QuadTree.h index 66de42a..fa42e35 100644 --- a/include/J3ML/Geometry/QuadTree.h +++ b/include/J3ML/Geometry/QuadTree.h @@ -5,15 +5,13 @@ #include #include "AABB2D.h" -namespace Geometry -{ +namespace Geometry { using LinearAlgebra::Vector2; - template - class QuadTree - { + + template + class QuadTree { public: - struct Node - { + struct Node { Node *parent; uint32_t childIndex; std::vector objects; @@ -21,19 +19,22 @@ namespace Geometry Vector2 center; Vector2 radius; - bool IsLeaf() const { return childIndex == 0xFFFFFFFF;} + bool IsLeaf() const { return childIndex == 0xFFFFFFFF; } - uint32_t TopLeftChildIndex() const { return childIndex;} - uint32_t TopRightChildIndex() const { return childIndex+1;} - uint32_t BottomLeftChildIndex() const { return childIndex+2;} - uint32_t BottomRightChildIndex() const { return childIndex+3;} + uint32_t TopLeftChildIndex() const { return childIndex; } + + uint32_t TopRightChildIndex() const { return childIndex + 1; } + + uint32_t BottomLeftChildIndex() const { return childIndex + 2; } + + uint32_t BottomRightChildIndex() const { return childIndex + 3; } /// This assumes that the QuadTree contains unique objects and never duplicates - void Remove(const T &object) - { - for (size_t i = 0; i < objects.size(); ++i){ + void Remove(const T &object) { + for (size_t i = 0; i < objects.size(); ++i) { if (objects[i] == object) { - AssociateQuadTreeNode(object, 0); // Mark in the object that it has been removed from the QuadTree. + AssociateQuadTreeNode(object, + 0); // Mark in the object that it has been removed from the QuadTree. std::swap(objects[i], objects.back()); objects.pop_back(); return; @@ -41,11 +42,9 @@ namespace Geometry } } - AABB2D ComputeAABB() - {} + AABB2D ComputeAABB() {} - float DistanceSq(const Vector2& point) const - { + float DistanceSq(const Vector2 &point) const { Vector2 centered = point - center; Vector2 closestPoint = centered.Clamp(-radius, radius); return closestPoint.DistanceSq(centered); @@ -54,30 +53,28 @@ namespace Geometry }; // Helper struct used when traversing through the tree - struct TraversalStackItem - { + struct TraversalStackItem { Node *node; }; - QuadTree(): - rootNodeIndex(-1), - boundingAABB(Vector2(0,0), Vector2(0,0)) - { + QuadTree() : + rootNodeIndex(-1), + boundingAABB(Vector2(0, 0), Vector2(0, 0)) { // TODO: currently storing persistent raw pointers to this array outside the array nodes.reserve(200000); } /// Removes all nodes and objects in this tree and reintializes the tree to a single root node. - void Clear(const Vector2& minXY = Vector2(-1, -1), const Vector2& maxXY = Vector2(1, 1)); + void Clear(const Vector2 &minXY = Vector2(-1, -1), const Vector2 &maxXY = Vector2(1, 1)); /// Places the given object onto the proper (leaf) node of the tree. After placing, if the leaf split rule is /// satisfied, subdivides the leaf node into 4 subquadrants and reassings the objects to the new leaves. - void Add(const T& object); + void Add(const T &object); /// Removes the given object from this tree. /// To call this function, you must define a function QuadTree::Node *GetQuadTreeNode(const T& object) /// which returns the node of this quadtree where the object resides in. - void Remove(const T& object); + void Remove(const T &object); /// @return The bounding rectangle for the whole tree. /// @note This bounding rectangle does not tightly bound the objects themselves, only the root node of the tree. @@ -85,6 +82,7 @@ namespace Geometry /// @return The topmost node in the tree. Node *Root(); + const Node *Root() const; /// Returns the total number of nodes (all nodes, i.e. inner nodes + leaves) in the tree. @@ -123,6 +121,7 @@ namespace Geometry /// specified callback function. template inline void CollidingPairsQuery(const AABB2D &aabb, Func &callback); + /// Performs various consistency checks on the given node. Use only for debugging purposes. void DebugSanityCheckNode(Node *n); @@ -139,8 +138,11 @@ namespace Geometry AABB2D boundingAABB; void GrowRootTopLeft(); + void GrowRootTopRight(); + void GrowRootBottomLeft(); + void GrowRootBottomRight(); void GrowImpl(int quadrantForRoot); @@ -151,9 +153,8 @@ namespace Geometry // but the presence of the implementation file is a requirement - template - void QuadTree::Clear(const Vector2& minXY, const Vector2& maxXY) - { + template + void QuadTree::Clear(const Vector2 &minXY, const Vector2 &maxXY) { nodes.clear(); boundingAABB.minPoint = minXY; @@ -165,4 +166,193 @@ namespace Geometry root->center = (minXY + maxXY) * 0.5f; root->radius = maxXY - root->center; } + + template + void QuadTree::Add(const T &object) { + Node *n = Root(); + AABB2D objectAABB = GetAABB2D(object); + + // Ramen Noodle Bowls of nested if statements are generally bad practice + // Unfortunately, sometimes the problem domain makes this unavoidable + + if (objectAABB.minPoint.x >= boundingAABB.minPoint.x) { + // object fits left. + if (objectAABB.maxPoint.x <= boundingAABB.maxPoint.x) { + // object fits left and right. + if (objectAABB.minPoint.y >= boundingAABB.minPoint.y) { + // Object fits left, right, and top. + if (objectAABB.maxPoint.y <= boundingAABB.maxPoint.y) { + // Object fits the whole root AABB. Can safely add into the existing tree size. + AddObject(object, n); + return; + } else { + // Object fits left, right, and top, but not bottom. + GrowRootBottomRight(); // Could grow bottom-left as well, wouldn't matter here. + } + } else { + // Object fits left and right, but not to top. + GrowRootTopRight(); // Could grow top-left as well, wouldn't matter here. + } + } else { + // Object fits left, but not to right. We must grow right. Check whether to grow top or bottom; + if (objectAABB.minPoint.y < boundingAABB.minPoint.y) + GrowRootTopRight(); + else + GrowRootBottomRight(); + } + } else { + // We must grow left. Check whether to grow top or bottom. + if (objectAABB.minPoint.y < boundingAABB.minPoint.y) + GrowRootTopLeft(); + else + GrowRootBottomLeft(); + } + // Now that we have grown the tree root node, try adding again. + Add(object); + } + + template + void QuadTree::Remove(const T &object) { + Node *n = GetQuadTreeNode(object); + if (n) { + n->Remove(object); + } + } + + template + void QuadTree::Add(const T &object, Node *n) { + for (;;) { + // Traverse the QuadTree to decide which quad to place this object on. + float left = n->center.x - MinX(object); // If left > 0.f, then the object overlaps with the left quadrant. + float right = MaxX(object) - n->center.x; // If right > 0.f, then the object overlaps with the right quadrant. + + float top = n->center.y - MinY(object); // If top > 0.f, then the object overlaps with the top quadrant + float bottom = + MaxY(object) - n->center.y; // If bottom > 0.f, then the object overlaps with the bottom quadrant + float leftAndRight = std::min(left, right); // If > 0.f, then the object straddles left-right halves. + float topAndBottom = std::min(top, bottom); // If > 0.f, then the object straddles top-bottom halves. + float straddledEitherOne = std::max(leftAndRight, + topAndBottom); // If > 0.f, thne the object is in two or more quadrants. + + // Note: It can happen that !left && !right, or !top && !bottom. + // but the if()s are setup below so that right/bottom is taken if no left/top, so that is ok. + + // We must put the object onto this node if + // a) the object straddled the parent->child split lines. + // b) this object is a leaf + if (straddledEitherOne > 0.f) { + n->objects.push_back(object); + AssociateQuadTreeNode(object, n); + return; + } + if (n->IsLeaf()) { + n->objects.push_back(object); + AssociateQuadTreeNode(object, n); + + if ((int) n->objects.size() > minQuadTreeNodeObjectCount && + std::min(n->radius.x, n->radius.y) >= minQuadTreeQuadrantSize) { + SplitLeaf(n); + } + + return; + } + if (left > 0.f) { + if (top > 0.f) { + n = &nodes[n->TopLeftChildIndex()]; + } else { + n = &nodes[n->BottomLeftChildIndex()]; + } + } else { + if (top > 0.f) { + n = &nodes[n->TopRightChildIndex()]; + } else { + n = &nodes[n->BottomRightChildIndex()]; + } + } + } + } + + template + typename QuadTree::Node *QuadTree::Root() { + return nodes.empty() ? 0 : &nodes[rootNodeIndex]; + } + + template + const typename QuadTree::Node *QuadTree::Root() const { + return nodes.empty() ? 0 : &nodes[rootNodeIndex]; + } + + template + int QuadTree::AllocateNodeGroup(Node* parent) { + size_t oldCap = nodes.capacity(); + + int index = (int)nodes.size(); + Node n; + n.parent = parent; + n.childIndex = 0xFFFFFFFF; + if (parent) { + n.radius = parent->radius * 0.5f; + n.center = parent->center - n.radius; + } + + nodes.push_back(n); + if (parent) + n.center.x = parent->center.x + n.radius.x; + nodes.push_back(n); + + if (parent) { + n.center.x = parent->center.x - n.radius.x; + n.center.y = parent->center.y + n.radius.y; + } + + nodes.push_back(n); + if (parent) + n.center.x = parent->center.x + n.radius.x; + nodes.push_back(n); + + return index; + } + + template + void QuadTree::SplitLeaf(Node *leaf) { + leaf->childIndex = AllocateNodeGroup(leaf); + + size_t i = 0; + while (i < leaf->objects.size()) { + const T& object = leaf->objects[i]; + + // Traverse the QuadTree to decide which quad to place this object into + float left = leaf->center.x - MinX(object); + float right = MaxX(object) - leaf->center; + float top = leaf->center.y - MinY(object); + float bottom = MaxY(object) - leaf->center.y; + float leftAndRight = std::min(left, right); + float topAndBottom = std::min(top, bottom); + + float straddledEitherOne = std::max(leftAndRight, topAndBottom); + + if (straddledEitherOne > 0.f) { + ++i; + continue; + } + + if (left > 0.f) { + if (top > 0.f) { + Add(object, &nodes[leaf->TopLeftChildIndex()]); + } else { + Add(object, &nodes[leaf->BottomLeftChildIndex()]); + } + } else { + if (top > 0.f) { + Add(object, &nodes[leaf->TopRightChildIndex()]); + } else { + Add(object, &nodes[leaf->BottomRightChildIndex()]); + } + } + + // Remove the object we added to a child from this node. + leaf->objects[i] = leaf->objects.back(); + leaf->objects.pop_back(); + } + } } \ No newline at end of file