/** Combine two AABBs into one. */
public static b2AABB CombineStatic(b2AABB aabb1, b2AABB aabb2)
{
b2AABB aabb = new b2AABB();
aabb.Combine(aabb1, aabb2);
return(aabb);
}
public void ValidateMetrics(int index)
{
if (index == b2TreeNode.b2_nullNode)
{
return;
}
b2TreeNode node = m_nodes[index];
int child1 = node.child1;
int child2 = node.child2;
if (node.IsLeaf())
{
Debug.Assert(child1 == b2TreeNode.b2_nullNode);
Debug.Assert(child2 == b2TreeNode.b2_nullNode);
Debug.Assert(node.height == 0);
return;
}
Debug.Assert(0 <= child1 && child1 < m_nodeCapacity);
Debug.Assert(0 <= child2 && child2 < m_nodeCapacity);
int height1 = m_nodes[child1].height;
int height2 = m_nodes[child2].height;
int height;
height = 1 + Math.Max(height1, height2);
Debug.Assert(node.height == height);
b2AABB aabb = b2AABB.Default;
aabb.Combine(ref m_nodes[child1].aabb, ref m_nodes[child2].aabb);
Debug.Assert(aabb.LowerBound == node.aabb.LowerBound);
Debug.Assert(aabb.UpperBound == node.aabb.UpperBound);
ValidateMetrics(child1);
ValidateMetrics(child2);
}
public void ValidateMetrics(int index)
{
if (index == b2TreeNode.b2_nullNode)
{
return;
}
b2TreeNode node = m_nodes[index];
int child1 = node.child1;
int child2 = node.child2;
if (node.IsLeaf())
{
Debug.Assert(child1 == b2TreeNode.b2_nullNode);
Debug.Assert(child2 == b2TreeNode.b2_nullNode);
Debug.Assert(node.height == 0);
return;
}
Debug.Assert(0 <= child1 && child1 < m_nodeCapacity);
Debug.Assert(0 <= child2 && child2 < m_nodeCapacity);
int height1 = m_nodes[child1].height;
int height2 = m_nodes[child2].height;
int height;
height = 1 + Math.Max(height1, height2);
Debug.Assert(node.height == height);
b2AABB aabb = new b2AABB();
aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);
Debug.Assert(aabb.lowerBound == node.aabb.lowerBound);
Debug.Assert(aabb.upperBound == node.aabb.upperBound);
ValidateMetrics(child1);
ValidateMetrics(child2);
}
public void RebuildBottomUp()
{
int[] nodes = new int[m_nodeCount];
int count = 0;
// Build array of leaves. Free the rest.
for (int i = 0; i < m_nodeCapacity; ++i)
{
if (m_nodes[i].height < 0)
{
// free node in pool
continue;
}
if (m_nodes[i].IsLeaf())
{
m_nodes[i].parentOrNext = b2TreeNode.b2_nullNode;
nodes[count] = i;
++count;
}
else
{
FreeNode(i);
}
}
while (count > 1)
{
float minCost = b2Settings.b2_maxFloat;
int iMin = -1, jMin = -1;
for (int i = 0; i < count; ++i)
{
b2AABB aabbi = m_nodes[nodes[i]].aabb;
for (int j = i + 1; j < count; ++j)
{
b2AABB aabbj = m_nodes[nodes[j]].aabb;
b2AABB b = new b2AABB();
b.Combine(aabbi, aabbj);
float cost = b.GetPerimeter();
if (cost < minCost)
{
iMin = i;
jMin = j;
minCost = cost;
}
}
}
int index1 = nodes[iMin];
int index2 = nodes[jMin];
b2TreeNode child1 = m_nodes[index1];
b2TreeNode child2 = m_nodes[index2];
int parentIndex = AllocateNode();
b2TreeNode parent = m_nodes[parentIndex];
parent.child1 = index1;
parent.child2 = index2;
parent.height = 1 + Math.Max(child1.height, child2.height);
parent.aabb.Combine(child1.aabb, child2.aabb);
parent.parentOrNext = b2TreeNode.b2_nullNode;
child1.parentOrNext = parentIndex;
child2.parentOrNext = parentIndex;
nodes[jMin] = nodes[count - 1];
nodes[iMin] = parentIndex;
--count;
}
m_root = nodes[0];
Validate();
}
public void InsertLeaf(int leaf)
{
++m_insertionCount;
if (m_root == b2TreeNode.b2_nullNode)
{
m_root = leaf;
m_nodes[m_root].parentOrNext = b2TreeNode.b2_nullNode;
return;
}
// Find the best sibling for this node
b2AABB leafAABB = m_nodes[leaf].aabb;
int index = m_root;
while (m_nodes[index].IsLeaf() == false)
{
int child1 = m_nodes[index].child1;
int child2 = m_nodes[index].child2;
float area = m_nodes[index].aabb.GetPerimeter();
b2AABB combinedAABB = new b2AABB();
combinedAABB.Combine(m_nodes[index].aabb, leafAABB);
float combinedArea = combinedAABB.GetPerimeter();
// Cost of creating a new parent for this node and the new leaf
float cost = 2.0f * combinedArea;
// Minimum cost of pushing the leaf further down the tree
float inheritanceCost = 2.0f * (combinedArea - area);
// Cost of descending into child1
float cost1;
if (m_nodes[child1].IsLeaf())
{
b2AABB aabb = new b2AABB();
aabb.Combine(leafAABB, m_nodes[child1].aabb);
cost1 = aabb.GetPerimeter() + inheritanceCost;
}
else
{
b2AABB aabb = new b2AABB();
aabb.Combine(leafAABB, m_nodes[child1].aabb);
float oldArea = m_nodes[child1].aabb.GetPerimeter();
float newArea = aabb.GetPerimeter();
cost1 = (newArea - oldArea) + inheritanceCost;
}
// Cost of descending into child2
float cost2;
if (m_nodes[child2].IsLeaf())
{
b2AABB aabb = new b2AABB();
aabb.Combine(leafAABB, m_nodes[child2].aabb);
cost2 = aabb.GetPerimeter() + inheritanceCost;
}
else
{
b2AABB aabb = new b2AABB();
aabb.Combine(leafAABB, m_nodes[child2].aabb);
float oldArea = m_nodes[child2].aabb.GetPerimeter();
float newArea = aabb.GetPerimeter();
cost2 = newArea - oldArea + inheritanceCost;
}
// Descend according to the minimum cost.
if (cost < cost1 && cost < cost2)
{
break;
}
// Descend
if (cost1 < cost2)
{
index = child1;
}
else
{
index = child2;
}
}
int sibling = index;
// Create a new parent.
int oldParent = m_nodes[sibling].parentOrNext;
int newParent = AllocateNode();
m_nodes[newParent].parentOrNext = oldParent;
m_nodes[newParent].userData = null;
m_nodes[newParent].aabb.Combine(leafAABB, m_nodes[sibling].aabb);
m_nodes[newParent].height = m_nodes[sibling].height + 1;
if (oldParent != b2TreeNode.b2_nullNode)
{
// The sibling was not the root.
if (m_nodes[oldParent].child1 == sibling)
{
m_nodes[oldParent].child1 = newParent;
}
else
{
m_nodes[oldParent].child2 = newParent;
}
m_nodes[newParent].child1 = sibling;
m_nodes[newParent].child2 = leaf;
m_nodes[sibling].parentOrNext = newParent;
m_nodes[leaf].parentOrNext = newParent;
}
else
{
// The sibling was the root.
m_nodes[newParent].child1 = sibling;
m_nodes[newParent].child2 = leaf;
m_nodes[sibling].parentOrNext = newParent;
m_nodes[leaf].parentOrNext = newParent;
m_root = newParent;
}
// Walk back up the tree fixing heights and AABBs
index = m_nodes[leaf].parentOrNext;
while (index != b2TreeNode.b2_nullNode)
{
index = Balance(index);
int child1 = m_nodes[index].child1;
int child2 = m_nodes[index].child2;
Debug.Assert(child1 != b2TreeNode.b2_nullNode);
Debug.Assert(child2 != b2TreeNode.b2_nullNode);
m_nodes[index].height = 1 + Math.Max(m_nodes[child1].height, m_nodes[child2].height);
m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);
index = m_nodes[index].parentOrNext;
}
//Validate();
}
public void RebuildBottomUp()
{
int[] nodes = new int[m_nodeCount];
int count = 0;
// Build array of leaves. Free the rest.
for (int i = 0; i < m_nodeCapacity; ++i)
{
if (m_nodes[i].height < 0)
{
// free node in pool
continue;
}
if (m_nodes[i].IsLeaf())
{
m_nodes[i].parentOrNext = b2TreeNode.b2_nullNode;
nodes[count] = i;
++count;
}
else
{
FreeNode(i);
}
}
while (count > 1)
{
float minCost = b2Settings.b2_maxFloat;
int iMin = -1, jMin = -1;
for (int i = 0; i < count; ++i)
{
b2AABB aabbi = m_nodes[nodes[i]].aabb;
for (int j = i + 1; j < count; ++j)
{
b2AABB aabbj = m_nodes[nodes[j]].aabb;
b2AABB b = b2AABB.Default;
b.Combine(ref aabbi, ref aabbj);
float cost = b.Perimeter;
if (cost < minCost)
{
iMin = i;
jMin = j;
minCost = cost;
}
}
}
int index1 = nodes[iMin];
int index2 = nodes[jMin];
b2TreeNode child1 = m_nodes[index1];
b2TreeNode child2 = m_nodes[index2];
int parentIndex = AllocateNode();
b2TreeNode parent = m_nodes[parentIndex];
parent.child1 = index1;
parent.child2 = index2;
parent.height = 1 + Math.Max(child1.height, child2.height);
parent.aabb.Combine(ref child1.aabb, ref child2.aabb);
parent.parentOrNext = b2TreeNode.b2_nullNode;
child1.parentOrNext = parentIndex;
child2.parentOrNext = parentIndex;
nodes[jMin] = nodes[count - 1];
nodes[iMin] = parentIndex;
--count;
}
m_root = nodes[0];
Validate();
}
public void InsertLeaf(int leaf)
{
++m_insertionCount;
if (m_root == b2TreeNode.b2_nullNode)
{
m_root = leaf;
m_nodes[m_root].parentOrNext = b2TreeNode.b2_nullNode;
return;
}
// Find the best sibling for this node
b2AABB leafAABB = m_nodes[leaf].aabb;
int index = m_root;
while (m_nodes[index].IsLeaf() == false)
{
int child1 = m_nodes[index].child1;
int child2 = m_nodes[index].child2;
float area = m_nodes[index].aabb.Perimeter;
b2AABB combinedAABB = b2AABB.Default;
combinedAABB.Combine(ref m_nodes[index].aabb, ref leafAABB);
float combinedArea = combinedAABB.Perimeter;
// Cost of creating a new parent for this node and the new leaf
float cost = 2.0f * combinedArea;
// Minimum cost of pushing the leaf further down the tree
float inheritanceCost = 2.0f * (combinedArea - area);
// Cost of descending into child1
float cost1;
if (m_nodes[child1].IsLeaf())
{
b2AABB aabb = b2AABB.Default;
aabb.Combine(ref leafAABB, ref m_nodes[child1].aabb);
cost1 = aabb.Perimeter + inheritanceCost;
}
else
{
b2AABB aabb = b2AABB.Default;
aabb.Combine(ref leafAABB, ref m_nodes[child1].aabb);
float oldArea = m_nodes[child1].aabb.Perimeter;
float newArea = aabb.Perimeter;
cost1 = (newArea - oldArea) + inheritanceCost;
}
// Cost of descending into child2
float cost2;
if (m_nodes[child2].IsLeaf())
{
b2AABB aabb = b2AABB.Default;
aabb.Combine(ref leafAABB, ref m_nodes[child2].aabb);
cost2 = aabb.Perimeter + inheritanceCost;
}
else
{
b2AABB aabb = b2AABB.Default;
aabb.Combine(ref leafAABB, ref m_nodes[child2].aabb);
float oldArea = m_nodes[child2].aabb.Perimeter;
float newArea = aabb.Perimeter;
cost2 = newArea - oldArea + inheritanceCost;
}
// Descend according to the minimum cost.
if (cost < cost1 && cost < cost2)
{
break;
}
// Descend
if (cost1 < cost2)
{
index = child1;
}
else
{
index = child2;
}
}
int sibling = index;
// Create a new parent.
int oldParent = m_nodes[sibling].parentOrNext;
int newParent = AllocateNode();
m_nodes[newParent].parentOrNext = oldParent;
m_nodes[newParent].userData = null;
m_nodes[newParent].aabb.Combine(ref leafAABB, ref m_nodes[sibling].aabb);
m_nodes[newParent].height = m_nodes[sibling].height + 1;
if (oldParent != b2TreeNode.b2_nullNode)
{
// The sibling was not the root.
if (m_nodes[oldParent].child1 == sibling)
{
m_nodes[oldParent].child1 = newParent;
}
else
{
m_nodes[oldParent].child2 = newParent;
}
m_nodes[newParent].child1 = sibling;
m_nodes[newParent].child2 = leaf;
m_nodes[sibling].parentOrNext = newParent;
m_nodes[leaf].parentOrNext = newParent;
}
else
{
// The sibling was the root.
m_nodes[newParent].child1 = sibling;
m_nodes[newParent].child2 = leaf;
m_nodes[sibling].parentOrNext = newParent;
m_nodes[leaf].parentOrNext = newParent;
m_root = newParent;
}
// Walk back up the tree fixing heights and AABBs
index = m_nodes[leaf].parentOrNext;
while (index != b2TreeNode.b2_nullNode)
{
index = Balance(index);
int child1 = m_nodes[index].child1;
int child2 = m_nodes[index].child2;
Debug.Assert(child1 != b2TreeNode.b2_nullNode);
Debug.Assert(child2 != b2TreeNode.b2_nullNode);
m_nodes[index].height = 1 + Math.Max(m_nodes[child1].height, m_nodes[child2].height);
m_nodes[index].aabb.Combine(ref m_nodes[child1].aabb, ref m_nodes[child2].aabb);
index = m_nodes[index].parentOrNext;
}
//Validate();
}