/// <summary>
/// Build an optimal tree. Very expensive. For testing.
/// </summary>
public void RebuildBottomUp()
{
int[] nodes = new int[nodeCount];
int count = 0;
// Build array of leaves. Free the rest.
for (int i = 0; i < nodeCapacity; ++i)
{
if (this.nodes[i].Height < 0)
{
// free node in pool
continue;
}
if (this.nodes[i].IsLeaf())
{
this.nodes[i].ParentOrNext = NullNode;
nodes[count] = i;
++count;
}
else
{
FreeNode(i);
}
}
while (count > 1)
{
float minCost = float.MaxValue;
int iMin = -1, jMin = -1;
for (int i = 0; i < count; ++i)
{
AABB AABBi = this.nodes[nodes[i]].AABB;
for (int j = i + 1; j < count; ++j)
{
AABB AABBj = this.nodes[nodes[j]].AABB;
AABB b = new AABB();
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];
TreeNode <T> child1 = this.nodes[index1];
TreeNode <T> child2 = this.nodes[index2];
int parentIndex = AllocateNode();
TreeNode <T> parent = this.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 = NullNode;
child1.ParentOrNext = parentIndex;
child2.ParentOrNext = parentIndex;
nodes[jMin] = nodes[count - 1];
nodes[iMin] = parentIndex;
--count;
}
root = nodes[0];
Validate();
}
private void InsertLeaf(int leaf)
{
if (root == NullNode)
{
root = leaf;
nodes[root].ParentOrNext = NullNode;
return;
}
// Find the best sibling for this node
AABB leafAABB = nodes[leaf].AABB;
int index = root;
while (nodes[index].IsLeaf() == false)
{
int child1 = nodes[index].Child1;
int child2 = nodes[index].Child2;
float area = nodes[index].AABB.Perimeter;
AABB combinedAABB = new AABB();
combinedAABB.Combine(ref 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 (nodes[child1].IsLeaf())
{
AABB aabb = new AABB();
aabb.Combine(ref leafAABB, ref nodes[child1].AABB);
cost1 = aabb.Perimeter + inheritanceCost;
}
else
{
AABB aabb = new AABB();
aabb.Combine(ref leafAABB, ref nodes[child1].AABB);
float oldArea = nodes[child1].AABB.Perimeter;
float newArea = aabb.Perimeter;
cost1 = (newArea - oldArea) + inheritanceCost;
}
// Cost of descending into child2
float cost2;
if (nodes[child2].IsLeaf())
{
AABB aabb = new AABB();
aabb.Combine(ref leafAABB, ref nodes[child2].AABB);
cost2 = aabb.Perimeter + inheritanceCost;
}
else
{
AABB aabb = new AABB();
aabb.Combine(ref leafAABB, ref nodes[child2].AABB);
float oldArea = nodes[child2].AABB.Perimeter;
float newArea = aabb.Perimeter;
cost2 = newArea - oldArea + inheritanceCost;
}
// Descend according to the minimum cost.
if (cost < cost1 && cost1 < cost2)
{
break;
}
// Descend
if (cost1 < cost2)
{
index = child1;
}
else
{
index = child2;
}
}
int sibling = index;
// Create a new parent.
int oldParent = nodes[sibling].ParentOrNext;
int newParent = AllocateNode();
nodes[newParent].ParentOrNext = oldParent;
nodes[newParent].UserData = default(T);
nodes[newParent].AABB.Combine(ref leafAABB, ref nodes[sibling].AABB);
nodes[newParent].Height = nodes[sibling].Height + 1;
if (oldParent != NullNode)
{
// The sibling was not the root.
if (nodes[oldParent].Child1 == sibling)
{
nodes[oldParent].Child1 = newParent;
}
else
{
nodes[oldParent].Child2 = newParent;
}
nodes[newParent].Child1 = sibling;
nodes[newParent].Child2 = leaf;
nodes[sibling].ParentOrNext = newParent;
nodes[leaf].ParentOrNext = newParent;
}
else
{
// The sibling was the root.
nodes[newParent].Child1 = sibling;
nodes[newParent].Child2 = leaf;
nodes[sibling].ParentOrNext = newParent;
nodes[leaf].ParentOrNext = newParent;
root = newParent;
}
// Walk back up the tree fixing heights and AABBs
index = nodes[leaf].ParentOrNext;
while (index != NullNode)
{
index = Balance(index);
int child1 = nodes[index].Child1;
int child2 = nodes[index].Child2;
Debug.Assert(child1 != NullNode);
Debug.Assert(child2 != NullNode);
nodes[index].Height = 1 + Math.Max(nodes[child1].Height, nodes[child2].Height);
nodes[index].AABB.Combine(ref nodes[child1].AABB, ref nodes[child2].AABB);
index = nodes[index].ParentOrNext;
}
//Validate();
}