public void Query(FPVector origin, FPVector direction, List <int> collisions)
{
Stack <int> stack = stackPool.GetNew();
stack.Push(_root);
while (stack.Count > 0)
{
int nodeId = stack.Pop();
DynamicTreeNode <T> node = _nodes[nodeId];
if (node.AABB.RayIntersect(ref origin, ref direction))
{
if (node.IsLeaf())
{
collisions.Add(nodeId);
}
else
{
if (_nodes[node.Child1].AABB.RayIntersect(ref origin, ref direction))
{
stack.Push(node.Child1);
}
if (_nodes[node.Child2].AABB.RayIntersect(ref origin, ref direction))
{
stack.Push(node.Child2);
}
}
}
}
stackPool.GiveBack(stack);
}
private int ComputeHeight(int nodeId)
{
if (nodeId == NullNode)
{
return(0);
}
Debug.Assert(0 <= nodeId && nodeId < _nodeCapacity);
DynamicTreeNode <T> node = _nodes[nodeId];
int height1 = ComputeHeight(node.Child1);
int height2 = ComputeHeight(node.Child2);
return(1 + Math.Max(height1, height2));
}
/// <summary>
/// Query an AABB for overlapping proxies. The callback class
/// is called for each proxy that overlaps the supplied AABB.
/// </summary>
/// <param name="callback">The callback.</param>
/// <param name="aabb">The aabb.</param>
public void Query(List <int> my, ref FPBBox aabb)
{
//Stack<int> _stack = new Stack<int>(256);
Stack <int> _stack = stackPool.GetNew();
_stack.Push(_root);
while (_stack.Count > 0)
{
int nodeId = _stack.Pop();
if (nodeId == NullNode)
{
continue;
}
DynamicTreeNode <T> node = _nodes[nodeId];
//if (JBBox.TestOverlap(ref node.AABB, ref aabb))
if (aabb.Contains(ref node.AABB) != FPBBox.ContainmentType.Disjoint)
{
if (node.IsLeaf())
{
my.Add(nodeId);
//bool proceed = callback(nodeId);
//if (proceed == false)
//{
// return;
//}
}
else
{
_stack.Push(node.Child1);
_stack.Push(node.Child2);
}
}
}
stackPool.GiveBack(_stack);
}
private int CountLeaves(int nodeId)
{
if (nodeId == NullNode)
{
return(0);
}
Debug.Assert(0 <= nodeId && nodeId < _nodeCapacity);
DynamicTreeNode <T> node = _nodes[nodeId];
if (node.IsLeaf())
{
Debug.Assert(node.LeafCount == 1);
return(1);
}
int count1 = CountLeaves(node.Child1);
int count2 = CountLeaves(node.Child2);
int count = count1 + count2;
Debug.Assert(count == node.LeafCount);
return(count);
}