private void InsertLeaf(int leaf)
{
++m_insertionCount;
if (m_root == TreeNode._nullNode)
{
m_root = leaf;
m_nodes[m_root].parent = TreeNode._nullNode;
return;
}
// Find the best sibling for this node
AABB 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();
AABB combinedAABB = new AABB();
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())
{
AABB aabb = new AABB();
aabb.Combine(leafAABB, m_nodes[child1].aabb);
cost1 = aabb.GetPerimeter() + inheritanceCost;
}
else
{
AABB aabb = new AABB();
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())
{
AABB aabb = new AABB();
aabb.Combine(leafAABB, m_nodes[child2].aabb);
cost2 = aabb.GetPerimeter() + inheritanceCost;
}
else
{
AABB aabb = new AABB();
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].parent;
int newParent = AllocateNode();
m_nodes[newParent].parent = 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 != TreeNode._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].parent = newParent;
m_nodes[leaf].parent = newParent;
}
else
{
// The sibling was the root.
m_nodes[newParent].child1 = sibling;
m_nodes[newParent].child2 = leaf;
m_nodes[sibling].parent = newParent;
m_nodes[leaf].parent = newParent;
m_root = newParent;
}
// Walk back up the tree fixing heights and AABBs
index = m_nodes[leaf].parent;
while (index != TreeNode._nullNode)
{
index = Balance(index);
int child1 = m_nodes[index].child1;
int child2 = m_nodes[index].child2;
Utilities.Assert(child1 != TreeNode._nullNode);
Utilities.Assert(child2 != TreeNode._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].parent;
}
//Validate();
}
private void InsertLeaf(int leaf)
{
++_insertionCount;
if (_root == NullNode)
{
_root = leaf;
_nodes[_root].parentOrNext = NullNode;
return;
}
// Find the best sibling for this node
AABB leafAABB = _nodes[leaf].aabb;
Vector2 leafCenter = leafAABB.GetCenter();
int sibling = _root;
while (_nodes[sibling].IsLeaf() == false)
{
// Expand the node's AABB.
_nodes[sibling].aabb.Combine(ref leafAABB);
_nodes[sibling].leafCount += 1;
int child1 = _nodes[sibling].child1;
int child2 = _nodes[sibling].child2;
#if false
// This seems to create imbalanced trees
Vector2 delta1 = Math.Abs(_nodes[child1].aabb.GetCenter() - leafCenter);
Vector2 delta2 = Math.Abs(_nodes[child2].aabb.GetCenter() - leafCenter);
float norm1 = delta1.x + delta1.y;
float norm2 = delta2.x + delta2.y;
#else
// Surface area heuristic
AABB aabb1 = new AABB();
AABB aabb2 = new AABB();
aabb1.Combine(ref leafAABB, ref _nodes[child1].aabb);
aabb2.Combine(ref leafAABB, ref _nodes[child2].aabb);
float norm1 = (_nodes[child1].leafCount + 1) * aabb1.GetPerimeter();
float norm2 = (_nodes[child2].leafCount + 1) * aabb2.GetPerimeter();
#endif
if (norm1 < norm2)
{
sibling = child1;
}
else
{
sibling = child2;
}
}
// Create a new parent for the siblings.
int oldParent = _nodes[sibling].parentOrNext;
int newParent = AllocateNode();
_nodes[newParent].parentOrNext = oldParent;
_nodes[newParent].userData = null;
_nodes[newParent].aabb.Combine(ref leafAABB, ref _nodes[sibling].aabb);
_nodes[newParent].leafCount = _nodes[sibling].leafCount + 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;
}
}
