/// Constructing the tree initializes the node pool.
public DynamicTree()
{
_root = NullNode;
_nodeCapacity = 16;
_nodeCount = 0;
_nodes = new DynamicTreeNode[_nodeCapacity];
//Fill the array with nodes:
for (int i = 0; i < _nodeCapacity; ++i)
{
_nodes[i] = new DynamicTreeNode();
}
// Build a linked list for the free list. The parent
// pointer becomes the "next" pointer.
for (int i = 0; i < _nodeCapacity - 1; ++i)
{
_nodes[i].Next = i + 1;
}
_nodes[_nodeCapacity - 1].Next = NullNode;
_freeList = 0;
_insertionCount = 0;
}
/// Constructing the tree initializes the node pool.
public DynamicTree()
{
_root = NullNode;
_nodeCapacity = 16;
_nodeCount = 0;
_nodes = new DynamicTreeNode[_nodeCapacity];
//Fill the array with nodes:
for (int i = 0; i < _nodeCapacity; ++i)
{
_nodes[i] = new DynamicTreeNode();
}
// Build a linked list for the free list. The parent
// pointer becomes the "next" pointer.
for (int i = 0; i < _nodeCapacity - 1; ++i)
{
_nodes[i].Next = i + 1;
}
_nodes[_nodeCapacity - 1].Next = NullNode;
_freeList = 0;
_insertionCount = 0;
}
//TODO: uncomment
//public void Rebalance(int iterations)
//{
// if (_root == NullNode)
// {
// return;
// }
// for (int i = 0; i < iterations; ++i)
// {
// int node = _root;
// uint bit = 0;
// while (_nodes[node].IsLeaf() == false)
// {
// int children = _nodes[node].Child1;
// node = children[(_path >> bit) & 1];
// bit = (bit + 1) & (8 * sizeof(uint) - 1);
// }
// ++_path;
// RemoveLeaf(node);
// InsertLeaf(node);
// }
//}
// Compute the height of a sub-tree.
public int ComputeHeight(int nodeId)
{
if (nodeId == NullNode)
{
return(0);
}
Box2DXDebug.Assert(0 <= nodeId && nodeId < _nodeCapacity);
DynamicTreeNode node = _nodes[nodeId];
int height1 = ComputeHeight(node.Child1);
int height2 = ComputeHeight(node.Child2);
return(1 + Math.Max(height1, height2));
}
/// Query an AABB for overlapping proxies. The callback class
/// is called for each proxy that overlaps the supplied AABB.
public void Query(IQueryEnabled callback, AABB aabb)
{
const int k_stackSize = 128;
int[] stack = new int[k_stackSize];
int count = 0;
stack[count++] = _root;
while (count > 0)
{
int nodeId = stack[--count];
if (nodeId == NullNode)
{
continue;
}
DynamicTreeNode node = _nodes[nodeId];
if (Collision.TestOverlap(node.Aabb, aabb))
{
if (node.IsLeaf())
{
bool proceed = callback.QueryCallback(nodeId);
if (proceed == false)
{
return;
}
}
else
{
Box2DXDebug.Assert(count + 1 < k_stackSize);
stack[count++] = node.Child1;
stack[count++] = node.Child2;
}
}
}
}
private int AllocateNode()
{
// Expand the node pool as needed.
if (_freeList == NullNode)
{
Box2DXDebug.Assert(_nodeCount == _nodeCapacity);
// The free list is empty. Rebuild a bigger pool.
_nodeCapacity *= 2;
Array.Resize(ref _nodes, _nodeCapacity);
//Fill array with nodes
for (int i = _nodeCount; i < _nodeCapacity; i++)
{
_nodes[i] = new DynamicTreeNode();
}
// Build a linked list for the free list. The parent
// pointer becomes the "next" pointer.
for (int i = _nodeCount; i < _nodeCapacity - 1; ++i)
{
_nodes[i].Next = i + 1;
}
_nodes[_nodeCapacity - 1].Next = NullNode;
_freeList = _nodeCount;
}
// Peel a node off the free list.
int nodeId = _freeList;
_freeList = _nodes[nodeId].Next;
_nodes[nodeId].Parent = NullNode;
_nodes[nodeId].Child1 = NullNode;
_nodes[nodeId].Child2 = NullNode;
++_nodeCount;
return(nodeId);
}
/// Ray-cast against the proxies in the tree. This relies on the callback
/// to perform a exact ray-cast in the case were the proxy contains a shape.
/// The callback also performs the any collision filtering. This has performance
/// roughly equal to k * log(n), where k is the number of collisions and n is the
/// number of proxies in the tree.
/// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
/// @param callback a callback class that is called for each proxy that is hit by the ray.
public void RayCast(IRayCastEnabled callback, RayCastInput input)
{
Vec2 p1 = input.P1;
Vec2 p2 = input.P2;
Vec2 r = p2 - p1;
Box2DXDebug.Assert(r.LengthSquared() > 0.0f);
r.Normalize();
// v is perpendicular to the segment.
Vec2 v = Vec2.Cross(1.0f, r);
Vec2 abs_v = Math.Abs(v);
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
float maxFraction = input.MaxFraction;
// Build a bounding box for the segment.
AABB segmentAABB = new AABB();
{
Vec2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.LowerBound = Math.Min(p1, t);
segmentAABB.UpperBound = Math.Max(p1, t);
}
const int k_stackSize = 128;
int[] stack = new int[k_stackSize];
int count = 0;
stack[count++] = _root;
while (count > 0)
{
int nodeId = stack[--count];
if (nodeId == NullNode)
{
continue;
}
DynamicTreeNode node = _nodes[nodeId];
if (Collision.TestOverlap(node.Aabb, segmentAABB) == false)
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
Vec2 c = node.Aabb.GetCenter();
Vec2 h = node.Aabb.GetExtents();
float separation = Math.Abs(Vec2.Dot(v, p1 - c)) - Vec2.Dot(abs_v, h);
if (separation > 0.0f)
{
continue;
}
if (node.IsLeaf())
{
RayCastInput subInput = new RayCastInput();
subInput.P1 = input.P1;
subInput.P2 = input.P2;
subInput.MaxFraction = maxFraction;
maxFraction = callback.RayCastCallback(subInput, nodeId);
if (maxFraction == 0.0f)
{
return;
}
// Update segment bounding box.
{
Vec2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.LowerBound = Math.Min(p1, t);
segmentAABB.UpperBound = Math.Max(p1, t);
}
}
else
{
Box2DXDebug.Assert(count + 1 < k_stackSize);
stack[count++] = node.Child1;
stack[count++] = node.Child2;
}
}
}
private int AllocateNode()
{
// Expand the node pool as needed.
if (_freeList == NullNode)
{
Box2DXDebug.Assert(_nodeCount == _nodeCapacity);
// The free list is empty. Rebuild a bigger pool.
_nodeCapacity *= 2;
Array.Resize(ref _nodes, _nodeCapacity);
//Fill array with nodes
for (int i = _nodeCount; i < _nodeCapacity; i++)
{
_nodes[i] = new DynamicTreeNode();
}
// Build a linked list for the free list. The parent
// pointer becomes the "next" pointer.
for (int i = _nodeCount; i < _nodeCapacity - 1; ++i)
{
_nodes[i].Next = i + 1;
}
_nodes[_nodeCapacity - 1].Next = NullNode;
_freeList = _nodeCount;
}
// Peel a node off the free list.
int nodeId = _freeList;
_freeList = _nodes[nodeId].Next;
_nodes[nodeId].Parent = NullNode;
_nodes[nodeId].Child1 = NullNode;
_nodes[nodeId].Child2 = NullNode;
++_nodeCount;
return nodeId;
}
