/// 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;
}
}
}
public void InsertLeaf(int leaf)
{
++_insertionCount;
if (_root == NullNode)
{
_root = leaf;
_nodes[_root].Parent = NullNode;
return;
}
// Find the best sibling for this node.
Vec2 center = _nodes[leaf].Aabb.GetCenter();
int sibling = _root;
if (_nodes[sibling].IsLeaf() == false)
{
do
{
int child1 = _nodes[sibling].Child1;
int child2 = _nodes[sibling].Child2;
Vec2 delta1 = Math.Abs(_nodes[child1].Aabb.GetCenter() - center);
Vec2 delta2 = Math.Abs(_nodes[child2].Aabb.GetCenter() - center);
float norm1 = delta1.X + delta1.Y;
float norm2 = delta2.X + delta2.Y;
if (norm1 < norm2)
{
sibling = child1;
}
else
{
sibling = child2;
}
}while (_nodes[sibling].IsLeaf() == false);
}
// Create a parent for the siblings.
int node1 = _nodes[sibling].Parent;
int node2 = AllocateNode();
_nodes[node2].Parent = node1;
_nodes[node2].UserData = null;
_nodes[node2].Aabb.Combine(_nodes[leaf].Aabb, _nodes[sibling].Aabb);
if (node1 != NullNode)
{
if (_nodes[_nodes[sibling].Parent].Child1 == sibling)
{
_nodes[node1].Child1 = node2;
}
else
{
_nodes[node1].Child2 = node2;
}
_nodes[node2].Child1 = sibling;
_nodes[node2].Child2 = leaf;
_nodes[sibling].Parent = node2;
_nodes[leaf].Parent = node2;
do
{
if (_nodes[node1].Aabb.Contains(_nodes[node2].Aabb))
{
break;
}
_nodes[node1].Aabb.Combine(_nodes[_nodes[node1].Child1].Aabb, _nodes[_nodes[node1].Child2].Aabb);
node2 = node1;
node1 = _nodes[node1].Parent;
}while (node1 != NullNode);
}
else
{
_nodes[node2].Child1 = sibling;
_nodes[node2].Child2 = leaf;
_nodes[sibling].Parent = node2;
_nodes[leaf].Parent = node2;
_root = node2;
}
}
