private void RemoveLeaf(int leaf)
{
if (leaf == m_root)
{
m_root = Null;
return;
}
int parent = m_nodes[leaf].Parent;
int grandParent = m_nodes[parent].Parent;
int sibling =
m_nodes[parent].ChildA == leaf
? m_nodes[parent].ChildB
: m_nodes[parent].ChildA;
if (grandParent != Null)
{
// destroy parent and connect sibling to grand parent
if (m_nodes[grandParent].ChildA == parent)
{
m_nodes[grandParent].ChildA = sibling;
}
else
{
m_nodes[grandParent].ChildB = sibling;
}
m_nodes[sibling].Parent = grandParent;
FreeNode(parent);
// adjust ancestor bounds
int index = grandParent;
while (index != Null)
{
index = Balance(index);
int childA = m_nodes[index].ChildA;
int childB = m_nodes[index].ChildB;
m_nodes[index].Bounds = Aabb.Union(m_nodes[childA].Bounds, m_nodes[childB].Bounds);
m_nodes[index].Height = 1 + Mathf.Max(m_nodes[childA].Height, m_nodes[childB].Height);
index = m_nodes[index].Parent;
}
}
else
{
m_root = sibling;
m_nodes[sibling].Parent = Null;
FreeNode(parent);
}
}
private int Balance(int a)
{
if (m_nodes[a].IsLeaf || m_nodes[a].Height < 2)
{
return(a);
}
int b = m_nodes[a].ChildA;
int c = m_nodes[a].ChildB;
int balance = m_nodes[c].Height - m_nodes[b].Height;
// rotate C up
if (balance > 1)
{
int f = m_nodes[c].ChildA;
int g = m_nodes[c].ChildB;
// swap A and C
m_nodes[c].ChildA = a;
m_nodes[c].Parent = m_nodes[a].Parent;
m_nodes[a].Parent = c;
// A's old parent should point to C
if (m_nodes[c].Parent != Null)
{
if (m_nodes[m_nodes[c].Parent].ChildA == a)
{
m_nodes[m_nodes[c].Parent].ChildA = c;
}
else
{
m_nodes[m_nodes[c].Parent].ChildB = c;
}
}
else
{
m_root = c;
}
// rotate
if (m_nodes[f].Height > m_nodes[g].Height)
{
m_nodes[c].ChildB = f;
m_nodes[a].ChildB = g;
m_nodes[g].Parent = a;
m_nodes[a].Bounds = Aabb.Union(m_nodes[b].Bounds, m_nodes[g].Bounds);
m_nodes[c].Bounds = Aabb.Union(m_nodes[a].Bounds, m_nodes[f].Bounds);
m_nodes[a].Height = 1 + Mathf.Max(m_nodes[b].Height, m_nodes[g].Height);
m_nodes[c].Height = 1 + Mathf.Max(m_nodes[a].Height, m_nodes[f].Height);
}
else
{
m_nodes[c].ChildB = g;
m_nodes[a].ChildB = f;
m_nodes[f].Parent = a;
m_nodes[a].Bounds = Aabb.Union(m_nodes[b].Bounds, m_nodes[f].Bounds);
m_nodes[c].Bounds = Aabb.Union(m_nodes[a].Bounds, m_nodes[g].Bounds);
m_nodes[a].Height = 1 + Mathf.Max(m_nodes[b].Height, m_nodes[f].Height);
m_nodes[c].Height = 1 + Mathf.Max(m_nodes[a].Height, m_nodes[g].Height);
}
return(c);
}
// rotate B up
if (balance < -1)
{
int d = m_nodes[b].ChildA;
int e = m_nodes[b].ChildB;
// swap A and B
m_nodes[b].ChildA = a;
m_nodes[b].Parent = m_nodes[a].Parent;
m_nodes[a].Parent = b;
// A's old parent should point to B
if (m_nodes[b].Parent != Null)
{
if (m_nodes[m_nodes[b].Parent].ChildA == a)
{
m_nodes[m_nodes[b].Parent].ChildA = b;
}
else
{
m_nodes[m_nodes[b].Parent].ChildB = b;
}
}
else
{
m_root = b;
}
// rotate
if (m_nodes[d].Height > m_nodes[e].Height)
{
m_nodes[b].ChildB = d;
m_nodes[a].ChildA = e;
m_nodes[e].Parent = a;
m_nodes[a].Bounds = Aabb.Union(m_nodes[c].Bounds, m_nodes[e].Bounds);
m_nodes[b].Bounds = Aabb.Union(m_nodes[a].Bounds, m_nodes[d].Bounds);
m_nodes[a].Height = 1 + Mathf.Max(m_nodes[c].Height, m_nodes[e].Height);
m_nodes[b].Height = 1 + Mathf.Max(m_nodes[a].Height, m_nodes[d].Height);
}
else
{
m_nodes[b].ChildB = e;
m_nodes[a].ChildA = d;
m_nodes[d].Parent = a;
m_nodes[a].Bounds = Aabb.Union(m_nodes[c].Bounds, m_nodes[d].Bounds);
m_nodes[b].Bounds = Aabb.Union(m_nodes[a].Bounds, m_nodes[e].Bounds);
m_nodes[a].Height = 1 + Mathf.Max(m_nodes[c].Height, m_nodes[d].Height);
m_nodes[b].Height = 1 + Mathf.Max(m_nodes[a].Height, m_nodes[e].Height);
}
return(b);
}
return(a);
}
private void InsertLeaf(int leaf)
{
if (m_root == Null)
{
m_root = leaf;
m_nodes[m_root].Parent = Null;
return;
}
// find best sibling
Aabb leafBounds = m_nodes[leaf].Bounds;
int index = m_root;
while (!m_nodes[index].IsLeaf)
{
int childA = m_nodes[index].ChildA;
int childB = m_nodes[index].ChildB;
float area = m_nodes[index].Bounds.HalfArea;
Aabb combinedBounds = Aabb.Union(m_nodes[index].Bounds, leafBounds);
float combinedArea = combinedBounds.HalfArea;
// 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 child A
float costA;
if (m_nodes[childA].IsLeaf)
{
Aabb bounds;
bounds = Aabb.Union(leafBounds, m_nodes[childA].Bounds);
costA = bounds.HalfArea + inheritanceCost;
}
else
{
Aabb bounds;
bounds = Aabb.Union(leafBounds, m_nodes[childA].Bounds);
float oldArea = m_nodes[childA].Bounds.HalfArea;
float newArea = bounds.HalfArea;
costA = (newArea - oldArea) + inheritanceCost;
}
// cost of descending into child B
float costB;
if (m_nodes[childB].IsLeaf)
{
Aabb bounds;
bounds = Aabb.Union(leafBounds, m_nodes[childB].Bounds);
costB = bounds.HalfArea + inheritanceCost;
}
else
{
Aabb bounds;
bounds = Aabb.Union(leafBounds, m_nodes[childB].Bounds);
float oldArea = m_nodes[childB].Bounds.HalfArea;
float newArea = bounds.HalfArea;
costB = (newArea - oldArea) + inheritanceCost;
}
// descend according to the minimum cost
if (cost < costA && cost < costB)
{
break;
}
//descend
index = (costA < costB) ? childA : childB;
}
int sibling = index;
// create a new parent
int oldParent = m_nodes[sibling].Parent;
int newParent = AllocateNode();
m_nodes[newParent].Parent = oldParent;
m_nodes[newParent].Bounds = Aabb.Union(leafBounds, m_nodes[sibling].Bounds);
m_nodes[newParent].Height = m_nodes[sibling].Height + 1;
if (oldParent != Null)
{
// sibling was not the root
if (m_nodes[oldParent].ChildA == sibling)
{
m_nodes[oldParent].ChildA = newParent;
}
else
{
m_nodes[oldParent].ChildB = newParent;
}
m_nodes[newParent].ChildA = sibling;
m_nodes[newParent].ChildB = leaf;
m_nodes[sibling].Parent = newParent;
m_nodes[leaf].Parent = newParent;
}
else
{
// sibling was the root
m_nodes[newParent].ChildA = sibling;
m_nodes[newParent].ChildB = leaf;
m_nodes[sibling].Parent = newParent;
m_nodes[leaf].Parent = newParent;
m_root = newParent;
}
// walk back up to re-balance heights
index = m_nodes[leaf].Parent;
while (index != Null)
{
index = Balance(index);
int childA = m_nodes[index].ChildA;
int childB = m_nodes[index].ChildB;
m_nodes[index].Height = 1 + Mathf.Max(m_nodes[childA].Height, m_nodes[childB].Height);
m_nodes[index].Bounds = Aabb.Union(m_nodes[childA].Bounds, m_nodes[childB].Bounds);
index = m_nodes[index].Parent;
}
}