1/*
2* Copyright (c) 2009 Erin Catto http://www.box2d.org
3*
4* This software is provided 'as-is', without any express or implied
5* warranty. In no event will the authors be held liable for any damages
6* arising from the use of this software.
7* Permission is granted to anyone to use this software for any purpose,
8* including commercial applications, and to alter it and redistribute it
9* freely, subject to the following restrictions:
10* 1. The origin of this software must not be misrepresented; you must not
11* claim that you wrote the original software. If you use this software
12* in a product, an acknowledgment in the product documentation would be
13* appreciated but is not required.
14* 2. Altered source versions must be plainly marked as such, and must not be
15* misrepresented as being the original software.
16* 3. This notice may not be removed or altered from any source distribution.
17*/
18
19#include <Box2D/Collision/b2DynamicTree.h>
20#include <string.h>
21
22b2DynamicTree::b2DynamicTree()
23{
24 m_root = b2_nullNode;
25
26 m_nodeCapacity = 16;
27 m_nodeCount = 0;
28 m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode));
29 memset(m_nodes, 0, m_nodeCapacity * sizeof(b2TreeNode));
30
31 // Build a linked list for the free list.
32 for (int32 i = 0; i < m_nodeCapacity - 1; ++i)
33 {
34 m_nodes[i].next = i + 1;
35 m_nodes[i].height = -1;
36 }
37 m_nodes[m_nodeCapacity-1].next = b2_nullNode;
38 m_nodes[m_nodeCapacity-1].height = -1;
39 m_freeList = 0;
40
41 m_path = 0;
42
43 m_insertionCount = 0;
44}
45
46b2DynamicTree::~b2DynamicTree()
47{
48 // This frees the entire tree in one shot.
49 b2Free(m_nodes);
50}
51
52// Allocate a node from the pool. Grow the pool if necessary.
53int32 b2DynamicTree::AllocateNode()
54{
55 // Expand the node pool as needed.
56 if (m_freeList == b2_nullNode)
57 {
58 b2Assert(m_nodeCount == m_nodeCapacity);
59
60 // The free list is empty. Rebuild a bigger pool.
61 b2TreeNode* oldNodes = m_nodes;
62 m_nodeCapacity *= 2;
63 m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode));
64 memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b2TreeNode));
65 b2Free(oldNodes);
66
67 // Build a linked list for the free list. The parent
68 // pointer becomes the "next" pointer.
69 for (int32 i = m_nodeCount; i < m_nodeCapacity - 1; ++i)
70 {
71 m_nodes[i].next = i + 1;
72 m_nodes[i].height = -1;
73 }
74 m_nodes[m_nodeCapacity-1].next = b2_nullNode;
75 m_nodes[m_nodeCapacity-1].height = -1;
76 m_freeList = m_nodeCount;
77 }
78
79 // Peel a node off the free list.
80 int32 nodeId = m_freeList;
81 m_freeList = m_nodes[nodeId].next;
82 m_nodes[nodeId].parent = b2_nullNode;
83 m_nodes[nodeId].child1 = b2_nullNode;
84 m_nodes[nodeId].child2 = b2_nullNode;
85 m_nodes[nodeId].height = 0;
86 m_nodes[nodeId].userData = NULL;
87 ++m_nodeCount;
88 return nodeId;
89}
90
91// Return a node to the pool.
92void b2DynamicTree::FreeNode(int32 nodeId)
93{
94 b2Assert(0 <= nodeId && nodeId < m_nodeCapacity);
95 b2Assert(0 < m_nodeCount);
96 m_nodes[nodeId].next = m_freeList;
97 m_nodes[nodeId].height = -1;
98 m_freeList = nodeId;
99 --m_nodeCount;
100}
101
102// Create a proxy in the tree as a leaf node. We return the index
103// of the node instead of a pointer so that we can grow
104// the node pool.
105int32 b2DynamicTree::CreateProxy(const b2AABB& aabb, void* userData)
106{
107 int32 proxyId = AllocateNode();
108
109 // Fatten the aabb.
110 b2Vec2 r(b2_aabbExtension, b2_aabbExtension);
111 m_nodes[proxyId].aabb.lowerBound = aabb.lowerBound - r;
112 m_nodes[proxyId].aabb.upperBound = aabb.upperBound + r;
113 m_nodes[proxyId].userData = userData;
114 m_nodes[proxyId].height = 0;
115
116 InsertLeaf(proxyId);
117
118 return proxyId;
119}
120
121void b2DynamicTree::DestroyProxy(int32 proxyId)
122{
123 b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
124 b2Assert(m_nodes[proxyId].IsLeaf());
125
126 RemoveLeaf(proxyId);
127 FreeNode(proxyId);
128}
129
130bool b2DynamicTree::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement)
131{
132 b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
133
134 b2Assert(m_nodes[proxyId].IsLeaf());
135
136 if (m_nodes[proxyId].aabb.Contains(aabb))
137 {
138 return false;
139 }
140
141 RemoveLeaf(proxyId);
142
143 // Extend AABB.
144 b2AABB b = aabb;
145 b2Vec2 r(b2_aabbExtension, b2_aabbExtension);
146 b.lowerBound = b.lowerBound - r;
147 b.upperBound = b.upperBound + r;
148
149 // Predict AABB displacement.
150 b2Vec2 d = b2_aabbMultiplier * displacement;
151
152 if (d.x < 0.0f)
153 {
154 b.lowerBound.x += d.x;
155 }
156 else
157 {
158 b.upperBound.x += d.x;
159 }
160
161 if (d.y < 0.0f)
162 {
163 b.lowerBound.y += d.y;
164 }
165 else
166 {
167 b.upperBound.y += d.y;
168 }
169
170 m_nodes[proxyId].aabb = b;
171
172 InsertLeaf(proxyId);
173 return true;
174}
175
176void b2DynamicTree::InsertLeaf(int32 leaf)
177{
178 ++m_insertionCount;
179
180 if (m_root == b2_nullNode)
181 {
182 m_root = leaf;
183 m_nodes[m_root].parent = b2_nullNode;
184 return;
185 }
186
187 // Find the best sibling for this node
188 b2AABB leafAABB = m_nodes[leaf].aabb;
189 int32 index = m_root;
190 while (m_nodes[index].IsLeaf() == false)
191 {
192 int32 child1 = m_nodes[index].child1;
193 int32 child2 = m_nodes[index].child2;
194
195 float32 area = m_nodes[index].aabb.GetPerimeter();
196
197 b2AABB combinedAABB;
198 combinedAABB.Combine(m_nodes[index].aabb, leafAABB);
199 float32 combinedArea = combinedAABB.GetPerimeter();
200
201 // Cost of creating a new parent for this node and the new leaf
202 float32 cost = 2.0f * combinedArea;
203
204 // Minimum cost of pushing the leaf further down the tree
205 float32 inheritanceCost = 2.0f * (combinedArea - area);
206
207 // Cost of descending into child1
208 float32 cost1;
209 if (m_nodes[child1].IsLeaf())
210 {
211 b2AABB aabb;
212 aabb.Combine(leafAABB, m_nodes[child1].aabb);
213 cost1 = aabb.GetPerimeter() + inheritanceCost;
214 }
215 else
216 {
217 b2AABB aabb;
218 aabb.Combine(leafAABB, m_nodes[child1].aabb);
219 float32 oldArea = m_nodes[child1].aabb.GetPerimeter();
220 float32 newArea = aabb.GetPerimeter();
221 cost1 = (newArea - oldArea) + inheritanceCost;
222 }
223
224 // Cost of descending into child2
225 float32 cost2;
226 if (m_nodes[child2].IsLeaf())
227 {
228 b2AABB aabb;
229 aabb.Combine(leafAABB, m_nodes[child2].aabb);
230 cost2 = aabb.GetPerimeter() + inheritanceCost;
231 }
232 else
233 {
234 b2AABB aabb;
235 aabb.Combine(leafAABB, m_nodes[child2].aabb);
236 float32 oldArea = m_nodes[child2].aabb.GetPerimeter();
237 float32 newArea = aabb.GetPerimeter();
238 cost2 = newArea - oldArea + inheritanceCost;
239 }
240
241 // Descend according to the minimum cost.
242 if (cost < cost1 && cost < cost2)
243 {
244 break;
245 }
246
247 // Descend
248 if (cost1 < cost2)
249 {
250 index = child1;
251 }
252 else
253 {
254 index = child2;
255 }
256 }
257
258 int32 sibling = index;
259
260 // Create a new parent.
261 int32 oldParent = m_nodes[sibling].parent;
262 int32 newParent = AllocateNode();
263 m_nodes[newParent].parent = oldParent;
264 m_nodes[newParent].userData = NULL;
265 m_nodes[newParent].aabb.Combine(leafAABB, m_nodes[sibling].aabb);
266 m_nodes[newParent].height = m_nodes[sibling].height + 1;
267
268 if (oldParent != b2_nullNode)
269 {
270 // The sibling was not the root.
271 if (m_nodes[oldParent].child1 == sibling)
272 {
273 m_nodes[oldParent].child1 = newParent;
274 }
275 else
276 {
277 m_nodes[oldParent].child2 = newParent;
278 }
279
280 m_nodes[newParent].child1 = sibling;
281 m_nodes[newParent].child2 = leaf;
282 m_nodes[sibling].parent = newParent;
283 m_nodes[leaf].parent = newParent;
284 }
285 else
286 {
287 // The sibling was the root.
288 m_nodes[newParent].child1 = sibling;
289 m_nodes[newParent].child2 = leaf;
290 m_nodes[sibling].parent = newParent;
291 m_nodes[leaf].parent = newParent;
292 m_root = newParent;
293 }
294
295 // Walk back up the tree fixing heights and AABBs
296 index = m_nodes[leaf].parent;
297 while (index != b2_nullNode)
298 {
299 index = Balance(index);
300
301 int32 child1 = m_nodes[index].child1;
302 int32 child2 = m_nodes[index].child2;
303
304 b2Assert(child1 != b2_nullNode);
305 b2Assert(child2 != b2_nullNode);
306
307 m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height);
308 m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);
309
310 index = m_nodes[index].parent;
311 }
312
313 //Validate();
314}
315
316void b2DynamicTree::RemoveLeaf(int32 leaf)
317{
318 if (leaf == m_root)
319 {
320 m_root = b2_nullNode;
321 return;
322 }
323
324 int32 parent = m_nodes[leaf].parent;
325 int32 grandParent = m_nodes[parent].parent;
326 int32 sibling;
327 if (m_nodes[parent].child1 == leaf)
328 {
329 sibling = m_nodes[parent].child2;
330 }
331 else
332 {
333 sibling = m_nodes[parent].child1;
334 }
335
336 if (grandParent != b2_nullNode)
337 {
338 // Destroy parent and connect sibling to grandParent.
339 if (m_nodes[grandParent].child1 == parent)
340 {
341 m_nodes[grandParent].child1 = sibling;
342 }
343 else
344 {
345 m_nodes[grandParent].child2 = sibling;
346 }
347 m_nodes[sibling].parent = grandParent;
348 FreeNode(parent);
349
350 // Adjust ancestor bounds.
351 int32 index = grandParent;
352 while (index != b2_nullNode)
353 {
354 index = Balance(index);
355
356 int32 child1 = m_nodes[index].child1;
357 int32 child2 = m_nodes[index].child2;
358
359 m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);
360 m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height);
361
362 index = m_nodes[index].parent;
363 }
364 }
365 else
366 {
367 m_root = sibling;
368 m_nodes[sibling].parent = b2_nullNode;
369 FreeNode(parent);
370 }
371
372 //Validate();
373}
374
375// Perform a left or right rotation if node A is imbalanced.
376// Returns the new root index.
377int32 b2DynamicTree::Balance(int32 iA)
378{
379 b2Assert(iA != b2_nullNode);
380
381 b2TreeNode* A = m_nodes + iA;
382 if (A->IsLeaf() || A->height < 2)
383 {
384 return iA;
385 }
386
387 int32 iB = A->child1;
388 int32 iC = A->child2;
389 b2Assert(0 <= iB && iB < m_nodeCapacity);
390 b2Assert(0 <= iC && iC < m_nodeCapacity);
391
392 b2TreeNode* B = m_nodes + iB;
393 b2TreeNode* C = m_nodes + iC;
394
395 int32 balance = C->height - B->height;
396
397 // Rotate C up
398 if (balance > 1)
399 {
400 int32 iF = C->child1;
401 int32 iG = C->child2;
402 b2TreeNode* F = m_nodes + iF;
403 b2TreeNode* G = m_nodes + iG;
404 b2Assert(0 <= iF && iF < m_nodeCapacity);
405 b2Assert(0 <= iG && iG < m_nodeCapacity);
406
407 // Swap A and C
408 C->child1 = iA;
409 C->parent = A->parent;
410 A->parent = iC;
411
412 // A's old parent should point to C
413 if (C->parent != b2_nullNode)
414 {
415 if (m_nodes[C->parent].child1 == iA)
416 {
417 m_nodes[C->parent].child1 = iC;
418 }
419 else
420 {
421 b2Assert(m_nodes[C->parent].child2 == iA);
422 m_nodes[C->parent].child2 = iC;
423 }
424 }
425 else
426 {
427 m_root = iC;
428 }
429
430 // Rotate
431 if (F->height > G->height)
432 {
433 C->child2 = iF;
434 A->child2 = iG;
435 G->parent = iA;
436 A->aabb.Combine(B->aabb, G->aabb);
437 C->aabb.Combine(A->aabb, F->aabb);
438
439 A->height = 1 + b2Max(B->height, G->height);
440 C->height = 1 + b2Max(A->height, F->height);
441 }
442 else
443 {
444 C->child2 = iG;
445 A->child2 = iF;
446 F->parent = iA;
447 A->aabb.Combine(B->aabb, F->aabb);
448 C->aabb.Combine(A->aabb, G->aabb);
449
450 A->height = 1 + b2Max(B->height, F->height);
451 C->height = 1 + b2Max(A->height, G->height);
452 }
453
454 return iC;
455 }
456
457 // Rotate B up
458 if (balance < -1)
459 {
460 int32 iD = B->child1;
461 int32 iE = B->child2;
462 b2TreeNode* D = m_nodes + iD;
463 b2TreeNode* E = m_nodes + iE;
464 b2Assert(0 <= iD && iD < m_nodeCapacity);
465 b2Assert(0 <= iE && iE < m_nodeCapacity);
466
467 // Swap A and B
468 B->child1 = iA;
469 B->parent = A->parent;
470 A->parent = iB;
471
472 // A's old parent should point to B
473 if (B->parent != b2_nullNode)
474 {
475 if (m_nodes[B->parent].child1 == iA)
476 {
477 m_nodes[B->parent].child1 = iB;
478 }
479 else
480 {
481 b2Assert(m_nodes[B->parent].child2 == iA);
482 m_nodes[B->parent].child2 = iB;
483 }
484 }
485 else
486 {
487 m_root = iB;
488 }
489
490 // Rotate
491 if (D->height > E->height)
492 {
493 B->child2 = iD;
494 A->child1 = iE;
495 E->parent = iA;
496 A->aabb.Combine(C->aabb, E->aabb);
497 B->aabb.Combine(A->aabb, D->aabb);
498
499 A->height = 1 + b2Max(C->height, E->height);
500 B->height = 1 + b2Max(A->height, D->height);
501 }
502 else
503 {
504 B->child2 = iE;
505 A->child1 = iD;
506 D->parent = iA;
507 A->aabb.Combine(C->aabb, D->aabb);
508 B->aabb.Combine(A->aabb, E->aabb);
509
510 A->height = 1 + b2Max(C->height, D->height);
511 B->height = 1 + b2Max(A->height, E->height);
512 }
513
514 return iB;
515 }
516
517 return iA;
518}
519
520int32 b2DynamicTree::GetHeight() const
521{
522 if (m_root == b2_nullNode)
523 {
524 return 0;
525 }
526
527 return m_nodes[m_root].height;
528}
529
530//
531float32 b2DynamicTree::GetAreaRatio() const
532{
533 if (m_root == b2_nullNode)
534 {
535 return 0.0f;
536 }
537
538 const b2TreeNode* root = m_nodes + m_root;
539 float32 rootArea = root->aabb.GetPerimeter();
540
541 float32 totalArea = 0.0f;
542 for (int32 i = 0; i < m_nodeCapacity; ++i)
543 {
544 const b2TreeNode* node = m_nodes + i;
545 if (node->height < 0)
546 {
547 // Free node in pool
548 continue;
549 }
550
551 totalArea += node->aabb.GetPerimeter();
552 }
553
554 return totalArea / rootArea;
555}
556
557// Compute the height of a sub-tree.
558int32 b2DynamicTree::ComputeHeight(int32 nodeId) const
559{
560 b2Assert(0 <= nodeId && nodeId < m_nodeCapacity);
561 b2TreeNode* node = m_nodes + nodeId;
562
563 if (node->IsLeaf())
564 {
565 return 0;
566 }
567
568 int32 height1 = ComputeHeight(node->child1);
569 int32 height2 = ComputeHeight(node->child2);
570 return 1 + b2Max(height1, height2);
571}
572
573int32 b2DynamicTree::ComputeHeight() const
574{
575 int32 height = ComputeHeight(m_root);
576 return height;
577}
578
579void b2DynamicTree::ValidateStructure(int32 index) const
580{
581 if (index == b2_nullNode)
582 {
583 return;
584 }
585
586 if (index == m_root)
587 {
588 b2Assert(m_nodes[index].parent == b2_nullNode);
589 }
590
591 const b2TreeNode* node = m_nodes + index;
592
593 int32 child1 = node->child1;
594 int32 child2 = node->child2;
595
596 if (node->IsLeaf())
597 {
598 b2Assert(child1 == b2_nullNode);
599 b2Assert(child2 == b2_nullNode);
600 b2Assert(node->height == 0);
601 return;
602 }
603
604 b2Assert(0 <= child1 && child1 < m_nodeCapacity);
605 b2Assert(0 <= child2 && child2 < m_nodeCapacity);
606
607 b2Assert(m_nodes[child1].parent == index);
608 b2Assert(m_nodes[child2].parent == index);
609
610 ValidateStructure(child1);
611 ValidateStructure(child2);
612}
613
614void b2DynamicTree::ValidateMetrics(int32 index) const
615{
616 if (index == b2_nullNode)
617 {
618 return;
619 }
620
621 const b2TreeNode* node = m_nodes + index;
622
623 int32 child1 = node->child1;
624 int32 child2 = node->child2;
625
626 if (node->IsLeaf())
627 {
628 b2Assert(child1 == b2_nullNode);
629 b2Assert(child2 == b2_nullNode);
630 b2Assert(node->height == 0);
631 return;
632 }
633
634 b2Assert(0 <= child1 && child1 < m_nodeCapacity);
635 b2Assert(0 <= child2 && child2 < m_nodeCapacity);
636
637 int32 height1 = m_nodes[child1].height;
638 int32 height2 = m_nodes[child2].height;
639 int32 height;
640 height = 1 + b2Max(height1, height2);
641 b2Assert(node->height == height);
642
643 b2AABB aabb;
644 aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);
645
646 b2Assert(aabb.lowerBound == node->aabb.lowerBound);
647 b2Assert(aabb.upperBound == node->aabb.upperBound);
648
649 ValidateMetrics(child1);
650 ValidateMetrics(child2);
651}
652
653void b2DynamicTree::Validate() const
654{
655 ValidateStructure(m_root);
656 ValidateMetrics(m_root);
657
658 int32 freeCount = 0;
659 int32 freeIndex = m_freeList;
660 while (freeIndex != b2_nullNode)
661 {
662 b2Assert(0 <= freeIndex && freeIndex < m_nodeCapacity);
663 freeIndex = m_nodes[freeIndex].next;
664 ++freeCount;
665 }
666
667 b2Assert(GetHeight() == ComputeHeight());
668
669 b2Assert(m_nodeCount + freeCount == m_nodeCapacity);
670}
671
672int32 b2DynamicTree::GetMaxBalance() const
673{
674 int32 maxBalance = 0;
675 for (int32 i = 0; i < m_nodeCapacity; ++i)
676 {
677 const b2TreeNode* node = m_nodes + i;
678 if (node->height <= 1)
679 {
680 continue;
681 }
682
683 b2Assert(node->IsLeaf() == false);
684
685 int32 child1 = node->child1;
686 int32 child2 = node->child2;
687 int32 balance = b2Abs(m_nodes[child2].height - m_nodes[child1].height);
688 maxBalance = b2Max(maxBalance, balance);
689 }
690
691 return maxBalance;
692}
693
694void b2DynamicTree::RebuildBottomUp()
695{
696 int32* nodes = (int32*)b2Alloc(m_nodeCount * sizeof(int32));
697 int32 count = 0;
698
699 // Build array of leaves. Free the rest.
700 for (int32 i = 0; i < m_nodeCapacity; ++i)
701 {
702 if (m_nodes[i].height < 0)
703 {
704 // free node in pool
705 continue;
706 }
707
708 if (m_nodes[i].IsLeaf())
709 {
710 m_nodes[i].parent = b2_nullNode;
711 nodes[count] = i;
712 ++count;
713 }
714 else
715 {
716 FreeNode(i);
717 }
718 }
719
720 while (count > 1)
721 {
722 float32 minCost = b2_maxFloat;
723 int32 iMin = -1, jMin = -1;
724 for (int32 i = 0; i < count; ++i)
725 {
726 b2AABB aabbi = m_nodes[nodes[i]].aabb;
727
728 for (int32 j = i + 1; j < count; ++j)
729 {
730 b2AABB aabbj = m_nodes[nodes[j]].aabb;
731 b2AABB b;
732 b.Combine(aabbi, aabbj);
733 float32 cost = b.GetPerimeter();
734 if (cost < minCost)
735 {
736 iMin = i;
737 jMin = j;
738 minCost = cost;
739 }
740 }
741 }
742
743 int32 index1 = nodes[iMin];
744 int32 index2 = nodes[jMin];
745 b2TreeNode* child1 = m_nodes + index1;
746 b2TreeNode* child2 = m_nodes + index2;
747
748 int32 parentIndex = AllocateNode();
749 b2TreeNode* parent = m_nodes + parentIndex;
750 parent->child1 = index1;
751 parent->child2 = index2;
752 parent->height = 1 + b2Max(child1->height, child2->height);
753 parent->aabb.Combine(child1->aabb, child2->aabb);
754 parent->parent = b2_nullNode;
755
756 child1->parent = parentIndex;
757 child2->parent = parentIndex;
758
759 nodes[jMin] = nodes[count-1];
760 nodes[iMin] = parentIndex;
761 --count;
762 }
763
764 m_root = nodes[0];
765 b2Free(nodes);
766
767 Validate();
768}
769
770void b2DynamicTree::ShiftOrigin(const b2Vec2& newOrigin)
771{
772 // Build array of leaves. Free the rest.
773 for (int32 i = 0; i < m_nodeCapacity; ++i)
774 {
775 m_nodes[i].aabb.lowerBound -= newOrigin;
776 m_nodes[i].aabb.upperBound -= newOrigin;
777 }
778}
779