b2DynamicTree.cpp
Go to the documentation of this file.
00001 /*
00002 * Copyright (c) 2009 Erin Catto http://www.box2d.org
00003 *
00004 * This software is provided 'as-is', without any express or implied
00005 * warranty.  In no event will the authors be held liable for any damages
00006 * arising from the use of this software.
00007 * Permission is granted to anyone to use this software for any purpose,
00008 * including commercial applications, and to alter it and redistribute it
00009 * freely, subject to the following restrictions:
00010 * 1. The origin of this software must not be misrepresented; you must not
00011 * claim that you wrote the original software. If you use this software
00012 * in a product, an acknowledgment in the product documentation would be
00013 * appreciated but is not required.
00014 * 2. Altered source versions must be plainly marked as such, and must not be
00015 * misrepresented as being the original software.
00016 * 3. This notice may not be removed or altered from any source distribution.
00017 */
00018 
00019 #include <Box2D/Collision/b2DynamicTree.h>
00020 #include <string.h>
00021 
00022 b2DynamicTree::b2DynamicTree()
00023 {
00024         m_root = b2_nullNode;
00025 
00026         m_nodeCapacity = 16;
00027         m_nodeCount = 0;
00028         m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode));
00029         memset(m_nodes, 0, m_nodeCapacity * sizeof(b2TreeNode));
00030 
00031         // Build a linked list for the free list.
00032         for (int32 i = 0; i < m_nodeCapacity - 1; ++i)
00033         {
00034                 m_nodes[i].next = i + 1;
00035                 m_nodes[i].height = -1;
00036         }
00037         m_nodes[m_nodeCapacity-1].next = b2_nullNode;
00038         m_nodes[m_nodeCapacity-1].height = -1;
00039         m_freeList = 0;
00040 
00041         m_path = 0;
00042 
00043         m_insertionCount = 0;
00044 }
00045 
00046 b2DynamicTree::~b2DynamicTree()
00047 {
00048         // This frees the entire tree in one shot.
00049         b2Free(m_nodes);
00050 }
00051 
00052 // Allocate a node from the pool. Grow the pool if necessary.
00053 int32 b2DynamicTree::AllocateNode()
00054 {
00055         // Expand the node pool as needed.
00056         if (m_freeList == b2_nullNode)
00057         {
00058                 b2Assert(m_nodeCount == m_nodeCapacity);
00059 
00060                 // The free list is empty. Rebuild a bigger pool.
00061                 b2TreeNode* oldNodes = m_nodes;
00062                 m_nodeCapacity *= 2;
00063                 m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode));
00064                 memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b2TreeNode));
00065                 b2Free(oldNodes);
00066 
00067                 // Build a linked list for the free list. The parent
00068                 // pointer becomes the "next" pointer.
00069                 for (int32 i = m_nodeCount; i < m_nodeCapacity - 1; ++i)
00070                 {
00071                         m_nodes[i].next = i + 1;
00072                         m_nodes[i].height = -1;
00073                 }
00074                 m_nodes[m_nodeCapacity-1].next = b2_nullNode;
00075                 m_nodes[m_nodeCapacity-1].height = -1;
00076                 m_freeList = m_nodeCount;
00077         }
00078 
00079         // Peel a node off the free list.
00080         int32 nodeId = m_freeList;
00081         m_freeList = m_nodes[nodeId].next;
00082         m_nodes[nodeId].parent = b2_nullNode;
00083         m_nodes[nodeId].child1 = b2_nullNode;
00084         m_nodes[nodeId].child2 = b2_nullNode;
00085         m_nodes[nodeId].height = 0;
00086         m_nodes[nodeId].userData = NULL;
00087         ++m_nodeCount;
00088         return nodeId;
00089 }
00090 
00091 // Return a node to the pool.
00092 void b2DynamicTree::FreeNode(int32 nodeId)
00093 {
00094         b2Assert(0 <= nodeId && nodeId < m_nodeCapacity);
00095         b2Assert(0 < m_nodeCount);
00096         m_nodes[nodeId].next = m_freeList;
00097         m_nodes[nodeId].height = -1;
00098         m_freeList = nodeId;
00099         --m_nodeCount;
00100 }
00101 
00102 // Create a proxy in the tree as a leaf node. We return the index
00103 // of the node instead of a pointer so that we can grow
00104 // the node pool.
00105 int32 b2DynamicTree::CreateProxy(const b2AABB& aabb, void* userData)
00106 {
00107         int32 proxyId = AllocateNode();
00108 
00109         // Fatten the aabb.
00110         b2Vec2 r(b2_aabbExtension, b2_aabbExtension);
00111         m_nodes[proxyId].aabb.lowerBound = aabb.lowerBound - r;
00112         m_nodes[proxyId].aabb.upperBound = aabb.upperBound + r;
00113         m_nodes[proxyId].userData = userData;
00114         m_nodes[proxyId].height = 0;
00115 
00116         InsertLeaf(proxyId);
00117 
00118         return proxyId;
00119 }
00120 
00121 void b2DynamicTree::DestroyProxy(int32 proxyId)
00122 {
00123         b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
00124         b2Assert(m_nodes[proxyId].IsLeaf());
00125 
00126         RemoveLeaf(proxyId);
00127         FreeNode(proxyId);
00128 }
00129 
00130 bool b2DynamicTree::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement)
00131 {
00132         b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
00133 
00134         b2Assert(m_nodes[proxyId].IsLeaf());
00135 
00136         if (m_nodes[proxyId].aabb.Contains(aabb))
00137         {
00138                 return false;
00139         }
00140 
00141         RemoveLeaf(proxyId);
00142 
00143         // Extend AABB.
00144         b2AABB b = aabb;
00145         b2Vec2 r(b2_aabbExtension, b2_aabbExtension);
00146         b.lowerBound = b.lowerBound - r;
00147         b.upperBound = b.upperBound + r;
00148 
00149         // Predict AABB displacement.
00150         b2Vec2 d = b2_aabbMultiplier * displacement;
00151 
00152         if (d.x < 0.0f)
00153         {
00154                 b.lowerBound.x += d.x;
00155         }
00156         else
00157         {
00158                 b.upperBound.x += d.x;
00159         }
00160 
00161         if (d.y < 0.0f)
00162         {
00163                 b.lowerBound.y += d.y;
00164         }
00165         else
00166         {
00167                 b.upperBound.y += d.y;
00168         }
00169 
00170         m_nodes[proxyId].aabb = b;
00171 
00172         InsertLeaf(proxyId);
00173         return true;
00174 }
00175 
00176 void b2DynamicTree::InsertLeaf(int32 leaf)
00177 {
00178         ++m_insertionCount;
00179 
00180         if (m_root == b2_nullNode)
00181         {
00182                 m_root = leaf;
00183                 m_nodes[m_root].parent = b2_nullNode;
00184                 return;
00185         }
00186 
00187         // Find the best sibling for this node
00188         b2AABB leafAABB = m_nodes[leaf].aabb;
00189         int32 index = m_root;
00190         while (m_nodes[index].IsLeaf() == false)
00191         {
00192                 int32 child1 = m_nodes[index].child1;
00193                 int32 child2 = m_nodes[index].child2;
00194 
00195                 float32 area = m_nodes[index].aabb.GetPerimeter();
00196 
00197                 b2AABB combinedAABB;
00198                 combinedAABB.Combine(m_nodes[index].aabb, leafAABB);
00199                 float32 combinedArea = combinedAABB.GetPerimeter();
00200 
00201                 // Cost of creating a new parent for this node and the new leaf
00202                 float32 cost = 2.0f * combinedArea;
00203 
00204                 // Minimum cost of pushing the leaf further down the tree
00205                 float32 inheritanceCost = 2.0f * (combinedArea - area);
00206 
00207                 // Cost of descending into child1
00208                 float32 cost1;
00209                 if (m_nodes[child1].IsLeaf())
00210                 {
00211                         b2AABB aabb;
00212                         aabb.Combine(leafAABB, m_nodes[child1].aabb);
00213                         cost1 = aabb.GetPerimeter() + inheritanceCost;
00214                 }
00215                 else
00216                 {
00217                         b2AABB aabb;
00218                         aabb.Combine(leafAABB, m_nodes[child1].aabb);
00219                         float32 oldArea = m_nodes[child1].aabb.GetPerimeter();
00220                         float32 newArea = aabb.GetPerimeter();
00221                         cost1 = (newArea - oldArea) + inheritanceCost;
00222                 }
00223 
00224                 // Cost of descending into child2
00225                 float32 cost2;
00226                 if (m_nodes[child2].IsLeaf())
00227                 {
00228                         b2AABB aabb;
00229                         aabb.Combine(leafAABB, m_nodes[child2].aabb);
00230                         cost2 = aabb.GetPerimeter() + inheritanceCost;
00231                 }
00232                 else
00233                 {
00234                         b2AABB aabb;
00235                         aabb.Combine(leafAABB, m_nodes[child2].aabb);
00236                         float32 oldArea = m_nodes[child2].aabb.GetPerimeter();
00237                         float32 newArea = aabb.GetPerimeter();
00238                         cost2 = newArea - oldArea + inheritanceCost;
00239                 }
00240 
00241                 // Descend according to the minimum cost.
00242                 if (cost < cost1 && cost < cost2)
00243                 {
00244                         break;
00245                 }
00246 
00247                 // Descend
00248                 if (cost1 < cost2)
00249                 {
00250                         index = child1;
00251                 }
00252                 else
00253                 {
00254                         index = child2;
00255                 }
00256         }
00257 
00258         int32 sibling = index;
00259 
00260         // Create a new parent.
00261         int32 oldParent = m_nodes[sibling].parent;
00262         int32 newParent = AllocateNode();
00263         m_nodes[newParent].parent = oldParent;
00264         m_nodes[newParent].userData = NULL;
00265         m_nodes[newParent].aabb.Combine(leafAABB, m_nodes[sibling].aabb);
00266         m_nodes[newParent].height = m_nodes[sibling].height + 1;
00267 
00268         if (oldParent != b2_nullNode)
00269         {
00270                 // The sibling was not the root.
00271                 if (m_nodes[oldParent].child1 == sibling)
00272                 {
00273                         m_nodes[oldParent].child1 = newParent;
00274                 }
00275                 else
00276                 {
00277                         m_nodes[oldParent].child2 = newParent;
00278                 }
00279 
00280                 m_nodes[newParent].child1 = sibling;
00281                 m_nodes[newParent].child2 = leaf;
00282                 m_nodes[sibling].parent = newParent;
00283                 m_nodes[leaf].parent = newParent;
00284         }
00285         else
00286         {
00287                 // The sibling was the root.
00288                 m_nodes[newParent].child1 = sibling;
00289                 m_nodes[newParent].child2 = leaf;
00290                 m_nodes[sibling].parent = newParent;
00291                 m_nodes[leaf].parent = newParent;
00292                 m_root = newParent;
00293         }
00294 
00295         // Walk back up the tree fixing heights and AABBs
00296         index = m_nodes[leaf].parent;
00297         while (index != b2_nullNode)
00298         {
00299                 index = Balance(index);
00300 
00301                 int32 child1 = m_nodes[index].child1;
00302                 int32 child2 = m_nodes[index].child2;
00303 
00304                 b2Assert(child1 != b2_nullNode);
00305                 b2Assert(child2 != b2_nullNode);
00306 
00307                 m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height);
00308                 m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);
00309 
00310                 index = m_nodes[index].parent;
00311         }
00312 
00313         //Validate();
00314 }
00315 
00316 void b2DynamicTree::RemoveLeaf(int32 leaf)
00317 {
00318         if (leaf == m_root)
00319         {
00320                 m_root = b2_nullNode;
00321                 return;
00322         }
00323 
00324         int32 parent = m_nodes[leaf].parent;
00325         int32 grandParent = m_nodes[parent].parent;
00326         int32 sibling;
00327         if (m_nodes[parent].child1 == leaf)
00328         {
00329                 sibling = m_nodes[parent].child2;
00330         }
00331         else
00332         {
00333                 sibling = m_nodes[parent].child1;
00334         }
00335 
00336         if (grandParent != b2_nullNode)
00337         {
00338                 // Destroy parent and connect sibling to grandParent.
00339                 if (m_nodes[grandParent].child1 == parent)
00340                 {
00341                         m_nodes[grandParent].child1 = sibling;
00342                 }
00343                 else
00344                 {
00345                         m_nodes[grandParent].child2 = sibling;
00346                 }
00347                 m_nodes[sibling].parent = grandParent;
00348                 FreeNode(parent);
00349 
00350                 // Adjust ancestor bounds.
00351                 int32 index = grandParent;
00352                 while (index != b2_nullNode)
00353                 {
00354                         index = Balance(index);
00355 
00356                         int32 child1 = m_nodes[index].child1;
00357                         int32 child2 = m_nodes[index].child2;
00358 
00359                         m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);
00360                         m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height);
00361 
00362                         index = m_nodes[index].parent;
00363                 }
00364         }
00365         else
00366         {
00367                 m_root = sibling;
00368                 m_nodes[sibling].parent = b2_nullNode;
00369                 FreeNode(parent);
00370         }
00371 
00372         //Validate();
00373 }
00374 
00375 // Perform a left or right rotation if node A is imbalanced.
00376 // Returns the new root index.
00377 int32 b2DynamicTree::Balance(int32 iA)
00378 {
00379         b2Assert(iA != b2_nullNode);
00380 
00381         b2TreeNode* A = m_nodes + iA;
00382         if (A->IsLeaf() || A->height < 2)
00383         {
00384                 return iA;
00385         }
00386 
00387         int32 iB = A->child1;
00388         int32 iC = A->child2;
00389         b2Assert(0 <= iB && iB < m_nodeCapacity);
00390         b2Assert(0 <= iC && iC < m_nodeCapacity);
00391 
00392         b2TreeNode* B = m_nodes + iB;
00393         b2TreeNode* C = m_nodes + iC;
00394 
00395         int32 balance = C->height - B->height;
00396 
00397         // Rotate C up
00398         if (balance > 1)
00399         {
00400                 int32 iF = C->child1;
00401                 int32 iG = C->child2;
00402                 b2TreeNode* F = m_nodes + iF;
00403                 b2TreeNode* G = m_nodes + iG;
00404                 b2Assert(0 <= iF && iF < m_nodeCapacity);
00405                 b2Assert(0 <= iG && iG < m_nodeCapacity);
00406 
00407                 // Swap A and C
00408                 C->child1 = iA;
00409                 C->parent = A->parent;
00410                 A->parent = iC;
00411 
00412                 // A's old parent should point to C
00413                 if (C->parent != b2_nullNode)
00414                 {
00415                         if (m_nodes[C->parent].child1 == iA)
00416                         {
00417                                 m_nodes[C->parent].child1 = iC;
00418                         }
00419                         else
00420                         {
00421                                 b2Assert(m_nodes[C->parent].child2 == iA);
00422                                 m_nodes[C->parent].child2 = iC;
00423                         }
00424                 }
00425                 else
00426                 {
00427                         m_root = iC;
00428                 }
00429 
00430                 // Rotate
00431                 if (F->height > G->height)
00432                 {
00433                         C->child2 = iF;
00434                         A->child2 = iG;
00435                         G->parent = iA;
00436                         A->aabb.Combine(B->aabb, G->aabb);
00437                         C->aabb.Combine(A->aabb, F->aabb);
00438 
00439                         A->height = 1 + b2Max(B->height, G->height);
00440                         C->height = 1 + b2Max(A->height, F->height);
00441                 }
00442                 else
00443                 {
00444                         C->child2 = iG;
00445                         A->child2 = iF;
00446                         F->parent = iA;
00447                         A->aabb.Combine(B->aabb, F->aabb);
00448                         C->aabb.Combine(A->aabb, G->aabb);
00449 
00450                         A->height = 1 + b2Max(B->height, F->height);
00451                         C->height = 1 + b2Max(A->height, G->height);
00452                 }
00453 
00454                 return iC;
00455         }
00456         
00457         // Rotate B up
00458         if (balance < -1)
00459         {
00460                 int32 iD = B->child1;
00461                 int32 iE = B->child2;
00462                 b2TreeNode* D = m_nodes + iD;
00463                 b2TreeNode* E = m_nodes + iE;
00464                 b2Assert(0 <= iD && iD < m_nodeCapacity);
00465                 b2Assert(0 <= iE && iE < m_nodeCapacity);
00466 
00467                 // Swap A and B
00468                 B->child1 = iA;
00469                 B->parent = A->parent;
00470                 A->parent = iB;
00471 
00472                 // A's old parent should point to B
00473                 if (B->parent != b2_nullNode)
00474                 {
00475                         if (m_nodes[B->parent].child1 == iA)
00476                         {
00477                                 m_nodes[B->parent].child1 = iB;
00478                         }
00479                         else
00480                         {
00481                                 b2Assert(m_nodes[B->parent].child2 == iA);
00482                                 m_nodes[B->parent].child2 = iB;
00483                         }
00484                 }
00485                 else
00486                 {
00487                         m_root = iB;
00488                 }
00489 
00490                 // Rotate
00491                 if (D->height > E->height)
00492                 {
00493                         B->child2 = iD;
00494                         A->child1 = iE;
00495                         E->parent = iA;
00496                         A->aabb.Combine(C->aabb, E->aabb);
00497                         B->aabb.Combine(A->aabb, D->aabb);
00498 
00499                         A->height = 1 + b2Max(C->height, E->height);
00500                         B->height = 1 + b2Max(A->height, D->height);
00501                 }
00502                 else
00503                 {
00504                         B->child2 = iE;
00505                         A->child1 = iD;
00506                         D->parent = iA;
00507                         A->aabb.Combine(C->aabb, D->aabb);
00508                         B->aabb.Combine(A->aabb, E->aabb);
00509 
00510                         A->height = 1 + b2Max(C->height, D->height);
00511                         B->height = 1 + b2Max(A->height, E->height);
00512                 }
00513 
00514                 return iB;
00515         }
00516 
00517         return iA;
00518 }
00519 
00520 int32 b2DynamicTree::GetHeight() const
00521 {
00522         if (m_root == b2_nullNode)
00523         {
00524                 return 0;
00525         }
00526 
00527         return m_nodes[m_root].height;
00528 }
00529 
00530 //
00531 float32 b2DynamicTree::GetAreaRatio() const
00532 {
00533         if (m_root == b2_nullNode)
00534         {
00535                 return 0.0f;
00536         }
00537 
00538         const b2TreeNode* root = m_nodes + m_root;
00539         float32 rootArea = root->aabb.GetPerimeter();
00540 
00541         float32 totalArea = 0.0f;
00542         for (int32 i = 0; i < m_nodeCapacity; ++i)
00543         {
00544                 const b2TreeNode* node = m_nodes + i;
00545                 if (node->height < 0)
00546                 {
00547                         // Free node in pool
00548                         continue;
00549                 }
00550 
00551                 totalArea += node->aabb.GetPerimeter();
00552         }
00553 
00554         return totalArea / rootArea;
00555 }
00556 
00557 // Compute the height of a sub-tree.
00558 int32 b2DynamicTree::ComputeHeight(int32 nodeId) const
00559 {
00560         b2Assert(0 <= nodeId && nodeId < m_nodeCapacity);
00561         b2TreeNode* node = m_nodes + nodeId;
00562 
00563         if (node->IsLeaf())
00564         {
00565                 return 0;
00566         }
00567 
00568         int32 height1 = ComputeHeight(node->child1);
00569         int32 height2 = ComputeHeight(node->child2);
00570         return 1 + b2Max(height1, height2);
00571 }
00572 
00573 int32 b2DynamicTree::ComputeHeight() const
00574 {
00575         int32 height = ComputeHeight(m_root);
00576         return height;
00577 }
00578 
00579 void b2DynamicTree::ValidateStructure(int32 index) const
00580 {
00581         if (index == b2_nullNode)
00582         {
00583                 return;
00584         }
00585 
00586         if (index == m_root)
00587         {
00588                 b2Assert(m_nodes[index].parent == b2_nullNode);
00589         }
00590 
00591         const b2TreeNode* node = m_nodes + index;
00592 
00593         int32 child1 = node->child1;
00594         int32 child2 = node->child2;
00595 
00596         if (node->IsLeaf())
00597         {
00598                 b2Assert(child1 == b2_nullNode);
00599                 b2Assert(child2 == b2_nullNode);
00600                 b2Assert(node->height == 0);
00601                 return;
00602         }
00603 
00604         b2Assert(0 <= child1 && child1 < m_nodeCapacity);
00605         b2Assert(0 <= child2 && child2 < m_nodeCapacity);
00606 
00607         b2Assert(m_nodes[child1].parent == index);
00608         b2Assert(m_nodes[child2].parent == index);
00609 
00610         ValidateStructure(child1);
00611         ValidateStructure(child2);
00612 }
00613 
00614 void b2DynamicTree::ValidateMetrics(int32 index) const
00615 {
00616         if (index == b2_nullNode)
00617         {
00618                 return;
00619         }
00620 
00621         const b2TreeNode* node = m_nodes + index;
00622 
00623         int32 child1 = node->child1;
00624         int32 child2 = node->child2;
00625 
00626         if (node->IsLeaf())
00627         {
00628                 b2Assert(child1 == b2_nullNode);
00629                 b2Assert(child2 == b2_nullNode);
00630                 b2Assert(node->height == 0);
00631                 return;
00632         }
00633 
00634         b2Assert(0 <= child1 && child1 < m_nodeCapacity);
00635         b2Assert(0 <= child2 && child2 < m_nodeCapacity);
00636 
00637         int32 height1 = m_nodes[child1].height;
00638         int32 height2 = m_nodes[child2].height;
00639         int32 height;
00640         height = 1 + b2Max(height1, height2);
00641         b2Assert(node->height == height);
00642 
00643         b2AABB aabb;
00644         aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);
00645 
00646         b2Assert(aabb.lowerBound == node->aabb.lowerBound);
00647         b2Assert(aabb.upperBound == node->aabb.upperBound);
00648 
00649         ValidateMetrics(child1);
00650         ValidateMetrics(child2);
00651 }
00652 
00653 void b2DynamicTree::Validate() const
00654 {
00655         ValidateStructure(m_root);
00656         ValidateMetrics(m_root);
00657 
00658         int32 freeCount = 0;
00659         int32 freeIndex = m_freeList;
00660         while (freeIndex != b2_nullNode)
00661         {
00662                 b2Assert(0 <= freeIndex && freeIndex < m_nodeCapacity);
00663                 freeIndex = m_nodes[freeIndex].next;
00664                 ++freeCount;
00665         }
00666 
00667         b2Assert(GetHeight() == ComputeHeight());
00668 
00669         b2Assert(m_nodeCount + freeCount == m_nodeCapacity);
00670 }
00671 
00672 int32 b2DynamicTree::GetMaxBalance() const
00673 {
00674         int32 maxBalance = 0;
00675         for (int32 i = 0; i < m_nodeCapacity; ++i)
00676         {
00677                 const b2TreeNode* node = m_nodes + i;
00678                 if (node->height <= 1)
00679                 {
00680                         continue;
00681                 }
00682 
00683                 b2Assert(node->IsLeaf() == false);
00684 
00685                 int32 child1 = node->child1;
00686                 int32 child2 = node->child2;
00687                 int32 balance = b2Abs(m_nodes[child2].height - m_nodes[child1].height);
00688                 maxBalance = b2Max(maxBalance, balance);
00689         }
00690 
00691         return maxBalance;
00692 }
00693 
00694 void b2DynamicTree::RebuildBottomUp()
00695 {
00696         int32* nodes = (int32*)b2Alloc(m_nodeCount * sizeof(int32));
00697         int32 count = 0;
00698 
00699         // Build array of leaves. Free the rest.
00700         for (int32 i = 0; i < m_nodeCapacity; ++i)
00701         {
00702                 if (m_nodes[i].height < 0)
00703                 {
00704                         // free node in pool
00705                         continue;
00706                 }
00707 
00708                 if (m_nodes[i].IsLeaf())
00709                 {
00710                         m_nodes[i].parent = b2_nullNode;
00711                         nodes[count] = i;
00712                         ++count;
00713                 }
00714                 else
00715                 {
00716                         FreeNode(i);
00717                 }
00718         }
00719 
00720         while (count > 1)
00721         {
00722                 float32 minCost = b2_maxFloat;
00723                 int32 iMin = -1, jMin = -1;
00724                 for (int32 i = 0; i < count; ++i)
00725                 {
00726                         b2AABB aabbi = m_nodes[nodes[i]].aabb;
00727 
00728                         for (int32 j = i + 1; j < count; ++j)
00729                         {
00730                                 b2AABB aabbj = m_nodes[nodes[j]].aabb;
00731                                 b2AABB b;
00732                                 b.Combine(aabbi, aabbj);
00733                                 float32 cost = b.GetPerimeter();
00734                                 if (cost < minCost)
00735                                 {
00736                                         iMin = i;
00737                                         jMin = j;
00738                                         minCost = cost;
00739                                 }
00740                         }
00741                 }
00742 
00743                 int32 index1 = nodes[iMin];
00744                 int32 index2 = nodes[jMin];
00745                 b2TreeNode* child1 = m_nodes + index1;
00746                 b2TreeNode* child2 = m_nodes + index2;
00747 
00748                 int32 parentIndex = AllocateNode();
00749                 b2TreeNode* parent = m_nodes + parentIndex;
00750                 parent->child1 = index1;
00751                 parent->child2 = index2;
00752                 parent->height = 1 + b2Max(child1->height, child2->height);
00753                 parent->aabb.Combine(child1->aabb, child2->aabb);
00754                 parent->parent = b2_nullNode;
00755 
00756                 child1->parent = parentIndex;
00757                 child2->parent = parentIndex;
00758 
00759                 nodes[jMin] = nodes[count-1];
00760                 nodes[iMin] = parentIndex;
00761                 --count;
00762         }
00763 
00764         m_root = nodes[0];
00765         b2Free(nodes);
00766 
00767         Validate();
00768 }
00769 
00770 void b2DynamicTree::ShiftOrigin(const b2Vec2& newOrigin)
00771 {
00772         // Build array of leaves. Free the rest.
00773         for (int32 i = 0; i < m_nodeCapacity; ++i)
00774         {
00775                 m_nodes[i].aabb.lowerBound -= newOrigin;
00776                 m_nodes[i].aabb.upperBound -= newOrigin;
00777         }
00778 }


mvsim
Author(s):
autogenerated on Thu Jun 6 2019 22:08:34