// From Real-time Collision Detection, p179.
public void RayCast(out RayCastOutput output, ref RayCastInput input)
{
output = new RayCastOutput();
float tmin = -Settings.b2_FLT_MAX;
float tmax = Settings.b2_FLT_MAX;
output.hit = false;
Vector2 p = input.p1;
Vector2 d = input.p2 - input.p1;
Vector2 absD = MathUtils.Abs(d);
Vector2 normal = Vector2.Zero;
for (int i = 0; i < 2; ++i)
{
float absD_i = i == 0 ? absD.X : absD.Y;
float lowerBound_i = i == 0 ? lowerBound.X : lowerBound.Y;
float upperBound_i = i == 0 ? upperBound.X : upperBound.Y;
float p_i = i == 0 ? p.X : p.Y;
if (absD_i < Settings.b2_FLT_EPSILON)
{
// Parallel.
if (p_i < lowerBound_i || upperBound_i < p_i)
{
return;
}
}
else
{
float d_i = i == 0 ? d.X : d.Y;
float inv_d = 1.0f / d_i;
float t1 = (lowerBound_i - p_i) * inv_d;
float t2 = (upperBound_i - p_i) * inv_d;
// Sign of the normal vector.
float s = -1.0f;
if (t1 > t2)
{
MathUtils.Swap<float>(ref t1, ref t2);
s = 1.0f;
}
// Push the min up
if (t1 > tmin)
{
if (i == 0)
{
normal.X = s;
}
else
{
normal.Y = s;
}
tmin = t1;
}
// Pull the max down
tmax = Math.Min(tmax, t2);
if (tmin > tmax)
{
return;
}
}
}
// Does the ray start inside the box?
// Does the ray intersect beyond the max fraction?
if (tmin < 0.0f || input.maxFraction < tmin)
{
return;
}
// Intersection.
output.fraction = tmin;
output.normal = normal;
output.hit = true;
}
/// 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(RayCastCallback callback, ref RayCastInput input)
{
Vector2 p1 = input.p1;
Vector2 p2 = input.p2;
Vector2 r = p2 - p1;
Debug.Assert(r.LengthSquared() > 0.0f);
r.Normalize();
// v is perpendicular to the segment.
Vector2 v = MathUtils.Cross(1.0f, r);
Vector2 abs_v = MathUtils.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();
{
Vector2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.lowerBound = Vector2.Min(p1, t);
segmentAABB.upperBound = Vector2.Max(p1, t);
}
int count = 0;
stack[count++] = _root;
while (count > 0)
{
int nodeId = stack[--count];
if (nodeId == NullNode)
{
continue;
}
DynamicTreeNode node = _nodes[nodeId];
if (AABB.TestOverlap(ref node.aabb, ref segmentAABB) == false)
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
Vector2 c = node.aabb.GetCenter();
Vector2 h = node.aabb.GetExtents();
float separation = Math.Abs(Vector2.Dot(v, p1 - c)) - Vector2.Dot(abs_v, h);
if (separation > 0.0f)
{
continue;
}
if (node.IsLeaf())
{
RayCastInput subInput;
subInput.p1 = input.p1;
subInput.p2 = input.p2;
subInput.maxFraction = maxFraction;
RayCastOutput output;
callback(out output, ref subInput, node.userData);
if (output.hit)
{
// Early exit.
if (output.fraction == 0.0f)
{
return;
}
maxFraction = output.fraction;
// Update segment bounding box.
{
Vector2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.lowerBound = Vector2.Min(p1, t);
segmentAABB.upperBound = Vector2.Max(p1, t);
}
}
}
else
{
Debug.Assert(count + 1 < k_stackSize);
stack[count++] = node.child1;
stack[count++] = node.child2;
}
}
}
