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); }