public List <IAABB> QueryOverlaps(IAABB obj)
{
List <IAABB> overlaps = new List <IAABB>();
Stack <int> stack = new Stack <int>();
AABB3d testAabb = obj.GetAABBBounds();
stack.Push(m_rootNodeIndex);
while (stack.Count != 0)
{
int nodeIndex = stack.Pop();
if (nodeIndex == NullIndex)
{
continue;
}
AABBTreeNode node = m_nodes[nodeIndex];
if (node.m_bounds.Overlaps(testAabb))
{
if (node.IsLeaf() && node.m_object != obj)
{
overlaps.Insert(0, node.m_object);
}
else
{
stack.Push(node.m_leftNodeIndex);
stack.Push(node.m_rightNodeIndex);
}
}
}
return(overlaps);
}
void InsertLeaf(int leafNodeIndex)
{
// make sure we're inserting a new leaf
DebugHelper.Assert(m_nodes[leafNodeIndex].m_parentNodeIndex == NullIndex);
DebugHelper.Assert(m_nodes[leafNodeIndex].m_leftNodeIndex == NullIndex);
DebugHelper.Assert(m_nodes[leafNodeIndex].m_rightNodeIndex == NullIndex);
// if the tree is empty then we make the root the leaf
if (m_rootNodeIndex == NullIndex)
{
m_rootNodeIndex = leafNodeIndex;
return;
}
// search for the best place to put the new leaf in the tree
// we use surface area and depth as search heuristics
int treeNodeIndex = m_rootNodeIndex;
AABBTreeNode leafNode = m_nodes[leafNodeIndex];
while (!m_nodes[treeNodeIndex].IsLeaf())
{
// because of the test in the while loop above we know we are never a leaf inside it
AABBTreeNode treeNode = m_nodes[treeNodeIndex];
int leftNodeIndex = treeNode.m_leftNodeIndex;
int rightNodeIndex = treeNode.m_rightNodeIndex;
AABBTreeNode leftNode = m_nodes[leftNodeIndex];
AABBTreeNode rightNode = m_nodes[rightNodeIndex];
AABB3d combinedAabb = treeNode.m_bounds.Merge(leafNode.m_bounds);
FloatL newParentNodeCost = 2.0f * combinedAabb.CalculateSurfaceArea();
FloatL minimumPushDownCost = 2.0f * (combinedAabb.CalculateSurfaceArea() - treeNode.m_bounds.CalculateSurfaceArea());
// use the costs to figure out whether to create a new parent here or descend
FloatL costLeft;
FloatL costRight;
if (leftNode.IsLeaf())
{
costLeft = leafNode.m_bounds.Merge(leftNode.m_bounds).CalculateSurfaceArea() + minimumPushDownCost;
}
else
{
AABB3d newLeftAabb = leafNode.m_bounds.Merge(leftNode.m_bounds);
costLeft = (newLeftAabb.CalculateSurfaceArea() - leftNode.m_bounds.CalculateSurfaceArea()) + minimumPushDownCost;
}
if (rightNode.IsLeaf())
{
costRight = leafNode.m_bounds.Merge(rightNode.m_bounds).CalculateSurfaceArea() + minimumPushDownCost;
}
else
{
AABB3d newRightAabb = leafNode.m_bounds.Merge(rightNode.m_bounds);
costRight = (newRightAabb.CalculateSurfaceArea() - rightNode.m_bounds.CalculateSurfaceArea()) + minimumPushDownCost;
}
// if the cost of creating a new parent node here is less than descending in either direction then
// we know we need to create a new parent node, errrr, here and attach the leaf to that
if (newParentNodeCost < costLeft && newParentNodeCost < costRight)
{
break;
}
// otherwise descend in the cheapest direction
if (costLeft < costRight)
{
treeNodeIndex = leftNodeIndex;
}
else
{
treeNodeIndex = rightNodeIndex;
}
}
// the leafs sibling is going to be the node we found above and we are going to create a new
// parent node and attach the leaf and this item
int leafSiblingIndex = treeNodeIndex;
AABBTreeNode leafSibling = m_nodes[leafSiblingIndex];
int oldParentIndex = leafSibling.m_parentNodeIndex;
int newParentIndex = AllocateNode();
AABBTreeNode newParent = m_nodes[newParentIndex];
newParent.m_parentNodeIndex = oldParentIndex;
newParent.m_bounds = leafNode.m_bounds.Merge(leafSibling.m_bounds); // the new parents aabb is the leaf aabb combined with it's siblings aabb
newParent.m_leftNodeIndex = leafSiblingIndex;
newParent.m_rightNodeIndex = leafNodeIndex;
leafNode.m_parentNodeIndex = newParentIndex;
leafSibling.m_parentNodeIndex = newParentIndex;
if (oldParentIndex == NullIndex)
{
// the old parent was the root and so this is now the root
m_rootNodeIndex = newParentIndex;
}
else
{
// the old parent was not the root and so we need to patch the left or right index to
// point to the new node
AABBTreeNode oldParent = m_nodes[oldParentIndex];
if (oldParent.m_leftNodeIndex == leafSiblingIndex)
{
oldParent.m_leftNodeIndex = newParentIndex;
}
else
{
oldParent.m_rightNodeIndex = newParentIndex;
}
}
// finally we need to walk back up the tree fixing heights and areas
treeNodeIndex = leafNode.m_parentNodeIndex;
FixUpwardsTree(treeNodeIndex);
}
