QuadTree.inl

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.inl
16         @author Jukka Jyl�nki
17         @brief Implementation for the QuadTree object. */
18 #pragma once
19
20 #include "../Math/MathFunc.h"
21
22 MATH_BEGIN_NAMESPACE
23
24 template<typename T>
25 void QuadTree<T>::Clear(const float2 &minXY, const float2 &maxXY)
26 {
27         nodes.clear();
28
29         boundingAABB.minPoint = minXY;
30         boundingAABB.maxPoint = maxXY;
31
32         assert(!boundingAABB.IsDegenerate());
33
34         rootNodeIndex = AllocateNodeGroup(0);
35         assert(Root());
36         Node *root = Root();
37         root->center = (minXY + maxXY) * 0.5f;
38         root->radius = maxXY - root->center;
39
40 #ifdef QUADTREE_VERBOSE_LOGGING
41         totalNumObjectsInTree = 0;
42 #endif
43 }
44
45 template<typename T>
46 void QuadTree<T>::Add(const T &object)
47 {
48         MGL_PROFILE(QuadTree_Add);
49         Node *n = Root();
50         assert(n && "Error: QuadTree has not been initialized with a root node! Call QuadTree::Clear() to initialize the root node.");
51
52         assert(boundingAABB.IsFinite());
53         assert(!boundingAABB.IsDegenerate());
54
55         AABB2D objectAABB = GetAABB2D(object);
56         assert(objectAABB.IsFinite());
57         assert(!objectAABB.HasNegativeVolume());
58
59 #ifdef QUADTREE_VERBOSE_LOGGING
60         ++totalNumObjectsInTree;
61 #endif
62
63         if (objectAABB.minPoint.x >= boundingAABB.minPoint.x)
64         {
65                 // Object fits left.
66
67                 if (objectAABB.maxPoint.x <= boundingAABB.maxPoint.x)
68                 {
69                         // Object fits left and right.
70
71                         if (objectAABB.minPoint.y >= boundingAABB.minPoint.y)
72                         {
73                                 // Object fits left, right and top.
74                                 if (objectAABB.maxPoint.y <= boundingAABB.maxPoint.y)
75                                 {
76                                         // Object fits the whole root AABB. Can safely add into the existing tree size.
77                                         Add(object, n);
78                                         return;
79                                 }
80                                 else
81                                 {
82                                         // Object fits left, right and top, but not bottom.
83                                         GrowRootBottomRight(); // Could grow bottom-left as well, wouldn't matter here.
84                                 }
85                         }
86                         else
87                         {
88                                 // Object fits left and right, but not to top.
89                                 GrowRootTopRight(); // Could grow top-left as well, wouldn't matter here.
90                         }
91                 }
92                 else
93                 {
94                         // Object fits left, but not to right. We must grow right. Check whether to grow top or bottom.
95                         if (objectAABB.minPoint.y < boundingAABB.minPoint.y)
96                                 GrowRootTopRight();
97                         else
98                                 GrowRootBottomRight();
99                 }
100         }
101         else
102         {
103                 // We must grow left. Check whether to grow top or bottom.
104                 if (objectAABB.minPoint.y < boundingAABB.minPoint.y)
105                         GrowRootTopLeft();
106                 else
107                         GrowRootBottomLeft();
108         }
109
110         // Now that we have grown the tree root node, try adding again.
111         Add(object);
112 }
113
114 template<typename T>
115 void QuadTree<T>::Remove(const T &object)
116 {
117         Node *n = GetQuadTreeNode(object);
118         if (n)
119         {
120                 n->Remove(object);
121
122 #ifdef QUADTREE_VERBOSE_LOGGING
123                 --totalNumObjectsInTree;
124 #endif
125         }
126 }
127
128 template<typename T>
129 void QuadTree<T>::Add(const T &object, Node *n)
130 {
131         for(;;)
132         {
133                 // Traverse the QuadTree to decide which quad to place this object into.
134                 assert(MinX(object) <= MaxX(object));
135                 float left = n->center.x - MinX(object); // If left > 0.f, then the object overlaps with the left quadrant.
136                 float right = MaxX(object) - n->center.x; // If right > 0.f, then the object overlaps with the right quadrant.
137                 assert(MinY(object) <= MaxY(object));
138                 float top = n->center.y - MinY(object); // If top > 0.f, then the object overlaps with the top quadrant.
139                 float bottom = MaxY(object) - n->center.y; // If bottom > 0.f, then the object overlaps with the bottom quadrant.
140                 float leftAndRight = Min(left, right); // If > 0.f, then the object straddles left-right halves.
141                 float topAndBottom = Min(top, bottom); // If > 0.f, then the object straddles top-bottom halves.
142                 float straddledEitherOne = Max(leftAndRight, topAndBottom); // If > 0.f, then the object is in two or more quadrants.
143
144                 // Note: It can happen that !left && !right, or !top && !bottom,
145                 // but the if()s below are set up so that right/bottom is taken if no left/top, so that is ok.
146
147                 // We must put the object onto this node if
148                 // a) the object straddled the parent->child split lines.
149                 // b) this object is a leaf.
150                 if (straddledEitherOne > 0.f)
151                 {
152                         n->objects.push_back(object);
153                         AssociateQuadTreeNode(object, n);
154                         return;
155                 }
156                 if (n->IsLeaf())
157                 {
158                         n->objects.push_back(object);
159                         AssociateQuadTreeNode(object, n);
160                         if ((int)n->objects.size() > minQuadTreeNodeObjectCount && Min(n->radius.x, n->radius.y) >= minQuadTreeQuadrantSize)
161                                 SplitLeaf(n);
162                         return;
163                 }
164                 if (left > 0.f)
165                 {
166                         if (top > 0.f)
167                         {
168                                 assert(nodes[n->TopLeftChildIndex()].parent == n);
169                                 n = &nodes[n->TopLeftChildIndex()];
170                         }
171                         else
172                         {
173                                 assert(nodes[n->BottomLeftChildIndex()].parent == n);
174                                 n = &nodes[n->BottomLeftChildIndex()];
175                         }
176                 }
177                 else
178                 {
179                         if (top > 0.f)
180                         {
181                                 assert(nodes[n->TopRightChildIndex()].parent == n);
182                                 n = &nodes[n->TopRightChildIndex()];
183                         }
184                         else
185                         {
186                                 assert(nodes[n->BottomRightChildIndex()].parent == n);
187                                 n = &nodes[n->BottomRightChildIndex()];
188                         }
189                 }
190         }
191 }
192
193 template<typename T>
194 typename QuadTree<T>::Node *QuadTree<T>::Root()
195 {
196         return nodes.empty() ? 0 : &nodes[rootNodeIndex];
197 }
198
199 template<typename T>
200 const typename QuadTree<T>::Node *QuadTree<T>::Root() const
201 {
202         return nodes.empty() ? 0 : &nodes[rootNodeIndex];
203 }
204
205 template<typename T>
206 int QuadTree<T>::AllocateNodeGroup(Node *parent)
207 {
208 #ifdef _DEBUG
209         size_t oldCap = nodes.capacity();
210 #endif
211         int index = (int)nodes.size();
212         Node n;
213         n.parent = parent;
214         n.childIndex = 0xFFFFFFFF;
215         // The nodes are in order top-left (--), top-right, bottom-left, bottom-right.
216         if (parent)
217         {
218                 n.radius = parent->radius * 0.5f;
219                 n.center = parent->center - n.radius;
220         }
221         nodes.push_back(n);
222         if (parent)
223                 n.center.x = parent->center.x + n.radius.x;
224         nodes.push_back(n);
225         if (parent)
226         {
227                 n.center.x = parent->center.x - n.radius.x;
228                 n.center.y = parent->center.y + n.radius.y;
229         }
230         nodes.push_back(n);
231         if (parent)
232                 n.center.x = parent->center.x + n.radius.x;
233         nodes.push_back(n);
234 #ifdef _DEBUG
235         assert(nodes.capacity() == oldCap); // Limitation: Cannot resize the nodes vector!
236 #endif
237         return index;
238 }
239
240 template<typename T>
241 void QuadTree<T>::SplitLeaf(Node *leaf)
242 {
243         assert(leaf->IsLeaf());
244         assert(leaf->childIndex == 0xFFFFFFFF);
245
246         leaf->childIndex = AllocateNodeGroup(leaf);
247
248         size_t i = 0;
249         while(i < leaf->objects.size())
250         {
251                 const T &object = leaf->objects[i];
252
253                 // Traverse the QuadTree to decide which quad to place this object into.
254                 assert(MinX(object) <= MaxX(object));
255                 float left = leaf->center.x - MinX(object); // If left > 0.f, then the object overlaps with the left quadrant.
256                 float right = MaxX(object) - leaf->center.x; // If right > 0.f, then the object overlaps with the right quadrant.
257                 assert(MinY(object) <= MaxY(object));
258                 float top = leaf->center.y - MinY(object); // If top > 0.f, then the object overlaps with the top quadrant.
259                 float bottom = MaxY(object) - leaf->center.y; // If bottom > 0.f, then the object overlaps with the bottom quadrant.
260                 float leftAndRight = Min(left, right); // If > 0.f, then the object straddles left-right halves.
261                 float topAndBottom = Min(top, bottom); // If > 0.f, then the object straddles top-bottom halves.
262                 float straddledEitherOne = Max(leftAndRight, topAndBottom); // If > 0.f, then the object is in two or more quadrants.
263
264                 // Note: It can happen that !left && !right, or !top && !bottom,
265                 // but the if()s below are set up so that right/bottom is taken if no left/top, so that is ok.
266
267                 // We must leave this object in this node if the object straddled the parent->child split lines.
268                 if (straddledEitherOne > 0.f)
269                 {
270                         ++i;
271                         continue;
272                 }
273
274                 if (left > 0.f)
275                 {
276                         if (top > 0.f)
277                         {
278                                 Add(object, &nodes[leaf->TopLeftChildIndex()]);
279                         }
280                         else
281                         {
282                                 Add(object, &nodes[leaf->BottomLeftChildIndex()]);
283                         }
284                 }
285                 else
286                 {
287                         if (top > 0.f)
288                         {
289                                 Add(object, &nodes[leaf->TopRightChildIndex()]);
290                         }
291                         else
292                         {
293                                 Add(object, &nodes[leaf->BottomRightChildIndex()]);
294                         }
295                 }
296
297                 // Remove the object we added to a child from this node.
298                 leaf->objects[i] = leaf->objects.back();
299                 leaf->objects.pop_back();
300         }
301 }
302
303 template<typename T>
304 template<typename Func>
305 inline void QuadTree<T>::AABBQuery(const AABB2D &aabb, Func &callback)
306 {
307         MGL_PROFILE(QuadTree_AABBQuery);
308         std::vector<TraversalStackItem> stack;
309         TraversalStackItem n;
310         n.node = Root();
311         if (!n.node || !aabb.Intersects(BoundingAABB()))
312                 return;
313         stack.push_back(n);
314
315         while(!stack.empty())
316         {
317                 TraversalStackItem i = stack.back();
318                 stack.pop_back();
319
320                 // aabb intersects the node's aabb.
321                 // Which aabb's of the four child quadrants does it intersect?
322
323                 if (i.node->objects.size() > 0)
324                 {
325                         if (callback(*this, aabb, *i.node))
326                                 return;
327                 }
328                 if (!i.node->IsLeaf())
329                 {
330                         if (aabb.minPoint.x <= i.node->center.x && aabb.minPoint.y <= i.node->center.y)
331                         {
332                                 TraversalStackItem child;
333                                 child.node = &nodes[i.node->TopLeftChildIndex()];
334                                 stack.push_back(child);
335                         }
336                         if (aabb.maxPoint.x >= i.node->center.x && aabb.maxPoint.y >= i.node->center.y)
337                         {
338                                 TraversalStackItem child;
339                                 child.node = &nodes[i.node->BottomRightChildIndex()];
340                                 stack.push_back(child);
341                         }
342                         if (aabb.minPoint.x <= i.node->center.x && aabb.maxPoint.y >= i.node->center.y)
343                         {
344                                 TraversalStackItem child;
345                                 child.node = &nodes[i.node->BottomLeftChildIndex()];
346                                 stack.push_back(child);
347                         }
348                         if (aabb.maxPoint.x >= i.node->center.x && aabb.minPoint.y <= i.node->center.y)
349                         {
350                                 TraversalStackItem child;
351                                 child.node = &nodes[i.node->TopRightChildIndex()];
352                                 stack.push_back(child);
353                         }
354                 }
355         }
356 }
357
358 template<typename T, typename Func>
359 class FindCollidingPairs
360 {
361 public:
362         Func *collisionCallback;
363
364         bool operator ()(QuadTree<T> & /*tree*/, const AABB2D &queryAABB, typename QuadTree<T>::Node &node)
365         {
366                 for(size_t i = 0; i < node.objects.size(); ++i)
367                 {
368                         AABB2D aabbI = GetAABB2D(node.objects[i]);
369                         if (!queryAABB.Intersects(aabbI))
370                                 continue;
371
372                         for(size_t j = i+1; j < node.objects.size(); ++j)
373                         {
374                                 AABB2D aabbJ = GetAABB2D(node.objects[j]);
375                                 if (aabbI.Intersects(aabbJ))
376                                         (*collisionCallback)(node.objects[i], node.objects[j]);
377                         }
378
379                         typename QuadTree<T>::Node *n = node.parent;
380                         while(n)
381                         {
382                                 for(size_t j = 0; j < n->objects.size(); ++j)
383                                 {
384                                         AABB2D aabbJ = GetAABB2D(n->objects[j]);
385                                         if (aabbI.Intersects(aabbJ))
386                                                 (*collisionCallback)(node.objects[i], n->objects[j]);
387                                 }
388                                 assert(n != n->parent);
389                                 n = n->parent;
390                         }
391                 }
392                 return false;
393         }
394 };
395
396 template<typename T>
397 template<typename Func>
398 inline void QuadTree<T>::CollidingPairsQuery(const AABB2D &aabb, Func &callback)
399 {
400         MGL_PROFILE(QuadTree_CollidingPairsQuery);
401         FindCollidingPairs<T, Func> func;
402         func.collisionCallback = &callback;
403         AABBQuery(aabb, func);
404 }
405
406 template<typename T>
407 struct TraversalNode
408 {
409         /// The squared distance of this node to the query point.
410         float d;
411         typename QuadTree<T>::Node *node;
412
413         struct TriCmp { float operator()(const TraversalNode &a, const TraversalNode &b) { return b.d - a.d; } };
414         struct EqualCmp { bool operator()(const TraversalNode &a, const TraversalNode &b) { return b.d == a.d; } };
415
416         /// We compare in reverse order, since we want the node with the smallest distance to be visited first,
417         /// and MaxHeap stores the node that compares largest in the root.
418         bool operator <(const TraversalNode &t) const { return d > t.d; }
419         bool operator ==(const TraversalNode &t) const { return d == t.d; }
420
421         static void Swap(TraversalNode &a, TraversalNode &b)
422         {
423                 float x = a.d;
424                 a.d = b.d;
425                 b.d = x;
426                 typename QuadTree<T>::Node *temp = a.node;
427                 a.node = b.node;
428                 b.node = temp;
429 //              std::swap(a, b);
430         }
431 };
432
433 #ifdef MATH_CONTAINERLIB_SUPPORT
434 template<typename T>
435 template<typename Func>
436 inline void QuadTree<T>::NearestNeighborNodes(const float2 &point, Func &leafCallback)
437 {
438         MaxHeap<TraversalNode<T>, typename TraversalNode<T>::TriCmp, typename TraversalNode<T>::EqualCmp > queue;
439
440         {
441                 TraversalNode<T> &rootNode = queue.BeginInsert();
442                 rootNode.d = 0.f;
443                 rootNode.node = Root();
444                 queue.FinishInsert();
445         }
446
447         while(queue.Size() > 0)
448         {
449                 const TraversalNode<T> &t = queue.Front();
450
451                 if (t.node->objects.size() > 0)
452                 {
453                         bool stopIteration = leafCallback(*this, point, *t.node, t.d);
454                         if (stopIteration)
455                                 return;
456                 }
457
458                 if (!t.node->IsLeaf())
459                 {
460                         typename QuadTree<T>::Node *childNode = &nodes[t.node->childIndex];
461                         queue.PopFront();
462
463                         // Insert top-left child node to the traversal queue.
464                         {
465                                 TraversalNode<T> &n = queue.BeginInsert();
466                                 n.node = childNode; // t.node->TopLeftChildIndex()
467                                 n.d = n.node->DistanceSq(point);
468                                 queue.FinishInsert();
469                         }
470
471                         // Insert top-right child node to the traversal queue.
472                         {
473                                 TraversalNode<T> &n = queue.BeginInsert();
474                                 n.node = childNode + 1; // t.node->TopRightChildIndex()
475                                 n.d = n.node->DistanceSq(point);
476                                 queue.FinishInsert();
477                         }
478
479                         // Insert bottom-left child node to the traversal queue.
480                         {
481                                 TraversalNode<T> &n = queue.BeginInsert();
482                                 n.node = childNode + 2; // t.node->BottomLeftChildIndex()
483                                 n.d = n.node->DistanceSq(point);
484                                 queue.FinishInsert();
485                         }
486
487                         // Insert bottom-right child node to the traversal queue.
488                         {
489                                 TraversalNode<T> &n = queue.BeginInsert();
490                                 n.node = childNode + 3; // t.node->BottomRightChildIndex()
491                                 n.d = n.node->DistanceSq(point);
492                                 queue.FinishInsert();
493                         }
494                 }
495                 else
496                         queue.PopFront();
497         }
498 }
499
500 template<typename ObjectCallbackFunc, typename T>
501 struct NearestNeighborObjectSearch
502 {
503         NearestNeighborObjectSearch()
504         :objectCallback(0), numObjectsOutputted(0)
505 #ifdef QUADTREE_VERBOSE_LOGGING
506         ,numNodesVisited(0)
507 #endif
508         {
509         }
510
511         ObjectCallbackFunc *objectCallback;
512
513         struct NearestObject
514         {
515                 /// The squared distance of this node to the query point.
516                 float d;
517
518 //              typename QuadTree<T>::Node *node;
519
520                 T *object;
521
522                 struct TriCmp { float operator()(const NearestObject &a, const NearestObject &b) { return b.d - a.d; } };
523                 struct EqualCmp { bool operator()(const NearestObject &a, const NearestObject &b) { return b.d == a.d; } };
524
525                 /// We compare in reverse order, since we want the object with the smallest distance to be visited first,
526                 /// and MaxHeap stores the object that compares largest in the root.
527                 bool operator <(const NearestObject &t) const { return d > t.d; }
528                 bool operator ==(const NearestObject &t) const { return d == t.d; }
529
530                 static void Swap(NearestObject &a, NearestObject &b)
531                 {
532                         float x = a.d;
533                         a.d = b.d;
534                         b.d = x;
535                         T *o = a.object;
536                         a.object = b.object;
537                         b.object = o;
538                 }
539         };
540
541         MaxHeap<NearestObject, typename NearestObject::TriCmp, typename NearestObject::EqualCmp> queue;
542
543         int numObjectsOutputted;
544
545 #ifdef QUADTREE_VERBOSE_LOGGING
546         int numNodesVisited;
547 #endif
548
549         bool operator ()(QuadTree<T> &tree, const float2 &point, typename QuadTree<T>::Node &leaf, float minDistanceSquared)
550         {
551 #ifdef QUADTREE_VERBOSE_LOGGING
552                 ++numNodesVisited;
553 #endif
554                 // Output all points that are closer than the next closest AABB node.
555                 while(queue.Size() > 0 && queue.Front().d <= minDistanceSquared)
556                 {
557                         const NearestObject &nextNearestPoint = queue.Front();
558                         bool shouldStopIteration = (*objectCallback)(tree, point, 0 /*nextNearestPoint.node*/, nextNearestPoint.d, *nextNearestPoint.object, numObjectsOutputted++);
559                         if (shouldStopIteration)
560                         {
561 #ifdef QUADTREE_VERBOSE_LOGGING
562                                 int numPoints = tree.NumObjects();
563                                 LOGI("Visited %d/%d (%.2f%%) of QuadTree nodes (tree height: %d). Saw %d/%d (%.2f%%) points of the QuadTree before outputting %d points.",
564                                         numNodesVisited, tree.NumNodes(), 100.f * numNodesVisited / tree.NumNodes(), -1/*tree.TreeHeight()*/,
565                                         (int)queue.Size() + numObjectsOutputted, numPoints, ((int)queue.Size() + numObjectsOutputted) * 100.f / numPoints,
566                                         numObjectsOutputted);
567 #endif
568                                 return true;
569                         }
570
571                         queue.PopFront();
572                 }
573
574                 // Queue up all points in the new AABB node.
575                 for(size_t i = 0; i < leaf.objects.size(); ++i)
576                 {
577                         NearestObject &obj = queue.BeginInsert();
578                         obj.d = leaf.objects[i].DistanceSq(point);
579                         obj.object = &leaf.objects[i];
580                         queue.FinishInsert();
581                 }
582
583                 return false;
584         }
585 };
586
587 template<typename T>
588 template<typename Func>
589 inline void QuadTree<T>::NearestNeighborObjects(const float2 &point, Func &leafCallback)
590 {
591         NearestNeighborObjectSearch<Func, T> search;
592         search.objectCallback = &leafCallback;
593
594         NearestNeighborNodes(point, search);
595 }
596 #endif
597
598 template<typename T>
599 void QuadTree<T>::GrowRootTopLeft()
600 {
601         boundingAABB.minPoint.x -= boundingAABB.maxPoint.x - boundingAABB.minPoint.x;
602         boundingAABB.minPoint.y -= boundingAABB.maxPoint.y - boundingAABB.minPoint.y;
603
604         // The old root will become the bottom-right child of the new root, at index 3. Swap the root node to its proper place.
605         GrowImpl(3);
606 }
607
608 template<typename T>
609 void QuadTree<T>::GrowRootTopRight()
610 {
611         boundingAABB.maxPoint.x += boundingAABB.maxPoint.x - boundingAABB.minPoint.x;
612         boundingAABB.minPoint.y -= boundingAABB.maxPoint.y - boundingAABB.minPoint.y;
613
614         // The old root will become the bottom-left child of the new root, at index 2. Swap the root node to its proper place.
615         GrowImpl(2);
616 }
617
618 template<typename T>
619 void QuadTree<T>::GrowRootBottomLeft()
620 {
621         boundingAABB.minPoint.x -= boundingAABB.maxPoint.x - boundingAABB.minPoint.x;
622         boundingAABB.maxPoint.y += boundingAABB.maxPoint.y - boundingAABB.minPoint.y;
623
624         // The old root will become the top-right child of the new root, at index 1. Swap the root node to its proper place.
625         GrowImpl(1);
626 }
627
628 template<typename T>
629 void QuadTree<T>::GrowRootBottomRight()
630 {
631         boundingAABB.maxPoint.x += boundingAABB.maxPoint.x - boundingAABB.minPoint.x;
632         boundingAABB.maxPoint.y += boundingAABB.maxPoint.y - boundingAABB.minPoint.y;
633
634         // The old root will become the top-left child of the new root, at index 0, so no swapping is necessary.
635         GrowImpl(0);
636 }
637
638 template<typename T>
639 void QuadTree<T>::GrowImpl(int quadrantForRoot)
640 {
641         // quadrantForRoot specifies the child quadrant the old root is put to in the new root node.
642
643         // rootNodeIndex always points to the first index of the four quadrants.
644         Node *oldRoot = &nodes[rootNodeIndex+quadrantForRoot];
645
646         if (quadrantForRoot != 0)
647         {
648                 Swap(nodes[rootNodeIndex], nodes[rootNodeIndex+quadrantForRoot]);
649
650                 // Fix up the refs to the swapped old root node.
651                 if (!oldRoot->IsLeaf())
652                 {
653                         nodes[oldRoot->TopLeftChildIndex()].parent = oldRoot;
654                         nodes[oldRoot->TopRightChildIndex()].parent = oldRoot;
655                         nodes[oldRoot->BottomLeftChildIndex()].parent = oldRoot;
656                         nodes[oldRoot->BottomRightChildIndex()].parent = oldRoot;
657                 }
658
659                 // Fix up object->node associations to the swapped old root node.
660                 for(size_t i = 0; i < oldRoot->objects.size(); ++i)
661                         AssociateQuadTreeNode(oldRoot->objects[i], oldRoot);
662         }
663
664         int oldRootNodeIndex = rootNodeIndex;
665         rootNodeIndex = AllocateNodeGroup(0);
666         Node *newRoot = &nodes[rootNodeIndex];
667         newRoot->center = (boundingAABB.minPoint + boundingAABB.maxPoint) * 0.5f;
668         newRoot->radius = boundingAABB.maxPoint - newRoot->center;
669         newRoot->childIndex = oldRootNodeIndex;
670
671         Node *n = &nodes[newRoot->TopLeftChildIndex()];
672         n->parent = newRoot;
673         n->radius = newRoot->radius * 0.5f;
674         n->center = newRoot->center - n->radius;
675
676         n = &nodes[newRoot->TopRightChildIndex()];
677         n->parent = newRoot;
678         n->radius = newRoot->radius * 0.5f;
679         n->center.x = newRoot->center.x + n->radius.x;
680         n->center.y = newRoot->center.y - n->radius.y;
681
682         n = &nodes[newRoot->BottomLeftChildIndex()];
683         n->parent = newRoot;
684         n->radius = newRoot->radius * 0.5f;
685         n->center.x = newRoot->center.x - n->radius.x;
686         n->center.y = newRoot->center.y + n->radius.y;
687
688         n = &nodes[newRoot->BottomRightChildIndex()];
689         n->parent = newRoot;
690         n->radius = newRoot->radius * 0.5f;
691         n->center = newRoot->center + n->radius;
692
693         DebugSanityCheckNode(Root());
694 }
695
696 template<typename T>
697 int QuadTree<T>::NumNodes() const
698 {
699         return std::max<int>(0, nodes.size() - 3); // The nodes rootNodeIndex+1, rootNodeIndex+2 and rootNodeIndex+3 are dummy unused, since the root node is not a quadrant.
700 }
701
702 template<typename T>
703 int QuadTree<T>::NumLeaves() const
704 {
705         int numLeaves = 0;
706         for(int i = 0; i < (int)nodes.size(); ++i)
707                 if (i <= rootNodeIndex || i >= rootNodeIndex + 4) // The nodes rootNodeIndex+1, rootNodeIndex+2 and rootNodeIndex+3 are dummy unused, since the root node is not a quadrant.
708                         if (nodes[i].IsLeaf())
709                                 ++numLeaves;
710
711         return numLeaves;
712 }
713
714 template<typename T>
715 int QuadTree<T>::NumInnerNodes() const
716 {
717         int numInnerNodes = 0;
718         for(int i = 0; i < (int)nodes.size(); ++i)
719                 if (i <= rootNodeIndex || i >= rootNodeIndex + 4) // The nodes rootNodeIndex+1, rootNodeIndex+2 and rootNodeIndex+3 are dummy unused, since the root node is not a quadrant.
720                         if (!nodes[i].IsLeaf())
721                                 ++numInnerNodes;
722
723         return numInnerNodes;
724 }
725
726 template<typename T>
727 int QuadTree<T>::NumObjects() const
728 {
729 #ifdef QUADTREE_VERBOSE_LOGGING
730         return totalNumObjectsInTree;
731 #else
732         int numObjects = 0;
733         for(int i = 0; i < (int)nodes.size(); ++i)
734                 numObjects += (int)nodes[i].objects.size();
735         return numObjects;
736 #endif
737 }
738
739 template<typename T>
740 int QuadTree<T>::TreeHeight(const Node *node) const
741 {
742         if (node->IsLeaf())
743                 return 1;
744         return 1 + Max(TreeHeight(&nodes[node->TopLeftChildIndex()]),
745                        TreeHeight(&nodes[node->TopRightChildIndex()]),
746                        TreeHeight(&nodes[node->BottomLeftChildIndex()]),
747                        TreeHeight(&nodes[node->BottomRightChildIndex()]));
748 }
749
750 template<typename T>
751 int QuadTree<T>::TreeHeight() const
752 {
753         if (!Root())
754                 return 0;
755         return TreeHeight(Root());
756 }
757
758 template<typename T>
759 void QuadTree<T>::DebugSanityCheckNode(Node *n)
760 {
761 #ifdef _DEBUG
762         assert(n);
763         assert(n->parent || n == Root()); // If no parent, must be root.
764         assert(n != Root() || !n->parent); // If not root, must have a parent.
765
766         // Must have a good AABB.
767         AABB2D aabb = n->ComputeAABB();
768         assert(aabb.IsFinite());
769         assert(aabb.minPoint.x <= aabb.maxPoint.x);
770         assert(aabb.minPoint.y <= aabb.maxPoint.y);
771
772         LOGI("Node AABB: %s.", aabb.ToString().c_str());
773         // Each object in this node must be contained in this node.
774         for(size_t i = 0; i < n->objects.size(); ++i)
775         {
776                 LOGI("Object AABB: %s.", GetAABB2D(n->objects[i]).ToString().c_str());
777
778                 assert(aabb.Contains(GetAABB2D(n->objects[i])));
779         }
780
781         // Parent <-> child links must be valid.
782         if (!n->IsLeaf())
783         {
784                 for(int i = 0; i < 4; ++i)
785                 {
786                         Node *child = &nodes[n->TopLeftChildIndex()+i];
787                         assert(child->parent == n);
788
789                         // Must contain all its child nodes.
790                         assert(aabb.Contains(child->center));
791                         assert(aabb.Contains(child->ComputeAABB()));
792
793                         DebugSanityCheckNode(child);
794                 }
795         }
796 #else
797         MARK_UNUSED(n);
798 #endif
799 }
800
801 MATH_END_NAMESPACE