Warning, file /education/gcompris/external/qml-box2d/Box2D/Collision/b2DynamicTree.cpp was not indexed or was modified since last indexation (in which case cross-reference links may be missing, inaccurate or erroneous).
0001 /* 0002 * Copyright (c) 2009 Erin Catto http://www.box2d.org 0003 * 0004 * This software is provided 'as-is', without any express or implied 0005 * warranty. In no event will the authors be held liable for any damages 0006 * arising from the use of this software. 0007 * Permission is granted to anyone to use this software for any purpose, 0008 * including commercial applications, and to alter it and redistribute it 0009 * freely, subject to the following restrictions: 0010 * 1. The origin of this software must not be misrepresented; you must not 0011 * claim that you wrote the original software. If you use this software 0012 * in a product, an acknowledgment in the product documentation would be 0013 * appreciated but is not required. 0014 * 2. Altered source versions must be plainly marked as such, and must not be 0015 * misrepresented as being the original software. 0016 * 3. This notice may not be removed or altered from any source distribution. 0017 */ 0018 0019 #include <Box2D/Collision/b2DynamicTree.h> 0020 #include <string.h> 0021 0022 b2DynamicTree::b2DynamicTree() 0023 { 0024 m_root = b2_nullNode; 0025 0026 m_nodeCapacity = 16; 0027 m_nodeCount = 0; 0028 m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode)); 0029 memset(m_nodes, 0, m_nodeCapacity * sizeof(b2TreeNode)); 0030 0031 // Build a linked list for the free list. 0032 for (int32 i = 0; i < m_nodeCapacity - 1; ++i) 0033 { 0034 m_nodes[i].next = i + 1; 0035 m_nodes[i].height = -1; 0036 } 0037 m_nodes[m_nodeCapacity-1].next = b2_nullNode; 0038 m_nodes[m_nodeCapacity-1].height = -1; 0039 m_freeList = 0; 0040 0041 m_path = 0; 0042 0043 m_insertionCount = 0; 0044 } 0045 0046 b2DynamicTree::~b2DynamicTree() 0047 { 0048 // This frees the entire tree in one shot. 0049 b2Free(m_nodes); 0050 } 0051 0052 // Allocate a node from the pool. Grow the pool if necessary. 0053 int32 b2DynamicTree::AllocateNode() 0054 { 0055 // Expand the node pool as needed. 0056 if (m_freeList == b2_nullNode) 0057 { 0058 b2Assert(m_nodeCount == m_nodeCapacity); 0059 0060 // The free list is empty. Rebuild a bigger pool. 0061 b2TreeNode* oldNodes = m_nodes; 0062 m_nodeCapacity *= 2; 0063 m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode)); 0064 memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b2TreeNode)); 0065 b2Free(oldNodes); 0066 0067 // Build a linked list for the free list. The parent 0068 // pointer becomes the "next" pointer. 0069 for (int32 i = m_nodeCount; i < m_nodeCapacity - 1; ++i) 0070 { 0071 m_nodes[i].next = i + 1; 0072 m_nodes[i].height = -1; 0073 } 0074 m_nodes[m_nodeCapacity-1].next = b2_nullNode; 0075 m_nodes[m_nodeCapacity-1].height = -1; 0076 m_freeList = m_nodeCount; 0077 } 0078 0079 // Peel a node off the free list. 0080 int32 nodeId = m_freeList; 0081 m_freeList = m_nodes[nodeId].next; 0082 m_nodes[nodeId].parent = b2_nullNode; 0083 m_nodes[nodeId].child1 = b2_nullNode; 0084 m_nodes[nodeId].child2 = b2_nullNode; 0085 m_nodes[nodeId].height = 0; 0086 m_nodes[nodeId].userData = NULL; 0087 ++m_nodeCount; 0088 return nodeId; 0089 } 0090 0091 // Return a node to the pool. 0092 void b2DynamicTree::FreeNode(int32 nodeId) 0093 { 0094 b2Assert(0 <= nodeId && nodeId < m_nodeCapacity); 0095 b2Assert(0 < m_nodeCount); 0096 m_nodes[nodeId].next = m_freeList; 0097 m_nodes[nodeId].height = -1; 0098 m_freeList = nodeId; 0099 --m_nodeCount; 0100 } 0101 0102 // Create a proxy in the tree as a leaf node. We return the index 0103 // of the node instead of a pointer so that we can grow 0104 // the node pool. 0105 int32 b2DynamicTree::CreateProxy(const b2AABB& aabb, void* userData) 0106 { 0107 int32 proxyId = AllocateNode(); 0108 0109 // Fatten the aabb. 0110 b2Vec2 r(b2_aabbExtension, b2_aabbExtension); 0111 m_nodes[proxyId].aabb.lowerBound = aabb.lowerBound - r; 0112 m_nodes[proxyId].aabb.upperBound = aabb.upperBound + r; 0113 m_nodes[proxyId].userData = userData; 0114 m_nodes[proxyId].height = 0; 0115 0116 InsertLeaf(proxyId); 0117 0118 return proxyId; 0119 } 0120 0121 void b2DynamicTree::DestroyProxy(int32 proxyId) 0122 { 0123 b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); 0124 b2Assert(m_nodes[proxyId].IsLeaf()); 0125 0126 RemoveLeaf(proxyId); 0127 FreeNode(proxyId); 0128 } 0129 0130 bool b2DynamicTree::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement) 0131 { 0132 b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); 0133 0134 b2Assert(m_nodes[proxyId].IsLeaf()); 0135 0136 if (m_nodes[proxyId].aabb.Contains(aabb)) 0137 { 0138 return false; 0139 } 0140 0141 RemoveLeaf(proxyId); 0142 0143 // Extend AABB. 0144 b2AABB b = aabb; 0145 b2Vec2 r(b2_aabbExtension, b2_aabbExtension); 0146 b.lowerBound = b.lowerBound - r; 0147 b.upperBound = b.upperBound + r; 0148 0149 // Predict AABB displacement. 0150 b2Vec2 d = b2_aabbMultiplier * displacement; 0151 0152 if (d.x < 0.0f) 0153 { 0154 b.lowerBound.x += d.x; 0155 } 0156 else 0157 { 0158 b.upperBound.x += d.x; 0159 } 0160 0161 if (d.y < 0.0f) 0162 { 0163 b.lowerBound.y += d.y; 0164 } 0165 else 0166 { 0167 b.upperBound.y += d.y; 0168 } 0169 0170 m_nodes[proxyId].aabb = b; 0171 0172 InsertLeaf(proxyId); 0173 return true; 0174 } 0175 0176 void b2DynamicTree::InsertLeaf(int32 leaf) 0177 { 0178 ++m_insertionCount; 0179 0180 if (m_root == b2_nullNode) 0181 { 0182 m_root = leaf; 0183 m_nodes[m_root].parent = b2_nullNode; 0184 return; 0185 } 0186 0187 // Find the best sibling for this node 0188 b2AABB leafAABB = m_nodes[leaf].aabb; 0189 int32 index = m_root; 0190 while (m_nodes[index].IsLeaf() == false) 0191 { 0192 int32 child1 = m_nodes[index].child1; 0193 int32 child2 = m_nodes[index].child2; 0194 0195 float32 area = m_nodes[index].aabb.GetPerimeter(); 0196 0197 b2AABB combinedAABB; 0198 combinedAABB.Combine(m_nodes[index].aabb, leafAABB); 0199 float32 combinedArea = combinedAABB.GetPerimeter(); 0200 0201 // Cost of creating a new parent for this node and the new leaf 0202 float32 cost = 2.0f * combinedArea; 0203 0204 // Minimum cost of pushing the leaf further down the tree 0205 float32 inheritanceCost = 2.0f * (combinedArea - area); 0206 0207 // Cost of descending into child1 0208 float32 cost1; 0209 if (m_nodes[child1].IsLeaf()) 0210 { 0211 b2AABB aabb; 0212 aabb.Combine(leafAABB, m_nodes[child1].aabb); 0213 cost1 = aabb.GetPerimeter() + inheritanceCost; 0214 } 0215 else 0216 { 0217 b2AABB aabb; 0218 aabb.Combine(leafAABB, m_nodes[child1].aabb); 0219 float32 oldArea = m_nodes[child1].aabb.GetPerimeter(); 0220 float32 newArea = aabb.GetPerimeter(); 0221 cost1 = (newArea - oldArea) + inheritanceCost; 0222 } 0223 0224 // Cost of descending into child2 0225 float32 cost2; 0226 if (m_nodes[child2].IsLeaf()) 0227 { 0228 b2AABB aabb; 0229 aabb.Combine(leafAABB, m_nodes[child2].aabb); 0230 cost2 = aabb.GetPerimeter() + inheritanceCost; 0231 } 0232 else 0233 { 0234 b2AABB aabb; 0235 aabb.Combine(leafAABB, m_nodes[child2].aabb); 0236 float32 oldArea = m_nodes[child2].aabb.GetPerimeter(); 0237 float32 newArea = aabb.GetPerimeter(); 0238 cost2 = newArea - oldArea + inheritanceCost; 0239 } 0240 0241 // Descend according to the minimum cost. 0242 if (cost < cost1 && cost < cost2) 0243 { 0244 break; 0245 } 0246 0247 // Descend 0248 if (cost1 < cost2) 0249 { 0250 index = child1; 0251 } 0252 else 0253 { 0254 index = child2; 0255 } 0256 } 0257 0258 int32 sibling = index; 0259 0260 // Create a new parent. 0261 int32 oldParent = m_nodes[sibling].parent; 0262 int32 newParent = AllocateNode(); 0263 m_nodes[newParent].parent = oldParent; 0264 m_nodes[newParent].userData = NULL; 0265 m_nodes[newParent].aabb.Combine(leafAABB, m_nodes[sibling].aabb); 0266 m_nodes[newParent].height = m_nodes[sibling].height + 1; 0267 0268 if (oldParent != b2_nullNode) 0269 { 0270 // The sibling was not the root. 0271 if (m_nodes[oldParent].child1 == sibling) 0272 { 0273 m_nodes[oldParent].child1 = newParent; 0274 } 0275 else 0276 { 0277 m_nodes[oldParent].child2 = newParent; 0278 } 0279 0280 m_nodes[newParent].child1 = sibling; 0281 m_nodes[newParent].child2 = leaf; 0282 m_nodes[sibling].parent = newParent; 0283 m_nodes[leaf].parent = newParent; 0284 } 0285 else 0286 { 0287 // The sibling was the root. 0288 m_nodes[newParent].child1 = sibling; 0289 m_nodes[newParent].child2 = leaf; 0290 m_nodes[sibling].parent = newParent; 0291 m_nodes[leaf].parent = newParent; 0292 m_root = newParent; 0293 } 0294 0295 // Walk back up the tree fixing heights and AABBs 0296 index = m_nodes[leaf].parent; 0297 while (index != b2_nullNode) 0298 { 0299 index = Balance(index); 0300 0301 int32 child1 = m_nodes[index].child1; 0302 int32 child2 = m_nodes[index].child2; 0303 0304 b2Assert(child1 != b2_nullNode); 0305 b2Assert(child2 != b2_nullNode); 0306 0307 m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height); 0308 m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); 0309 0310 index = m_nodes[index].parent; 0311 } 0312 0313 //Validate(); 0314 } 0315 0316 void b2DynamicTree::RemoveLeaf(int32 leaf) 0317 { 0318 if (leaf == m_root) 0319 { 0320 m_root = b2_nullNode; 0321 return; 0322 } 0323 0324 int32 parent = m_nodes[leaf].parent; 0325 int32 grandParent = m_nodes[parent].parent; 0326 int32 sibling; 0327 if (m_nodes[parent].child1 == leaf) 0328 { 0329 sibling = m_nodes[parent].child2; 0330 } 0331 else 0332 { 0333 sibling = m_nodes[parent].child1; 0334 } 0335 0336 if (grandParent != b2_nullNode) 0337 { 0338 // Destroy parent and connect sibling to grandParent. 0339 if (m_nodes[grandParent].child1 == parent) 0340 { 0341 m_nodes[grandParent].child1 = sibling; 0342 } 0343 else 0344 { 0345 m_nodes[grandParent].child2 = sibling; 0346 } 0347 m_nodes[sibling].parent = grandParent; 0348 FreeNode(parent); 0349 0350 // Adjust ancestor bounds. 0351 int32 index = grandParent; 0352 while (index != b2_nullNode) 0353 { 0354 index = Balance(index); 0355 0356 int32 child1 = m_nodes[index].child1; 0357 int32 child2 = m_nodes[index].child2; 0358 0359 m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); 0360 m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height); 0361 0362 index = m_nodes[index].parent; 0363 } 0364 } 0365 else 0366 { 0367 m_root = sibling; 0368 m_nodes[sibling].parent = b2_nullNode; 0369 FreeNode(parent); 0370 } 0371 0372 //Validate(); 0373 } 0374 0375 // Perform a left or right rotation if node A is imbalanced. 0376 // Returns the new root index. 0377 int32 b2DynamicTree::Balance(int32 iA) 0378 { 0379 b2Assert(iA != b2_nullNode); 0380 0381 b2TreeNode* A = m_nodes + iA; 0382 if (A->IsLeaf() || A->height < 2) 0383 { 0384 return iA; 0385 } 0386 0387 int32 iB = A->child1; 0388 int32 iC = A->child2; 0389 b2Assert(0 <= iB && iB < m_nodeCapacity); 0390 b2Assert(0 <= iC && iC < m_nodeCapacity); 0391 0392 b2TreeNode* B = m_nodes + iB; 0393 b2TreeNode* C = m_nodes + iC; 0394 0395 int32 balance = C->height - B->height; 0396 0397 // Rotate C up 0398 if (balance > 1) 0399 { 0400 int32 iF = C->child1; 0401 int32 iG = C->child2; 0402 b2TreeNode* F = m_nodes + iF; 0403 b2TreeNode* G = m_nodes + iG; 0404 b2Assert(0 <= iF && iF < m_nodeCapacity); 0405 b2Assert(0 <= iG && iG < m_nodeCapacity); 0406 0407 // Swap A and C 0408 C->child1 = iA; 0409 C->parent = A->parent; 0410 A->parent = iC; 0411 0412 // A's old parent should point to C 0413 if (C->parent != b2_nullNode) 0414 { 0415 if (m_nodes[C->parent].child1 == iA) 0416 { 0417 m_nodes[C->parent].child1 = iC; 0418 } 0419 else 0420 { 0421 b2Assert(m_nodes[C->parent].child2 == iA); 0422 m_nodes[C->parent].child2 = iC; 0423 } 0424 } 0425 else 0426 { 0427 m_root = iC; 0428 } 0429 0430 // Rotate 0431 if (F->height > G->height) 0432 { 0433 C->child2 = iF; 0434 A->child2 = iG; 0435 G->parent = iA; 0436 A->aabb.Combine(B->aabb, G->aabb); 0437 C->aabb.Combine(A->aabb, F->aabb); 0438 0439 A->height = 1 + b2Max(B->height, G->height); 0440 C->height = 1 + b2Max(A->height, F->height); 0441 } 0442 else 0443 { 0444 C->child2 = iG; 0445 A->child2 = iF; 0446 F->parent = iA; 0447 A->aabb.Combine(B->aabb, F->aabb); 0448 C->aabb.Combine(A->aabb, G->aabb); 0449 0450 A->height = 1 + b2Max(B->height, F->height); 0451 C->height = 1 + b2Max(A->height, G->height); 0452 } 0453 0454 return iC; 0455 } 0456 0457 // Rotate B up 0458 if (balance < -1) 0459 { 0460 int32 iD = B->child1; 0461 int32 iE = B->child2; 0462 b2TreeNode* D = m_nodes + iD; 0463 b2TreeNode* E = m_nodes + iE; 0464 b2Assert(0 <= iD && iD < m_nodeCapacity); 0465 b2Assert(0 <= iE && iE < m_nodeCapacity); 0466 0467 // Swap A and B 0468 B->child1 = iA; 0469 B->parent = A->parent; 0470 A->parent = iB; 0471 0472 // A's old parent should point to B 0473 if (B->parent != b2_nullNode) 0474 { 0475 if (m_nodes[B->parent].child1 == iA) 0476 { 0477 m_nodes[B->parent].child1 = iB; 0478 } 0479 else 0480 { 0481 b2Assert(m_nodes[B->parent].child2 == iA); 0482 m_nodes[B->parent].child2 = iB; 0483 } 0484 } 0485 else 0486 { 0487 m_root = iB; 0488 } 0489 0490 // Rotate 0491 if (D->height > E->height) 0492 { 0493 B->child2 = iD; 0494 A->child1 = iE; 0495 E->parent = iA; 0496 A->aabb.Combine(C->aabb, E->aabb); 0497 B->aabb.Combine(A->aabb, D->aabb); 0498 0499 A->height = 1 + b2Max(C->height, E->height); 0500 B->height = 1 + b2Max(A->height, D->height); 0501 } 0502 else 0503 { 0504 B->child2 = iE; 0505 A->child1 = iD; 0506 D->parent = iA; 0507 A->aabb.Combine(C->aabb, D->aabb); 0508 B->aabb.Combine(A->aabb, E->aabb); 0509 0510 A->height = 1 + b2Max(C->height, D->height); 0511 B->height = 1 + b2Max(A->height, E->height); 0512 } 0513 0514 return iB; 0515 } 0516 0517 return iA; 0518 } 0519 0520 int32 b2DynamicTree::GetHeight() const 0521 { 0522 if (m_root == b2_nullNode) 0523 { 0524 return 0; 0525 } 0526 0527 return m_nodes[m_root].height; 0528 } 0529 0530 // 0531 float32 b2DynamicTree::GetAreaRatio() const 0532 { 0533 if (m_root == b2_nullNode) 0534 { 0535 return 0.0f; 0536 } 0537 0538 const b2TreeNode* root = m_nodes + m_root; 0539 float32 rootArea = root->aabb.GetPerimeter(); 0540 0541 float32 totalArea = 0.0f; 0542 for (int32 i = 0; i < m_nodeCapacity; ++i) 0543 { 0544 const b2TreeNode* node = m_nodes + i; 0545 if (node->height < 0) 0546 { 0547 // Free node in pool 0548 continue; 0549 } 0550 0551 totalArea += node->aabb.GetPerimeter(); 0552 } 0553 0554 return totalArea / rootArea; 0555 } 0556 0557 // Compute the height of a sub-tree. 0558 int32 b2DynamicTree::ComputeHeight(int32 nodeId) const 0559 { 0560 b2Assert(0 <= nodeId && nodeId < m_nodeCapacity); 0561 b2TreeNode* node = m_nodes + nodeId; 0562 0563 if (node->IsLeaf()) 0564 { 0565 return 0; 0566 } 0567 0568 int32 height1 = ComputeHeight(node->child1); 0569 int32 height2 = ComputeHeight(node->child2); 0570 return 1 + b2Max(height1, height2); 0571 } 0572 0573 int32 b2DynamicTree::ComputeHeight() const 0574 { 0575 int32 height = ComputeHeight(m_root); 0576 return height; 0577 } 0578 0579 void b2DynamicTree::ValidateStructure(int32 index) const 0580 { 0581 if (index == b2_nullNode) 0582 { 0583 return; 0584 } 0585 0586 if (index == m_root) 0587 { 0588 b2Assert(m_nodes[index].parent == b2_nullNode); 0589 } 0590 0591 const b2TreeNode* node = m_nodes + index; 0592 0593 int32 child1 = node->child1; 0594 int32 child2 = node->child2; 0595 0596 if (node->IsLeaf()) 0597 { 0598 b2Assert(child1 == b2_nullNode); 0599 b2Assert(child2 == b2_nullNode); 0600 b2Assert(node->height == 0); 0601 return; 0602 } 0603 0604 b2Assert(0 <= child1 && child1 < m_nodeCapacity); 0605 b2Assert(0 <= child2 && child2 < m_nodeCapacity); 0606 0607 b2Assert(m_nodes[child1].parent == index); 0608 b2Assert(m_nodes[child2].parent == index); 0609 0610 ValidateStructure(child1); 0611 ValidateStructure(child2); 0612 } 0613 0614 void b2DynamicTree::ValidateMetrics(int32 index) const 0615 { 0616 if (index == b2_nullNode) 0617 { 0618 return; 0619 } 0620 0621 const b2TreeNode* node = m_nodes + index; 0622 0623 int32 child1 = node->child1; 0624 int32 child2 = node->child2; 0625 0626 if (node->IsLeaf()) 0627 { 0628 b2Assert(child1 == b2_nullNode); 0629 b2Assert(child2 == b2_nullNode); 0630 b2Assert(node->height == 0); 0631 return; 0632 } 0633 0634 b2Assert(0 <= child1 && child1 < m_nodeCapacity); 0635 b2Assert(0 <= child2 && child2 < m_nodeCapacity); 0636 0637 int32 height1 = m_nodes[child1].height; 0638 int32 height2 = m_nodes[child2].height; 0639 int32 height; 0640 height = 1 + b2Max(height1, height2); 0641 b2Assert(node->height == height); 0642 0643 b2AABB aabb; 0644 aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); 0645 0646 b2Assert(aabb.lowerBound == node->aabb.lowerBound); 0647 b2Assert(aabb.upperBound == node->aabb.upperBound); 0648 0649 ValidateMetrics(child1); 0650 ValidateMetrics(child2); 0651 } 0652 0653 void b2DynamicTree::Validate() const 0654 { 0655 ValidateStructure(m_root); 0656 ValidateMetrics(m_root); 0657 0658 int32 freeCount = 0; 0659 int32 freeIndex = m_freeList; 0660 while (freeIndex != b2_nullNode) 0661 { 0662 b2Assert(0 <= freeIndex && freeIndex < m_nodeCapacity); 0663 freeIndex = m_nodes[freeIndex].next; 0664 ++freeCount; 0665 } 0666 0667 b2Assert(GetHeight() == ComputeHeight()); 0668 0669 b2Assert(m_nodeCount + freeCount == m_nodeCapacity); 0670 } 0671 0672 int32 b2DynamicTree::GetMaxBalance() const 0673 { 0674 int32 maxBalance = 0; 0675 for (int32 i = 0; i < m_nodeCapacity; ++i) 0676 { 0677 const b2TreeNode* node = m_nodes + i; 0678 if (node->height <= 1) 0679 { 0680 continue; 0681 } 0682 0683 b2Assert(node->IsLeaf() == false); 0684 0685 int32 child1 = node->child1; 0686 int32 child2 = node->child2; 0687 int32 balance = b2Abs(m_nodes[child2].height - m_nodes[child1].height); 0688 maxBalance = b2Max(maxBalance, balance); 0689 } 0690 0691 return maxBalance; 0692 } 0693 0694 void b2DynamicTree::RebuildBottomUp() 0695 { 0696 int32* nodes = (int32*)b2Alloc(m_nodeCount * sizeof(int32)); 0697 int32 count = 0; 0698 0699 // Build array of leaves. Free the rest. 0700 for (int32 i = 0; i < m_nodeCapacity; ++i) 0701 { 0702 if (m_nodes[i].height < 0) 0703 { 0704 // free node in pool 0705 continue; 0706 } 0707 0708 if (m_nodes[i].IsLeaf()) 0709 { 0710 m_nodes[i].parent = b2_nullNode; 0711 nodes[count] = i; 0712 ++count; 0713 } 0714 else 0715 { 0716 FreeNode(i); 0717 } 0718 } 0719 0720 while (count > 1) 0721 { 0722 float32 minCost = b2_maxFloat; 0723 int32 iMin = -1, jMin = -1; 0724 for (int32 i = 0; i < count; ++i) 0725 { 0726 b2AABB aabbi = m_nodes[nodes[i]].aabb; 0727 0728 for (int32 j = i + 1; j < count; ++j) 0729 { 0730 b2AABB aabbj = m_nodes[nodes[j]].aabb; 0731 b2AABB b; 0732 b.Combine(aabbi, aabbj); 0733 float32 cost = b.GetPerimeter(); 0734 if (cost < minCost) 0735 { 0736 iMin = i; 0737 jMin = j; 0738 minCost = cost; 0739 } 0740 } 0741 } 0742 0743 int32 index1 = nodes[iMin]; 0744 int32 index2 = nodes[jMin]; 0745 b2TreeNode* child1 = m_nodes + index1; 0746 b2TreeNode* child2 = m_nodes + index2; 0747 0748 int32 parentIndex = AllocateNode(); 0749 b2TreeNode* parent = m_nodes + parentIndex; 0750 parent->child1 = index1; 0751 parent->child2 = index2; 0752 parent->height = 1 + b2Max(child1->height, child2->height); 0753 parent->aabb.Combine(child1->aabb, child2->aabb); 0754 parent->parent = b2_nullNode; 0755 0756 child1->parent = parentIndex; 0757 child2->parent = parentIndex; 0758 0759 nodes[jMin] = nodes[count-1]; 0760 nodes[iMin] = parentIndex; 0761 --count; 0762 } 0763 0764 m_root = nodes[0]; 0765 b2Free(nodes); 0766 0767 Validate(); 0768 } 0769 0770 void b2DynamicTree::ShiftOrigin(const b2Vec2& newOrigin) 0771 { 0772 // Build array of leaves. Free the rest. 0773 for (int32 i = 0; i < m_nodeCapacity; ++i) 0774 { 0775 m_nodes[i].aabb.lowerBound -= newOrigin; 0776 m_nodes[i].aabb.upperBound -= newOrigin; 0777 } 0778 }