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