// The following basic top-down approach is used in Bullet (btDbvt).
/// <summary>
/// Builds the subtree top-down.
/// </summary>
/// <typeparam name="T">The type of item stored in the tree.</typeparam>
/// <param name="leaves">The leaves of the tree.</param>
/// <param name="firstLeaf">The first leaf.</param>
/// <param name="lastLeaf">The last leaf.</param>
/// <param name="createNode">A function that creates a new, empty node.</param>
/// <returns>The root node of the subtree.</returns>
private static IAabbTreeNode <T> BuildBottomUp <T>(List <IAabbTreeNode <T> > leaves, int firstLeaf, int lastLeaf, Func <IAabbTreeNode <T> > createNode)
{
// Replace 'leaves' with new list we can work with.
var nodes = DigitalRune.ResourcePools <IAabbTreeNode <T> > .Lists.Obtain();
for (int i = firstLeaf; i <= lastLeaf; i++)
{
nodes.Add(leaves[i]);
}
// Iteratively merge nodes (subtrees) until we have a single tree.
while (nodes.Count > 1)
{
float minSize = float.PositiveInfinity;
Pair <int> minPair = new Pair <int>(-1, -1);
for (int i = 0; i < nodes.Count; i++)
{
// Compare node with all subsequent nodes in list.
for (int j = i + 1; j < nodes.Count; j++)
{
Aabb mergedAabb = Aabb.Merge(nodes[i].Aabb, nodes[j].Aabb);
// Compute a "size" which can be used to estimate the fit of the new node.
// Here: volume + edges
Vector3F edges = mergedAabb.Extent;
float size = edges.X * edges.Y * edges.Z + edges.X + edges.Y + edges.Z;
if (size <= minSize) // Note: We compare with ≤ because size can be ∞.
{
minSize = size;
minPair.First = i;
minPair.Second = j;
}
}
}
if (minPair.First < 0 || minPair.Second < 0)
{
throw new GeometryException("Could not build AABB tree because the AABB of an item is invalid (e.g. NaN).");
}
// Create a new parent node that merges the two subtrees.
IAabbTreeNode <T> leftChild = nodes[minPair.First];
IAabbTreeNode <T> rightChild = nodes[minPair.Second];
IAabbTreeNode <T> parent = createNode();
parent.Aabb = Aabb.Merge(leftChild.Aabb, rightChild.Aabb);
parent.LeftChild = leftChild;
parent.RightChild = rightChild;
// Remove subtrees from list and add the new node.
nodes.RemoveAt(minPair.Second);
nodes.RemoveAt(minPair.First);
nodes.Add(parent);
}
IAabbTreeNode <T> root = nodes[0];
DigitalRune.ResourcePools <IAabbTreeNode <T> > .Lists.Recycle(nodes);
return(root);
}
/// <summary>
/// Sorts the leaves such that objects of the left subtree come first.
/// </summary>
/// <typeparam name="T">The type of item stored in the AABB tree.</typeparam>
/// <param name="leaves">The leaves.</param>
/// <param name="firstLeaf">The index of the first leaf.</param>
/// <param name="lastLeaf">The index of the last leaf.</param>
/// <param name="splitAxis">The index of the split axis.</param>
/// <param name="splitValue">The split value.</param>
/// <param name="rightLeaf">The index of the first leaf of the right subtree.</param>
private static void SortLeaves <T>(List <IAabbTreeNode <T> > leaves, int firstLeaf, int lastLeaf, int splitAxis, float splitValue, out int rightLeaf)
{
int unhandledLeaf = firstLeaf; // Leaf index of first untested leaf.
rightLeaf = lastLeaf + 1;
// Go through leaves for this subtree and sort the indices such that
// first are objects in the left half and then all objects in the right half.
while (unhandledLeaf < rightLeaf)
{
if (leaves[unhandledLeaf].Aabb.Center[splitAxis] <= splitValue)
{
// Object of leaf is in left half. We can test the next.
unhandledLeaf++;
}
else
{
// Object of leaf is in right half. Swap with a leaf at end.
rightLeaf--;
IAabbTreeNode <T> dummy = leaves[unhandledLeaf];
leaves[unhandledLeaf] = leaves[rightLeaf];
leaves[rightLeaf] = dummy;
}
}
}
private static IAabbTreeNode <T> BuildTopDownVarianceBasedSplit <T>(List <IAabbTreeNode <T> > leaves, int firstLeaf, int lastLeaf, Func <IAabbTreeNode <T> > createNode)
{
int numberOfNodes = lastLeaf - firstLeaf + 1;
if (numberOfNodes == 1)
{
return(leaves[firstLeaf]);
}
// Compute mean of AABB centers.
Vector3F mean = new Vector3F();
for (int i = firstLeaf; i <= lastLeaf; i++)
{
mean += leaves[i].Aabb.Center;
}
mean /= numberOfNodes;
// Compute variance of AABB centers.
Vector3F variance = new Vector3F();
for (int i = firstLeaf; i <= lastLeaf; i++)
{
Vector3F difference = leaves[i].Aabb.Center - mean;
variance += difference * difference;
}
variance /= numberOfNodes;
// Choose axis of max variance as split axis.
int splitAxis = variance.IndexOfLargestComponent;
float splitValue = mean[splitAxis];
IAabbTreeNode <T> node = createNode();
node.Aabb = MergeLeaveAabbs(leaves, firstLeaf, lastLeaf);
// Sort indices in list.
int rightLeaf; // Leaf index where the right tree begins.
SortLeaves(leaves, firstLeaf, lastLeaf, splitAxis, splitValue, out rightLeaf);
// Avoid unbalanced trees. (Unbalanced trees could cause stack overflows.)
int minNodesPerSubtree = numberOfNodes / 3;
if (rightLeaf == firstLeaf || // Left subtree is empty.
rightLeaf > lastLeaf || // Right subtree is empty.
firstLeaf + minNodesPerSubtree >= rightLeaf || // Not enough nodes in right subtree.
rightLeaf + minNodesPerSubtree >= lastLeaf) // Not enough nodes in left subtree.
{
// Evenly distribute nodes among subtree.
rightLeaf = (firstLeaf + lastLeaf + 1) / 2;
}
// Build subtrees.
node.LeftChild = BuildTopDownVarianceBasedSplit(leaves, firstLeaf, rightLeaf - 1, createNode);
node.RightChild = BuildTopDownVarianceBasedSplit(leaves, rightLeaf, lastLeaf, createNode);
return(node);
}
/// <summary>
/// Gets the direct children of the given node.
/// </summary>
/// <typeparam name="T">The type of item stored in the tree.</typeparam>
/// <param name="node">The node.</param>
/// <returns>The direct children of <paramref name="node"/>.</returns>
/// <exception cref="ArgumentNullException">
/// <paramref name="node"/> is <see langword="null"/>.
/// </exception>
public static IEnumerable <IAabbTreeNode <T> > GetChildren <T>(this IAabbTreeNode <T> node)
{
if (node == null)
{
throw new ArgumentNullException("node");
}
return(GetChildrenImpl(node));
}
private static IEnumerable <IAabbTreeNode <T> > GetChildrenImpl <T>(IAabbTreeNode <T> node)
{
if (node.IsLeaf)
{
yield break;
}
yield return(node.LeftChild);
yield return(node.RightChild);
}
private static IAabbTreeNode <T> BuildTopDownCenterSplit <T>(List <IAabbTreeNode <T> > leaves, int firstLeaf, int lastLeaf, Func <IAabbTreeNode <T> > createNode)
{
int numberOfNodes = lastLeaf - firstLeaf + 1;
if (numberOfNodes == 1)
{
return(leaves[firstLeaf]);
}
IAabbTreeNode <T> node = createNode();
node.Aabb = MergeLeaveAabbs(leaves, firstLeaf, lastLeaf);
// Get max axis.
int splitAxis = node.Aabb.Extent.IndexOfLargestComponent;
// Split at center of AABB.
float splitValue = node.Aabb.Center[splitAxis];
// Sort indices in list.
int rightLeaf; // Leaf index where the right tree begins.
SortLeaves(leaves, firstLeaf, lastLeaf, splitAxis, splitValue, out rightLeaf);
// If one subtree is empty, we create two equal trees.
if (rightLeaf == firstLeaf || rightLeaf > lastLeaf)
{
rightLeaf = (firstLeaf + lastLeaf + 1) / 2;
}
if (rightLeaf == firstLeaf + 1)
{
// Left child is leaf.
node.LeftChild = leaves[firstLeaf];
}
else
{
// Build left subtree.
node.LeftChild = BuildTopDownCenterSplit(leaves, firstLeaf, rightLeaf - 1, createNode);
}
if (rightLeaf == lastLeaf)
{
// Right child is leaf.
node.RightChild = leaves[rightLeaf];
}
else
{
// Build right subtree.
node.RightChild = BuildTopDownCenterSplit(leaves, rightLeaf, lastLeaf, createNode);
}
return(node);
}
/// <overloads>
/// <summary>
/// Gets the descendants of a given node.
/// </summary>
/// </overloads>
///
/// <summary>
/// Gets the descendants of the given node using a depth-first search.
/// </summary>
/// <typeparam name="T">The type of item stored in the tree.</typeparam>
/// <param name="node">The node.</param>
/// <returns>The descendants of <paramref name="node"/>.</returns>
/// <exception cref="ArgumentNullException">
/// <paramref name="node"/> is <see langword="null"/>.
/// </exception>
/// <remarks>
/// This method can be used to traverse a tree in depth-first order (pre-order).
/// </remarks>
public static IEnumerable <IAabbTreeNode <T> > GetDescendants <T>(this IAabbTreeNode <T> node)
{
return(TreeHelper.GetDescendants(node, GetChildrenImpl));
}
/// <summary>
/// Gets the ancestors of the given node.
/// </summary>
/// <typeparam name="T">The type of item stored in the tree.</typeparam>
/// <param name="node">The node.</param>
/// <returns>
/// The ancestors of <paramref name="node"/> starting with the direct parent of
/// <paramref name="node"/> going upwards to the root of the tree.
/// </returns>
/// <exception cref="ArgumentNullException">
/// <paramref name="node"/> is <see langword="null"/>.
/// </exception>
public static IEnumerable <IAabbTreeNode <T> > GetAncestors <T>(this IAabbTreeNode <T> node)
{
return(TreeHelper.GetAncestors(node, n => n.Parent));
}
/// <summary>
/// Gets the height of the given tree or subtree.
/// </summary>
/// <typeparam name="T">The type of item stored in the tree.</typeparam>
/// <param name="node">The tree.</param>
/// <returns>The height of the tree.</returns>
/// <remarks>
/// The height of the tree is the length of the longest downward path to a leaf. Therefore, a
/// leaf node has a height of 0.
/// </remarks>
/// <exception cref="ArgumentNullException">
/// <paramref name="node"/> is <see langword="null"/>.
/// </exception>
public static int GetHeight <T>(this IAabbTreeNode <T> node)
{
return(TreeHelper.GetHeight(node, GetChildrenImpl));
}
/// <summary>
/// Gets the depth of the given node.
/// </summary>
/// <typeparam name="T">The type of item stored in the tree.</typeparam>
/// <param name="node">The node.</param>
/// <returns>The depth of the node.</returns>
/// <remarks>
/// The depth of a node is the length of the longest upward path to the root. Therefore, a root
/// node has a depth of 0.
/// </remarks>
/// <exception cref="ArgumentNullException">
/// <paramref name="node"/> is <see langword="null"/>.
/// </exception>
public static int GetDepth <T>(this IAabbTreeNode <T> node)
{
return(TreeHelper.GetDepth(node, n => node.Parent));
}
/// <summary>
/// Gets the subtree (the given node and all of its descendants) using a depth-first search or a
/// breadth-first search.
/// </summary>
/// <typeparam name="T">The type of item stored in the tree.</typeparam>
/// <param name="node">
/// The reference node where to start the search. (The reference node will be the first
/// element in the enumeration.)
/// </param>
/// <param name="depthFirst">
/// If set to <see langword="true"/> then a depth-first search for descendants will be made;
/// otherwise a breadth-first search will be made.
/// </param>
/// <returns>The descendants of <paramref name="node"/>.</returns>
/// <exception cref="ArgumentNullException">
/// <paramref name="node"/> is <see langword="null"/>.
/// </exception>
/// <remarks>
/// This method can be used to traverse a tree in either depth-first order (pre-order) or in
/// breadth-first order (also known as level-order).
/// </remarks>
public static IEnumerable <IAabbTreeNode <T> > GetSubtree <T>(this IAabbTreeNode <T> node, bool depthFirst)
{
return(TreeHelper.GetSubtree(node, GetChildrenImpl, depthFirst));
}
private static IAabbTreeNode <T> BuildMixed <T>(List <IAabbTreeNode <T> > leaves, int firstLeaf, int lastLeaf, Func <IAabbTreeNode <T> > createNode, int bottomUpThreshold)
{
int numberOfNodes = lastLeaf - firstLeaf + 1;
if (numberOfNodes == 1)
{
return(leaves[firstLeaf]);
}
if (numberOfNodes <= bottomUpThreshold)
{
return(BuildBottomUp(leaves, firstLeaf, lastLeaf, createNode));
}
IAabbTreeNode <T> node = createNode();
node.Aabb = MergeLeaveAabbs(leaves, firstLeaf, lastLeaf);
Vector3F center = node.Aabb.Center;
// Check which split yields the most balanced tree.
int[,] splitCount = new int[3, 2]; // splitCount[number of axis, left or right]
for (int i = firstLeaf; i <= lastLeaf; i++)
{
Vector3F offset = leaves[i].Aabb.Center - center;
for (int axis = 0; axis < 3; axis++)
{
if (offset[axis] <= 0)
{
splitCount[axis, 0]++;
}
else
{
splitCount[axis, 1]++;
}
}
}
int minDifference = numberOfNodes;
int splitAxis = -1;
float splitValue = 0;
for (int axis = 0; axis < 3; axis++)
{
int leftCount = splitCount[axis, 0];
int rightCount = splitCount[axis, 1];
if (leftCount > 0 && rightCount > 0)
{
int difference = Math.Abs(leftCount - rightCount);
if (difference < minDifference)
{
minDifference = difference;
splitAxis = axis;
splitValue = center[axis];
}
}
}
int rightLeaf; // Leaf index where the right tree begins.
if (splitAxis >= 0)
{
// Sort indices in list.
SortLeaves(leaves, firstLeaf, lastLeaf, splitAxis, splitValue, out rightLeaf);
}
else
{
// Evenly distribute nodes among subtree.
rightLeaf = (firstLeaf + lastLeaf + 1) / 2;
}
// Build subtrees.
node.LeftChild = BuildMixed(leaves, firstLeaf, rightLeaf - 1, createNode, bottomUpThreshold);
node.RightChild = BuildMixed(leaves, rightLeaf, lastLeaf, createNode, bottomUpThreshold);
return(node);
}
