• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
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>
22 using namespace std;
23 
24 
b2DynamicTree()25 b2DynamicTree::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 
~b2DynamicTree()49 b2DynamicTree::~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.
AllocateNode()56 int32 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.
FreeNode(int32 nodeId)95 void 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.
CreateProxy(const b2AABB & aabb,void * userData)108 int32 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 
DestroyProxy(int32 proxyId)124 void 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 
MoveProxy(int32 proxyId,const b2AABB & aabb,const b2Vec2 & displacement)133 bool 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 
InsertLeaf(int32 leaf)179 void 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 
RemoveLeaf(int32 leaf)319 void 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.
Balance(int32 iA)380 int32 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 
GetHeight() const523 int32 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 //
GetAreaRatio() const534 float32 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.
ComputeHeight(int32 nodeId) const561 int32 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 
ComputeHeight() const576 int32 b2DynamicTree::ComputeHeight() const
577 {
578 	int32 height = ComputeHeight(m_root);
579 	return height;
580 }
581 
ValidateStructure(int32 index) const582 void 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 
ValidateMetrics(int32 index) const617 void 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 
Validate() const656 void 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 
GetMaxBalance() const675 int32 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 
RebuildBottomUp()697 void 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 
ShiftOrigin(const b2Vec2 & newOrigin)773 void 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