public List <IAABB> QueryOverlaps(IAABB obj)
{
List <IAABB> overlaps = new List <IAABB>();
Stack <int?> stack = new Stack <int?>();
AABB testAabb = obj.GetAABB();
stack.Push(RootNodeIndex);
while (stack.Any())
{
int?nodeIndex = stack.Peek();
stack.Pop();
if (!nodeIndex.HasValue)
{
continue;
}
AABBNode node = Nodes[nodeIndex.Value].Clone() as AABBNode;
if (node.aabb.Intersect(testAabb))
{
if (node.IsLeaf() && node.Obj != obj)
{
overlaps.Add((IAABB)node.Obj);
}
else
{
stack.Push(node.LeftNodeIndex);
stack.Push(node.RightNodeIndex);
}
}
}
return(overlaps);
}
private void InsertLeaf(int leafNodeIndex)
{
if (RootNodeIndex == null)
{
RootNodeIndex = leafNodeIndex;
return;
}
int treeNodeIndex = RootNodeIndex.Value;
AABBNode leafNode = Nodes[leafNodeIndex];
while (!Nodes[treeNodeIndex].IsLeaf())
{
AABBNode treeNode = Nodes[treeNodeIndex].Clone() as AABBNode;
int leftNodeIndex = treeNode.LeftNodeIndex.Value;
int rightNodeIndex = treeNode.RightNodeIndex.Value;
AABBNode leftNode = Nodes[leftNodeIndex].Clone() as AABBNode;
AABBNode rightNode = Nodes[rightNodeIndex].Clone() as AABBNode;
AABB combinedAabb = treeNode.aabb.Merge(leafNode.aabb);
double newParentNodeCost = 2.0 * combinedAabb.SurfaceArea;
double minimumPushDownCost = 2.0 * (combinedAabb.SurfaceArea - treeNode.aabb.SurfaceArea);
double costLeft;
double costRight;
if (leftNode.IsLeaf())
{
costLeft = leafNode.aabb.Merge(leftNode.aabb).SurfaceArea + minimumPushDownCost;
}
else
{
AABB newLeftAabb = leafNode.aabb.Merge(leftNode.aabb);
costLeft = (newLeftAabb.SurfaceArea - leftNode.aabb.SurfaceArea) + minimumPushDownCost;
}
if (rightNode.IsLeaf())
{
costRight = leafNode.aabb.Merge(rightNode.aabb).SurfaceArea + minimumPushDownCost;
}
else
{
AABB newRightAabb = leafNode.aabb.Merge(rightNode.aabb);
costRight = (newRightAabb.SurfaceArea - rightNode.aabb.SurfaceArea) + minimumPushDownCost;
}
if (newParentNodeCost < costRight)
{
treeNodeIndex = leftNodeIndex;
}
else
{
treeNodeIndex = rightNodeIndex;
}
}
int leafSiblingIndex = treeNodeIndex;
AABBNode leafSibling = Nodes[leafSiblingIndex];
int? oldParentIndex = leafSibling.ParentNodeIndex;
int newParentIndex = AllocateNode();
AABBNode newParent = Nodes[newParentIndex];
newParent.ParentNodeIndex = oldParentIndex;
newParent.aabb = leafNode.aabb.Merge(leafSibling.aabb);
newParent.LeftNodeIndex = leafSiblingIndex;
newParent.RightNodeIndex = leafNodeIndex;
leafNode.ParentNodeIndex = newParentIndex;
leafSibling.ParentNodeIndex = newParentIndex;
if (oldParentIndex == null)
{
RootNodeIndex = newParentIndex;
}
else
{
AABBNode oldParent = Nodes[oldParentIndex.Value];
if (oldParent.LeftNodeIndex == leafSiblingIndex)
{
oldParent.LeftNodeIndex = newParentIndex;
}
else
{
oldParent.RightNodeIndex = newParentIndex;
}
}
treeNodeIndex = leafNode.ParentNodeIndex.Value;
FixUpwardsTree(treeNodeIndex);
}