QuadTree.h

1 /* Copyright Jukka Jyl�nki
2
3    Licensed under the Apache License, Version 2.0 (the "License");
4    you may not use this file except in compliance with the License.
5    You may obtain a copy of the License at
6
7        http://www.apache.org/licenses/LICENSE-2.0
8
9    Unless required by applicable law or agreed to in writing, software
10    distributed under the License is distributed on an "AS IS" BASIS,
11    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12    See the License for the specific language governing permissions and
13    limitations under the License. */
14
15 /** @file QuadTree.h
16         @author Jukka Jyl�nki
17         @brief A QuadTree spatial query acceleration structure for dynamic data. */
18 #pragma once
19
20 #ifdef MATH_GRAPHICSENGINE_INTEROP
21 #include "Time/Profiler.h"
22 #else
23 #define MGL_PROFILE(x)
24 #endif
25 #include "../Math/float2.h"
26 #include "AABB2D.h"
27 #include "../Math/MathTypes.h"
28
29 #ifdef MATH_CONTAINERLIB_SUPPORT
30 #include "Container/MaxHeap.h"
31 #endif
32
33 MATH_BEGIN_NAMESPACE
34
35 /// A fixed split rule for all QuadTrees: A QuadTree leaf node is only ever split if the leaf contains at least this many objects.
36 /// Leaves containing fewer than this many objects are always kept as leaves until the object count is exceeded.
37 static const int minQuadTreeNodeObjectCount = 16;
38
39 /// A fixed split limit rule for all QuadTrees: If the QuadTree node side length is smaller than this, the node will
40 /// never be split again into smaller subnodes. This provides a hard limit safety net for infinite/extra long recursion
41 /// in case multiple identical overlapping objects are placed into the tree.
42 static const float minQuadTreeQuadrantSize = 0.05f;
43
44 /// Helper for interpreting how to place float3 elements into a QuadTree<float3>.
45 inline float MinX(const float3 &pt) { return pt.x; }
46 inline float MaxX(const float3 &pt) { return pt.x; }
47 inline float MinY(const float3 &pt) { return pt.y; }
48 inline float MaxY(const float3 &pt) { return pt.y; }
49
50 //#ifdef _DEBUG
51 /// If enabled, QuadTree queries generate debug trace/stats logging when invoked. Use only for debugging/behavioral profiling.
52 //#define QUADTREE_VERBOSE_LOGGING
53 //#endif
54
55 template<typename T>
56 class QuadTree
57 {
58 public:
59         /// @note For space compactness, a QuadTree node does not store its own AABB2D extents.
60         struct Node
61         {
62                 /// If 0, this node is the root.
63                 Node *parent;
64                 /// Indicates the quad of child nodes for this node, or 0xFFFFFFFF if this node is a leaf.
65                 u32 childIndex;
66                 /// Stores the actual objects in this node/leaf.
67 #ifdef MATH_CONTAINERLIB_SUPPORT
68                 Array<T> objects;
69 #else
70                 std::vector<T> objects;
71 #endif
72
73                 float2 center;
74                 float2 radius;
75
76                 bool IsLeaf() const { return childIndex == 0xFFFFFFFF; }
77
78                 u32 TopLeftChildIndex() const { return childIndex; }
79                 u32 TopRightChildIndex() const { return childIndex+1; }
80                 u32 BottomLeftChildIndex() const { return childIndex+2; }
81                 u32 BottomRightChildIndex() const { return childIndex+3; }
82
83                 /// This assumes that the QuadTree contains unique objects and never duplicates.
84                 void Remove(const T &object)
85                 {
86                         for(size_t i = 0; i < objects.size(); ++i)
87                                 if (objects[i] == object)
88                                 {
89                                         AssociateQuadTreeNode(object, 0); // Mark in the object that it has been removed from the quadtree.
90                                         std::swap(objects[i], objects.back());
91                                         objects.pop_back();
92                                         return;
93                                 }
94                 }
95
96                 AABB2D ComputeAABB() const
97                 {
98                         return AABB2D(center - radius, center + radius);
99                 }
100
101                 float DistanceSq(const float2 &point) const
102                 {
103                         float2 centered = point - center;
104                         float2 closestPoint = centered.Clamp(-radius, radius);
105                         return closestPoint.DistanceSq(centered);
106 //                      float2 diff = Max(Abs(point - center) - radius, float2(0,0));
107 //                      return diff.LengthSq();
108                 }
109         };
110
111         /// Helper struct used when traversing through the tree.
112         struct TraversalStackItem
113         {
114                 Node *node;
115         };
116
117         QuadTree()
118         :rootNodeIndex(-1),
119         boundingAABB(float2(0,0), float2(1,1))
120 #ifdef QUADTREE_VERBOSE_LOGGING
121         ,totalNumObjectsInTree(0)
122 #endif
123         {
124                 ///\todo Currently storing persistent raw pointers to this array outside the array.
125                 /// Remove the requirement to never reallocate the vector!
126                 nodes.reserve(200000);
127         }
128
129         /// Removes all nodes and objects in this tree and reinitializes the tree to a single root node.
130         void Clear(const float2 &minXY = float2(-1.f, -1.f), const float2 &maxXY = float2(1.f, 1.f));
131
132         /// Places the given object into the proper (leaf) node of the tree. After placing, if the leaf split rule is
133         /// satisfied, subdivides the leaf node into 4 subquadrants and reassigns the objects to new leaves.
134         void Add(const T &object);
135
136         /// Removes the given object from this tree.
137         /// To call this function, you must define a function QuadTree<T>::Node *GetQuadTreeNode(const T &object)
138         /// which returns the node of this quadtree where the object resides in.
139         void Remove(const T &object);
140
141         /// @return The bounding rectangle for the whole tree.
142         /// @note This bounding rectangle does not tightly bound the objects themselves, only the root node of the tree.
143         AABB2D BoundingAABB() const { return boundingAABB; }
144
145         /// @return The topmost node in the tree.
146         Node *Root();
147         const Node *Root() const;
148
149         /// Returns the total number of nodes (all nodes, i.e. inner nodes + leaves) in the tree.
150         /// Runs in constant time.
151         int NumNodes() const;
152
153         /// Returns the total number of leaf nodes in the tree.
154         /// @warning Runs in time linear 'O(n)' to the number of nodes in the tree.
155         int NumLeaves() const;
156
157         /// Returns the total number of inner nodes in the tree.
158         /// @warning Runs in time linear 'O(n)' to the number of nodes in the tree.
159         int NumInnerNodes() const;
160
161         /// Returns the total number of objects stored in the tree.
162         /// @warning Runs in time linear 'O(n)' to the number of nodes in the tree.
163         int NumObjects() const;
164
165         /// Returns the maximum height of the whole tree (the path from the root to the farthest leaf node).
166         int TreeHeight() const;
167
168         /// Returns the height of the subtree rooted at 'node'.
169         int TreeHeight(const Node *node) const;
170
171         /// Performs an AABB intersection query in this Quadtreee, and calls the given callback function for each non-empty
172         /// node of the tree which intersects the given AABB.
173         /** @param aabb The axis-aligned bounding box to intersect this QuadTree with.
174                 @param callback A function or a function object of prototype
175                         bool callbackFunction(QuadTree<T> &tree, const AABB2D &queryAABB, QuadTree<T>::Node &node);
176                 If the callback function returns true, the execution of the query is stopped and this function immediately
177                 returns afterwards. If the callback function returns false, the execution of the query continues. */
178         template<typename Func>
179         inline void AABBQuery(const AABB2D &aabb, Func &callback);
180
181         /// Finds all object pairs inside the given AABB which have colliding AABBs. For each such pair, calls the
182         /// specified callback function.
183         template<typename Func>
184         inline void CollidingPairsQuery(const AABB2D &aabb, Func &callback);
185
186 #ifdef MATH_CONTAINERLIB_SUPPORT
187         /// Performs a node-granular nearest neighbor search on this QuadTree.
188         /** This query calls the given nodeCallback function for each node of this QuadTree that contains objects, sorted by closest first
189                 to the target point. At any given time, the nodeCallback function may terminate the search by returning true in its callback.
190                 @param point The
191                 @param nodeCallback A function or a function object of prototype
192                    bool NodeCallbackFunction(QuadTree<T> &tree, const float2 &targetPoint, QuadTree<T>::Node &node, float minDistanceSquared);
193                    If the callback function returns true, the execution of the query is immediately stopped.
194                    If the callback function returns false, the execution of the query continues.
195                    tree points to this QuadTree, in which the query is being performed.
196                    targetPoint is the point passed in the function call to NearestNeighborObjects.
197                    node points to the QuadTree (leaf) node that is being traversed.
198                    minDistanceSquared is the squared minimum distance the objects in this node (and all future nodes to be passed to the
199                    callback) have to the point that is being queried. */
200         template<typename Func>
201         inline void NearestNeighborNodes(const float2 &point, Func &nodeCallback);
202
203         /// Performs an object-granular nearest neighbor search on this QuadTree.
204         /** This query calls the given objectCallback function for each object in this QuadTree, starting from the object closest to the
205                 given target point, and proceeding in distance-sorted order.
206                 @param targetPoint The target point to find the nearest neighbors to.
207                 @param objectCallback The function object that should be invoked by the query for each object. This function should be of prototype
208                    bool NearestNeighborObjectCallback(QuadTree<T> &tree, const float2 &targetPoint, QuadTree<T>::Node *node,
209                                                       float distanceSquared, const T &nearestNeighborObject, int nearestNeighborIndex);
210                    If this function returns true, the execution of the query is immediately stopped.
211                    If the callback function returns false, the execution of the query continues.
212                    tree points to this QuadTree, in which the query is being performed.
213                    targetPoint is the point passed in the function call to NearestNeighborObjects.
214                    node points to the QuadTree (leaf) node where nearestNeighborObject resides in.
215                    distanceSquared is the squared distance between targetPoint and nearestNeighborObject.
216                    nearestNeighborObject gives the next closest object to targetPoint.
217                    nearestNeighborIndex provides a conveniency counter that tells how many nearest neighbors are closer to targetPoint than this object. */
218         template<typename Func>
219         inline void NearestNeighborObjects(const float2 &targetPoint, Func &objectCallback);
220 #endif
221
222         /// Performs various consistency checks on the given node. Use only for debugging purposes.
223         void DebugSanityCheckNode(Node *n);
224
225 private:
226         void Add(const T &object, Node *n);
227
228         /// Allocates a sequential 4-tuple of QuadtreeNodes, contiguous in memory.
229         int AllocateNodeGroup(Node *parent);
230
231         void SplitLeaf(Node *leaf);
232
233 //#ifdef MATH_CONTAINERLIB_SUPPORT
234 //      Array<Node> nodes;
235 //#else
236         std::vector<Node> nodes;
237 //#endif
238
239         /// Specifies the index to the root node, or -1 if there is no root (nodes.size() == 0).
240         int rootNodeIndex;
241         AABB2D boundingAABB;
242
243         void GrowRootTopLeft();
244         void GrowRootTopRight();
245         void GrowRootBottomLeft();
246         void GrowRootBottomRight();
247
248         void GrowImpl(int quadrantForRoot);
249
250 #ifdef QUADTREE_VERBOSE_LOGGING
251         int totalNumObjectsInTree;
252 #endif
253 };
254
255 inline void AssociateQuadTreeNode(const float3 &, QuadTree<float3>::Node *) {}
256
257 MATH_END_NAMESPACE
258
259 #include "QuadTree.inl"