public void RemoveProxy(DynamicTreeNode node)
{
Debug.Assert(node.IsLeaf);
RemoveLeaf(node);
FreeNode(node);
}
void ValidateStructure(DynamicTreeNode node)
{
if (node == null)
{
return;
}
if (node == root)
{
Debug.Assert(node.Parent == null);
}
var leftChild = node.LeftChild;
var rightChild = node.RightChild;
if (node.IsLeaf)
{
Debug.Assert(leftChild == null);
Debug.Assert(rightChild == null);
Debug.Assert(node.Depth == 0);
return;
}
Debug.Assert(leftChild.Parent == node);
Debug.Assert(rightChild.Parent == node);
ValidateStructure(leftChild);
ValidateStructure(rightChild);
}
void ValidateMetrics(DynamicTreeNode node)
{
if (node == null)
{
return;
}
var leftChild = node.LeftChild;
var rightChild = node.RightChild;
if (node.IsLeaf)
{
Debug.Assert(leftChild == null);
Debug.Assert(rightChild == null);
Debug.Assert(node.Depth == 0);
return;
}
int depth1 = leftChild.Depth;
int depth2 = rightChild.Depth;
int depth = 1 + Math.Max(depth1, depth2);
Debug.Assert(node.Depth == depth);
var aabb = leftChild.AABB.Combine(rightChild.AABB);
Debug.Assert(aabb.LowerBound == node.AABB.LowerBound);
Debug.Assert(aabb.UpperBound == node.AABB.UpperBound);
ValidateMetrics(leftChild);
ValidateMetrics(rightChild);
}
private DynamicTreeNode FindSibling(DynamicTreeNode leaf)
{
var sibling = root;
while (!sibling.IsLeaf)
{
var area = sibling.AABB.Perimeter;
var combinedAABB = leaf.AABB.Combine(sibling.AABB);
var combinedArea = combinedAABB.Perimeter;
// Cost of creating a new parent for this node and the new leaf
var cost = 2.0f * combinedArea;
// Minimum cost of pushing the leaf further down the tree
var inheritanceCost = 2.0f * (combinedArea - area);
// Cost of descending into child1
var cost1 = CalculateSiblingChildCost(leaf, sibling.LeftChild, inheritanceCost);
var cost2 = CalculateSiblingChildCost(leaf, sibling.RightChild, inheritanceCost);
// Descend according to the minimum cost.
if (cost < cost1 && cost1 < cost2)
{
return(sibling);
}
// Descend
sibling = cost1 < cost2 ? sibling.LeftChild : sibling.RightChild;
}
return(sibling);
}
private DynamicTreeNode Balance(DynamicTreeNode node)
{
Debug.Assert(node != null);
if (node.IsLeaf || node.Depth < 2)
{
return(node);
}
var nodeB = node.LeftChild;
var nodeC = node.RightChild;
int balance = nodeC.Depth - nodeB.Depth;
// Rotate C up
if (balance > 1)
{
return(Rotate(node, false));
}
// Rotate B up
if (balance < -1)
{
return(Rotate(node, true));
}
return(node);
}
private void FreeNode(DynamicTreeNode node)
{
Debug.Assert(0 < count);
node.Free();
freeNodes.Enqueue(node);
--count;
}
internal void Free()
{
AABB = default(AABB);
Context = null;
Depth = -1;
Parent = LeftChild = RightChild = null;
}
public int ComputeDepth(DynamicTreeNode node)
{
if (node.IsLeaf)
{
return(0);
}
int height1 = ComputeDepth(node.LeftChild);
int height2 = ComputeDepth(node.RightChild);
return(1 + Math.Max(height1, height2));
}
private void InsertLeaf(DynamicTreeNode leaf)
{
if (root == null)
{
root = leaf;
root.Parent = null;
return;
}
// Find the best sibling for this node
var sibling = FindSibling(leaf);
// Create a new parent.
var oldParent = sibling.Parent;
var newParent = AllocateNode();
newParent.Initialize(leaf.AABB.Combine(sibling.AABB), null, sibling.Depth + 1);
newParent.Parent = oldParent;
if (oldParent != null)
{
// The sibling was not the root.
if (oldParent.LeftChild == sibling)
{
oldParent.LeftChild = newParent;
}
else
{
oldParent.RightChild = newParent;
}
newParent.LeftChild = sibling;
newParent.RightChild = leaf;
sibling.Parent = newParent;
leaf.Parent = newParent;
}
else
{
// The sibling was the root.
newParent.LeftChild = sibling;
newParent.RightChild = leaf;
sibling.Parent = newParent;
leaf.Parent = newParent;
root = newParent;
}
// Walk back up the tree fixing depths and AABBs
BalanceBranch(leaf.Parent);
#if DEBUG
Validate();
#endif
}
private float CalculateSiblingChildCost(DynamicTreeNode leaf, DynamicTreeNode child, float inheritanceCost)
{
var aabb = leaf.AABB.Combine(child.AABB);
if (child.IsLeaf)
{
return(aabb.Perimeter + inheritanceCost);
}
else
{
return((aabb.Perimeter - child.AABB.Perimeter) + inheritanceCost);
}
}
private void BalanceBranch(DynamicTreeNode node)
{
while (node != null)
{
node = Balance(node);
Debug.Assert(node.LeftChild != null);
Debug.Assert(node.RightChild != null);
node.Depth = 1 + Math.Max(node.LeftChild.Depth, node.RightChild.Depth);
node.AABB = node.LeftChild.AABB.Combine(node.RightChild.AABB);
node = node.Parent;
}
}
private DynamicTreeNode AllocateNode()
{
DynamicTreeNode node;
if (freeNodes.Count > 0)
{
node = freeNodes.Dequeue();
}
else
{
node = new DynamicTreeNode();
nodes.Add(node);
}
++count;
return(node);
}
private void RemoveLeaf(DynamicTreeNode leaf)
{
Debug.Assert(leaf.IsLeaf);
if (leaf == root)
{
root = null;
return;
}
var parent = leaf.Parent;
var grandParent = parent.Parent;
var sibling = parent.LeftChild == leaf ? parent.RightChild : parent.LeftChild;
if (grandParent != null)
{
// Destroy parent and connect sibling to grandParent.
if (grandParent.LeftChild == parent)
{
grandParent.LeftChild = sibling;
}
else
{
grandParent.RightChild = sibling;
}
sibling.Parent = grandParent;
FreeNode(parent);
// Adjust ancestor bounds.
BalanceBranch(grandParent);
}
else
{
root = sibling;
sibling.Parent = null;
FreeNode(parent);
}
#if DEBUG
Validate();
#endif
}
public bool MoveProxy(DynamicTreeNode node, AABB aabb, Vector2D displacement)
{
Debug.Assert(node.IsLeaf);
if (node.AABB.Contains(aabb))
{
return(false);
}
RemoveLeaf(node);
// Extend AABB.
aabb = aabb.Enlarge(new Vector2D(Configuration.FatAABBExtension, Configuration.FatAABBExtension));
// Predict AABB displacement.
displacement = Configuration.AABBDisplacementMultiplier * displacement;
if (displacement.X < 0.0f)
{
aabb.LowerBound.X += displacement.X;
}
else
{
aabb.UpperBound.X += displacement.X;
}
if (displacement.Y < 0.0f)
{
aabb.LowerBound.Y += displacement.Y;
}
else
{
aabb.UpperBound.Y += displacement.Y;
}
node.AABB = aabb;
InsertLeaf(node);
return(true);
}
private DynamicTreeNode Rotate(DynamicTreeNode parent, bool leftChild)
{
var child1 = leftChild ? parent.LeftChild : parent.RightChild;
var child2 = leftChild ? parent.RightChild : parent.LeftChild;
var grandChild1 = child1.LeftChild;
var grandChild2 = child1.RightChild;
// Swap A and C
child1.LeftChild = parent;
child1.Parent = parent.Parent;
parent.Parent = child1;
// A's old parent should point to C
if (child1.Parent != null)
{
if (child1.Parent.LeftChild == parent)
{
child1.Parent.LeftChild = child1;
}
else
{
Debug.Assert(child1.Parent.RightChild == parent);
child1.Parent.RightChild = child1;
}
}
else
{
root = child1;
}
// Rotate
if (grandChild1.Depth > grandChild2.Depth)
{
child1.RightChild = grandChild1;
if (leftChild)
{
parent.LeftChild = grandChild2;
}
else
{
parent.RightChild = grandChild2;
}
grandChild2.Parent = parent;
parent.AABB = child2.AABB.Combine(grandChild2.AABB);
child1.AABB = parent.AABB.Combine(grandChild1.AABB);
parent.Depth = 1 + Math.Max(child2.Depth, grandChild2.Depth);
child1.Depth = 1 + Math.Max(parent.Depth, grandChild1.Depth);
}
else
{
child1.RightChild = grandChild2;
if (leftChild)
{
parent.LeftChild = grandChild1;
}
else
{
parent.RightChild = grandChild1;
}
grandChild1.Parent = parent;
parent.AABB = child2.AABB.Combine(grandChild1.AABB);
child1.AABB = parent.AABB.Combine(grandChild2.AABB);
parent.Depth = 1 + Math.Max(child2.Depth, grandChild1.Depth);
child1.Depth = 1 + Math.Max(parent.Depth, grandChild2.Depth);
}
return(child1);
}
