private void RemoveLeaf(int leaf)
{
if (_root == NullNode)
{
return;
}
if (leaf == _root)
{
_root = NullNode;
return;
}
int parent = _nodes[leaf].ParentOrNext;
int grandParent = _nodes[parent].ParentOrNext;
int sibling;
if (_nodes[parent].Child1 == leaf)
{
sibling = _nodes[parent].Child2;
}
else
{
sibling = _nodes[parent].Child1;
}
if (grandParent != NullNode)
{
// Destroy node2 and connect node1 to sibling.
if (_nodes[grandParent].Child1 == parent)
{
_nodes[grandParent].Child1 = sibling;
}
else
{
_nodes[grandParent].Child2 = sibling;
}
_nodes[sibling].ParentOrNext = grandParent;
FreeNode(parent);
// Adjust ancestor bounds.
int index = grandParent;
while (index != NullNode)
{
index = Balance(index);
int child1 = _nodes[index].Child1;
int child2 = _nodes[index].Child2;
_nodes[index].Aabb = BoundingBoxD.CreateMerged(_nodes[child1].Aabb, _nodes[child2].Aabb);
_nodes[index].Height = 1 + Math.Max(_nodes[child1].Height, _nodes[child2].Height);
index = _nodes[index].ParentOrNext;
}
}
else
{
_root = sibling;
_nodes[sibling].ParentOrNext = NullNode;
FreeNode(parent);
}
}
// Perform a left or right rotation if node A is imbalanced.
// Returns the new root index.
public int Balance(int iA)
{
Debug.Assert(iA != NullNode);
DynamicTreeNode A = _nodes[iA];
if (A.IsLeaf() || A.Height < 2)
{
return(iA);
}
int iB = A.Child1;
int iC = A.Child2;
Debug.Assert(0 <= iB && iB < _nodeCapacity);
Debug.Assert(0 <= iC && iC < _nodeCapacity);
DynamicTreeNode B = _nodes[iB];
DynamicTreeNode C = _nodes[iC];
int balance = C.Height - B.Height;
//Rotate C up
if (balance > 1)
{
int iF = C.Child1;
int iG = C.Child2;
DynamicTreeNode F = _nodes[iF];
DynamicTreeNode G = _nodes[iG];
Debug.Assert(0 <= iF && iF < _nodeCapacity);
Debug.Assert(0 <= iG && iG < _nodeCapacity);
// Swap A and C
C.Child1 = iA;
C.ParentOrNext = A.ParentOrNext;
A.ParentOrNext = iC;
// A's old parent should point to C
if (C.ParentOrNext != NullNode)
{
if (_nodes[C.ParentOrNext].Child1 == iA)
{
_nodes[C.ParentOrNext].Child1 = iC;
}
else
{
Debug.Assert(_nodes[C.ParentOrNext].Child2 == iA);
_nodes[C.ParentOrNext].Child2 = iC;
}
}
else
{
_root = iC;
}
// Rotate
if (F.Height > G.Height)
{
C.Child2 = iF;
A.Child2 = iG;
G.ParentOrNext = iA;
BoundingBoxD.CreateMerged(ref B.Aabb, ref G.Aabb, out A.Aabb);
BoundingBoxD.CreateMerged(ref A.Aabb, ref F.Aabb, out C.Aabb);
A.Height = 1 + Math.Max(B.Height, G.Height);
C.Height = 1 + Math.Max(A.Height, F.Height);
}
else
{
C.Child2 = iG;
A.Child2 = iF;
F.ParentOrNext = iA;
BoundingBoxD.CreateMerged(ref B.Aabb, ref F.Aabb, out A.Aabb);
BoundingBoxD.CreateMerged(ref A.Aabb, ref G.Aabb, out C.Aabb);
A.Height = 1 + Math.Max(B.Height, F.Height);
C.Height = 1 + Math.Max(A.Height, G.Height);
}
return(iC);
}
// Rotate B up
if (balance < -1)
{
int iD = B.Child1;
int iE = B.Child2;
DynamicTreeNode D = _nodes[iD];
DynamicTreeNode E = _nodes[iE];
Debug.Assert(0 <= iD && iD < _nodeCapacity);
Debug.Assert(0 <= iE && iE < _nodeCapacity);
// Swap A and B
B.Child1 = iA;
B.ParentOrNext = A.ParentOrNext;
A.ParentOrNext = iB;
// A's old parent should point to B
if (B.ParentOrNext != NullNode)
{
if (_nodes[B.ParentOrNext].Child1 == iA)
{
_nodes[B.ParentOrNext].Child1 = iB;
}
else
{
Debug.Assert(_nodes[B.ParentOrNext].Child2 == iA);
_nodes[B.ParentOrNext].Child2 = iB;
}
}
else
{
_root = iB;
}
// Rotate
if (D.Height > E.Height)
{
B.Child2 = iD;
A.Child1 = iE;
E.ParentOrNext = iA;
BoundingBoxD.CreateMerged(ref C.Aabb, ref E.Aabb, out A.Aabb);
BoundingBoxD.CreateMerged(ref A.Aabb, ref D.Aabb, out B.Aabb);
A.Height = 1 + Math.Max(C.Height, E.Height);
B.Height = 1 + Math.Max(A.Height, D.Height);
}
else
{
B.Child2 = iE;
A.Child1 = iD;
D.ParentOrNext = iA;
BoundingBoxD.CreateMerged(ref C.Aabb, ref D.Aabb, out A.Aabb);
BoundingBoxD.CreateMerged(ref A.Aabb, ref E.Aabb, out B.Aabb);
A.Height = 1 + Math.Max(C.Height, D.Height);
B.Height = 1 + Math.Max(A.Height, E.Height);
}
return(iB);
}
return(iA);
}
private void InsertLeaf(int leaf, bool rebalance)
{
++_insertionCount;
if (_root == NullNode)
{
_root = leaf;
_nodes[_root].ParentOrNext = NullNode;
return;
}
// Find the best sibling for this node
BoundingBoxD leafAABB = _nodes[leaf].Aabb;
//Vector3 leafCenter = leafAABB.Min + (leafAABB.Max - leafAABB.Min) * 0.5f;
int index = _root;
while (_nodes[index].IsLeaf() == false)
{
int child1 = _nodes[index].Child1;
int child2 = _nodes[index].Child2;
if (rebalance)
{
double area = _nodes[index].Aabb.Perimeter;
// Combined area
BoundingBoxD combinedAABB = BoundingBoxD.CreateMerged(_nodes[index].Aabb, leafAABB);
double combinedArea = combinedAABB.Perimeter;
//Cost of creating a new parent for this node and the new leaf
double cost = 2.0f * combinedArea;
//Minimum cost of pushing the leaf further down the tree
double inheritanceCost = 2.0f * (combinedArea - area);
//Cost of descending into child1
double cost1;
if (_nodes[child1].IsLeaf())
{
BoundingBoxD aabb;
BoundingBoxD.CreateMerged(ref leafAABB, ref _nodes[child1].Aabb, out aabb);
cost1 = aabb.Perimeter + inheritanceCost;
}
else
{
BoundingBoxD aabb;
BoundingBoxD.CreateMerged(ref leafAABB, ref _nodes[child1].Aabb, out aabb);
double oldArea = _nodes[child1].Aabb.Perimeter;
double newArea = aabb.Perimeter;
cost1 = (newArea - oldArea) + inheritanceCost;
}
//Cost of descending into child2
double cost2;
if (_nodes[child2].IsLeaf())
{
BoundingBoxD aabb;
BoundingBoxD.CreateMerged(ref leafAABB, ref _nodes[child2].Aabb, out aabb);
cost2 = aabb.Perimeter + inheritanceCost;
}
else
{
BoundingBoxD aabb;
BoundingBoxD.CreateMerged(ref leafAABB, ref _nodes[child2].Aabb, out aabb);
double oldArea = _nodes[child2].Aabb.Perimeter;
double 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;
}
}
else
{
// Surface area heuristic
BoundingBoxD aabb1;
BoundingBoxD aabb2;
BoundingBoxD.CreateMerged(ref leafAABB, ref _nodes[child1].Aabb, out aabb1);
BoundingBoxD.CreateMerged(ref leafAABB, ref _nodes[child2].Aabb, out aabb2);
double norm1 = (_nodes[child1].Height + 1) * aabb1.Perimeter;
double norm2 = (_nodes[child2].Height + 1) * aabb2.Perimeter;
if (norm1 < norm2)
{
index = child1;
}
else
{
index = child2;
}
}
}
int sibling = index;
// Create a new parent for the siblings.
int oldParent = _nodes[index].ParentOrNext;
int newParent = AllocateNode();
_nodes[newParent].ParentOrNext = oldParent;
_nodes[newParent].UserData = null;
_nodes[newParent].Aabb = BoundingBoxD.CreateMerged(leafAABB, _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[index].ParentOrNext = newParent;
_nodes[leaf].ParentOrNext = newParent;
}
else
{
// The sibling was the root.
_nodes[newParent].Child1 = sibling;
_nodes[newParent].Child2 = leaf;
_nodes[index].ParentOrNext = newParent;
_nodes[leaf].ParentOrNext = newParent;
_root = newParent;
}
// Walk back up the tree fixing heights and AABBs
index = _nodes[leaf].ParentOrNext;
while (index != NullNode)
{
if (rebalance)
{
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);
BoundingBoxD.CreateMerged(ref _nodes[child1].Aabb, ref _nodes[child2].Aabb, out _nodes[index].Aabb);
index = _nodes[index].ParentOrNext;
}
}