/// <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 (_nodes[i].Height < 0)
{
// free node in pool
continue;
}
if (_nodes[i].IsLeaf())
{
_nodes[i].ParentOrNext = NullNode;
nodes[count] = i;
++count;
}
else
{
FreeNode(i);
}
}
while (count > 1)
{
FP minCost = Settings.MaxFP;
int iMin = -1, jMin = -1;
for (int i = 0; i < count; ++i)
{
AABB AABBi = _nodes[nodes[i]].AABB;
for (int j = i + 1; j < count; ++j)
{
AABB AABBj = _nodes[nodes[j]].AABB;
AABB b = new AABB();
b.Combine(ref AABBi, ref AABBj);
FP cost = b.Perimeter;
if (cost < minCost)
{
iMin = i;
jMin = j;
minCost = cost;
}
}
}
int index1 = nodes[iMin];
int index2 = nodes[jMin];
TreeNode <T> child1 = _nodes[index1];
TreeNode <T> child2 = _nodes[index2];
int parentIndex = AllocateNode();
TreeNode <T> parent = _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();
}
public void RebuildBottomUp()
{
int[] array = new int[this._nodeCount];
int i = 0;
for (int j = 0; j < this._nodeCapacity; j++)
{
bool flag = this._nodes[j].Height < 0;
if (!flag)
{
bool flag2 = this._nodes[j].IsLeaf();
if (flag2)
{
this._nodes[j].ParentOrNext = -1;
array[i] = j;
i++;
}
else
{
this.FreeNode(j);
}
}
}
while (i > 1)
{
FP y = Settings.MaxFP;
int num = -1;
int num2 = -1;
for (int k = 0; k < i; k++)
{
AABB aABB = this._nodes[array[k]].AABB;
for (int l = k + 1; l < i; l++)
{
AABB aABB2 = this._nodes[array[l]].AABB;
AABB aABB3 = default(AABB);
aABB3.Combine(ref aABB, ref aABB2);
FP perimeter = aABB3.Perimeter;
bool flag3 = perimeter < y;
if (flag3)
{
num = k;
num2 = l;
y = perimeter;
}
}
}
int num3 = array[num];
int num4 = array[num2];
TreeNode <T> treeNode = this._nodes[num3];
TreeNode <T> treeNode2 = this._nodes[num4];
int num5 = this.AllocateNode();
TreeNode <T> treeNode3 = this._nodes[num5];
treeNode3.Child1 = num3;
treeNode3.Child2 = num4;
treeNode3.Height = 1 + Math.Max(treeNode.Height, treeNode2.Height);
treeNode3.AABB.Combine(ref treeNode.AABB, ref treeNode2.AABB);
treeNode3.ParentOrNext = -1;
treeNode.ParentOrNext = num5;
treeNode2.ParentOrNext = num5;
array[num2] = array[i - 1];
array[num] = num5;
i--;
}
this._root = array[0];
this.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;
FP area = _nodes[index].AABB.Perimeter;
AABB combinedAABB = new AABB();
combinedAABB.Combine(ref _nodes[index].AABB, ref leafAABB);
FP combinedArea = combinedAABB.Perimeter;
// Cost of creating a new parent for this node and the new leaf
FP cost = 2.0f * combinedArea;
// Minimum cost of pushing the leaf further down the tree
FP inheritanceCost = 2.0f * (combinedArea - area);
// Cost of descending into child1
FP 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);
FP oldArea = _nodes[child1].AABB.Perimeter;
FP newArea = aabb.Perimeter;
cost1 = (newArea - oldArea) + inheritanceCost;
}
// Cost of descending into child2
FP 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);
FP oldArea = _nodes[child2].AABB.Perimeter;
FP 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();
}
private void InsertLeaf(int leaf)
{
bool flag = this._root == -1;
if (flag)
{
this._root = leaf;
this._nodes[this._root].ParentOrNext = -1;
}
else
{
AABB aABB = this._nodes[leaf].AABB;
int num = this._root;
while (!this._nodes[num].IsLeaf())
{
int child = this._nodes[num].Child1;
int child2 = this._nodes[num].Child2;
FP perimeter = this._nodes[num].AABB.Perimeter;
AABB aABB2 = default(AABB);
aABB2.Combine(ref this._nodes[num].AABB, ref aABB);
FP perimeter2 = aABB2.Perimeter;
FP x = 2f * perimeter2;
FP y = 2f * (perimeter2 - perimeter);
bool flag2 = this._nodes[child].IsLeaf();
FP fP;
if (flag2)
{
AABB aABB3 = default(AABB);
aABB3.Combine(ref aABB, ref this._nodes[child].AABB);
fP = aABB3.Perimeter + y;
}
else
{
AABB aABB4 = default(AABB);
aABB4.Combine(ref aABB, ref this._nodes[child].AABB);
FP perimeter3 = this._nodes[child].AABB.Perimeter;
FP perimeter4 = aABB4.Perimeter;
fP = perimeter4 - perimeter3 + y;
}
bool flag3 = this._nodes[child2].IsLeaf();
FP y2;
if (flag3)
{
AABB aABB5 = default(AABB);
aABB5.Combine(ref aABB, ref this._nodes[child2].AABB);
y2 = aABB5.Perimeter + y;
}
else
{
AABB aABB6 = default(AABB);
aABB6.Combine(ref aABB, ref this._nodes[child2].AABB);
FP perimeter5 = this._nodes[child2].AABB.Perimeter;
FP perimeter6 = aABB6.Perimeter;
y2 = perimeter6 - perimeter5 + y;
}
bool flag4 = x < fP && fP < y2;
if (flag4)
{
break;
}
bool flag5 = fP < y2;
if (flag5)
{
num = child;
}
else
{
num = child2;
}
}
int num2 = num;
int parentOrNext = this._nodes[num2].ParentOrNext;
int num3 = this.AllocateNode();
this._nodes[num3].ParentOrNext = parentOrNext;
this._nodes[num3].UserData = default(T);
this._nodes[num3].AABB.Combine(ref aABB, ref this._nodes[num2].AABB);
this._nodes[num3].Height = this._nodes[num2].Height + 1;
bool flag6 = parentOrNext != -1;
if (flag6)
{
bool flag7 = this._nodes[parentOrNext].Child1 == num2;
if (flag7)
{
this._nodes[parentOrNext].Child1 = num3;
}
else
{
this._nodes[parentOrNext].Child2 = num3;
}
this._nodes[num3].Child1 = num2;
this._nodes[num3].Child2 = leaf;
this._nodes[num2].ParentOrNext = num3;
this._nodes[leaf].ParentOrNext = num3;
}
else
{
this._nodes[num3].Child1 = num2;
this._nodes[num3].Child2 = leaf;
this._nodes[num2].ParentOrNext = num3;
this._nodes[leaf].ParentOrNext = num3;
this._root = num3;
}
for (num = this._nodes[leaf].ParentOrNext; num != -1; num = this._nodes[num].ParentOrNext)
{
num = this.Balance(num);
int child3 = this._nodes[num].Child1;
int child4 = this._nodes[num].Child2;
Debug.Assert(child3 != -1);
Debug.Assert(child4 != -1);
this._nodes[num].Height = 1 + Math.Max(this._nodes[child3].Height, this._nodes[child4].Height);
this._nodes[num].AABB.Combine(ref this._nodes[child3].AABB, ref this._nodes[child4].AABB);
}
}
}
