public void Raycast(ITreeRayCastCallback callback, RayCastInput input)
{
Vec2 p1 = input.P1;
Vec2 p2 = input.P2;
r.Set(p2).SubLocal(p1);
Debug.Assert(r.LengthSquared() > 0f);
r.Normalize();
// v is perpendicular to the segment.
Vec2.CrossToOutUnsafe(1f, r, v);
absV.Set(v).AbsLocal();
// 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 segAABB = aabb;
// Vec2 t = p1 + maxFraction * (p2 - p1);
temp.Set(p2).SubLocal(p1).MulLocal(maxFraction).AddLocal(p1);
Vec2.MinToOut(p1, temp, segAABB.LowerBound);
Vec2.MaxToOut(p1, temp, segAABB.UpperBound);
intStack.Push(m_root);
while (intStack.Count > 0)
{
int nodeId = intStack.Pop();
if (nodeId == TreeNode.NULL_NODE)
{
continue;
}
TreeNode node = m_nodes[nodeId];
if (!AABB.TestOverlap(node.AABB, segAABB))
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
node.AABB.GetCenterToOut(c);
node.AABB.GetExtentsToOut(h);
temp.Set(p1).SubLocal(c);
float separation = MathUtils.Abs(Vec2.Dot(v, temp)) - Vec2.Dot(absV, h);
if (separation > 0.0f)
{
continue;
}
if (node.Leaf)
{
subInput.P1.Set(input.P1);
subInput.P2.Set(input.P2);
subInput.MaxFraction = maxFraction;
float value = callback.RaycastCallback(subInput, nodeId);
if (value == 0.0f)
{
// The client has terminated the ray cast.
return;
}
if (value > 0.0f)
{
// Update segment bounding box.
maxFraction = value;
t.Set(p2).SubLocal(p1).MulLocal(maxFraction).AddLocal(p1);
Vec2.MinToOut(p1, t, segAABB.LowerBound);
Vec2.MaxToOut(p1, t, segAABB.UpperBound);
}
}
else
{
intStack.Push(node.Child1);
intStack.Push(node.Child2);
}
}
}