private void UpdateRotated(
Node P,
Node Q,
Node R,
int iP,
int iX,
int iY,
bool left)
{
Node X = this.nodes[iX];
Node Y = this.nodes[iY];
R.right = iX;
if (left)
{
P.left = iY;
}
else
{
P.right = iY;
}
Y.parentOrNext = iP;
P.aabb = VoltAABB.CreateMerged(Q.aabb, Y.aabb);
R.aabb = VoltAABB.CreateMerged(P.aabb, X.aabb);
P.height = 1 + Math.Max(Q.height, Y.height);
R.height = 1 + Math.Max(P.height, X.height);
}
private void InsertLeaf(int leafId)
{
if (this.rootId == NULL_NODE)
{
this.rootId = leafId;
this.nodes[this.rootId].parentOrNext = NULL_NODE;
return;
}
// Find the best sibling for this node
Node leafNode = this.nodes[leafId];
VoltAABB leafAABB = leafNode.aabb;
int siblingId = this.FindBestSibling(ref leafAABB);
Node sibling = this.nodes[siblingId];
// Create a new parent
int oldParentId = sibling.parentOrNext;
int newParentId;
Node newParent = this.AllocateNode(out newParentId);
newParent.Initialize(oldParentId, sibling.height + 1);
newParent.aabb = VoltAABB.CreateMerged(leafAABB, sibling.aabb);
if (oldParentId != NULL_NODE)
{
Node oldParent = this.nodes[oldParentId];
// The sibling was not the root
if (oldParent.left == siblingId)
{
oldParent.left = newParentId;
}
else
{
oldParent.right = newParentId;
}
}
else
{
// The sibling was the root
this.rootId = newParentId;
}
newParent.left = siblingId;
newParent.right = leafId;
sibling.parentOrNext = newParentId;
leafNode.parentOrNext = newParentId;
// Walk back up the tree fixing heights and AABBs
this.FixAncestors(leafNode.parentOrNext);
}
private void FixAncestors(int index)
{
while (index != NULL_NODE)
{
index = this.Balance(index);
Node indexNode = this.nodes[index];
Node left = this.nodes[indexNode.left];
Node right = this.nodes[indexNode.right];
indexNode.aabb = VoltAABB.CreateMerged(left.aabb, right.aabb);
indexNode.height = 1 + Math.Max(left.height, right.height);
index = indexNode.parentOrNext;
}
}
private Fix64 GetCost(int index, ref VoltAABB leafAABB)
{
if (this.nodes[index].IsLeaf)
{
VoltAABB aabb =
VoltAABB.CreateMerged(leafAABB, this.nodes[index].aabb);
return(aabb.Perimeter);
}
else
{
VoltAABB aabb =
VoltAABB.CreateMerged(leafAABB, this.nodes[index].aabb);
Fix64 oldArea = this.nodes[index].aabb.Perimeter;
Fix64 newArea = aabb.Perimeter;
return(newArea - oldArea);
}
}
private int FindBestSibling(ref VoltAABB leafAABB)
{
int index = this.rootId;
while (this.nodes[index].IsLeaf == false)
{
Node indexNode = this.nodes[index];
int child1 = indexNode.left;
int child2 = indexNode.right;
Fix64 area = indexNode.aabb.Perimeter;
VoltAABB combinedAABB = new VoltAABB();
VoltAABB.CreateMerged(indexNode.aabb, leafAABB);
Fix64 combinedArea = combinedAABB.Perimeter;
// Cost of creating a new parent for this node and the new leaf
Fix64 cost = (Fix64)2 * combinedArea;
// Minimum cost of pushing the leaf further down the tree
Fix64 inheritanceCost = (Fix64)2 * (combinedArea - area);
Fix64 cost1 = this.GetCost(child1, ref leafAABB) + inheritanceCost;
Fix64 cost2 = this.GetCost(child2, ref leafAABB) + inheritanceCost;
// Descend according to the minimum cost.
if ((cost < cost1) && (cost1 < cost2))
{
break;
}
// Descend
if (cost1 < cost2)
{
index = child1;
}
else
{
index = child2;
}
}
return(index);
}