/// <summary>
/// Adds the leaf node to the specified subtree.
/// </summary>
/// <param name="node">The root of the subtree.</param>
/// <param name="leaf">The leaf node.</param>
private void AddLeaf(Node node, Node leaf)
{
Debug.Assert(!node.IsLeaf);
if (node.IsValid)
{
// Choose closest node among the children.
int selection = AabbTreeHelper.SelectClosest(leaf.Aabb, node.LeftChild.Aabb, node.RightChild.Aabb);
if (selection == 0)
{
// Insert into left subtree.
if (node.LeftChild.IsLeaf)
{
// Invalidate the node above the leaf node. Insert the leaf node (see below) and
// re-split in next collision detection query as needed.
InvalidateSubtree(node);
}
else
{
// Recursively add leaf node to subtree.
AddLeaf(node.LeftChild, leaf);
}
}
else
{
// Insert into right subtree.
if (node.RightChild.IsLeaf)
{
// Invalidate the node above the leaf node. Insert the leaf node (see below) and
// re-split in next collision detection query as needed.
InvalidateSubtree(node);
}
else
{
// Recursively add leaf node to subtree.
AddLeaf(node.RightChild, leaf);
}
}
}
if (!node.IsValid)
{
// All nodes are stored locally.
node.Leaves.Add(leaf);
}
RecomputeAabb(node);
}
/// <summary>
/// Adds a leaf node to the given tree.
/// </summary>
/// <param name="root">The root of the tree (or subtree).</param>
/// <param name="leaf">The leaf node to be added.</param>
private void AddLeaf(Node root, Node leaf)
{
if (_root == null)
{
// Insert leaf as new root.
_root = leaf;
}
else
{
// Search for leaf node that is closest to 'leaf'. This node that will become the new
// sibling of 'leaf'.
Node sibling = root;
while (!sibling.IsLeaf)
{
int selection = AabbTreeHelper.SelectClosest(leaf.Aabb, sibling.LeftChild.Aabb, sibling.RightChild.Aabb);
sibling = (selection == 0) ? sibling.LeftChild : sibling.RightChild;
}
// Add a new node as the parent of (sibling, leaf).
Node parent = sibling.Parent;
Node node = Nodes.Obtain();
node.Aabb = Aabb.Merge(sibling.Aabb, leaf.Aabb);
node.Parent = parent;
node.LeftChild = sibling;
node.RightChild = leaf;
sibling.Parent = node;
leaf.Parent = node;
if (parent == null)
{
// node is the new root.
_root = node;
}
else
{
// node is an internal node.
if (parent.LeftChild == sibling)
{
parent.LeftChild = node;
}
else
{
parent.RightChild = node;
}
// Update AABBs of ancestor.
do
{
if (!parent.Aabb.Contains(node.Aabb))
{
parent.Aabb = Aabb.Merge(parent.LeftChild.Aabb, parent.RightChild.Aabb);
}
else
{
break;
}
node = parent;
parent = parent.Parent;
} while (parent != null);
}
}
}
