// From Real-time Collision Detection, p179.
public bool RayCast(out RayCastOutput output, ref RayCastInput input)
{
output = new RayCastOutput();
float tmin = -Settings.MaxFloat;
float tmax = Settings.MaxFloat;
Vector2 p = input.Point1;
Vector2 d = input.Point2 - input.Point1;
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.Epsilon)
{
// Parallel.
if (p_i < lowerBound_i || upperBound_i < p_i)
{
return false;
}
}
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(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 false;
}
}
}
// Does the ray start inside the box?
// Does the ray intersect beyond the max fraction?
if (tmin < 0.0f || input.MaxFraction < tmin)
{
return false;
}
// Intersection.
output.Fraction = tmin;
output.Normal = normal;
return true;
}
/// <summary>
/// Cast a ray against this Shape.
/// </summary>
/// <param name="output">The ray-cast results.</param>
/// <param name="input">The ray-cast input parameters.</param>
/// <param name="childIndex">Index of the child.</param>
/// <returns></returns>
public bool RayCast(out RayCastOutput output, ref RayCastInput input, int childIndex)
{
return Shape.RayCast(out output, ref input, ref Body.Xf, childIndex);
}
private void RayCast()
{
_rayActor = null;
RayCastInput input = _rayCastInput;
// Ray cast against the dynamic tree.
_tree.RayCast(RayCastCallback, ref input);
// Brute force ray cast.
Actor bruteActor = null;
RayCastOutput bruteOutput = new RayCastOutput();
for (int i = 0; i < ActorCount; ++i)
{
if (_actors[i].ProxyId == -1)
{
continue;
}
RayCastOutput output;
bool hit = _actors[i].AABB.RayCast(out output, ref input);
if (hit)
{
bruteActor = _actors[i];
bruteOutput = output;
input.MaxFraction = output.Fraction;
}
}
if (bruteActor != null)
{
Debug.Assert(bruteOutput.Fraction == _rayCastOutput.Fraction);
}
}
private float RayCastCallback(ref RayCastInput input, int proxyid)
{
Actor actor = _tree.GetUserData(proxyid);
RayCastOutput output;
bool hit = actor.AABB.RayCast(out output, ref input);
if (hit)
{
_rayCastOutput = output;
_rayActor = actor;
actor.Fraction = output.Fraction;
return output.Fraction;
}
return input.MaxFraction;
}
/// <summary>
/// Cast a ray against this Shape.
/// </summary>
/// <param name="output">The ray-cast results.</param>
/// <param name="input">The ray-cast input parameters.</param>
/// <param name="childIndex">Index of the child.</param>
/// <returns></returns>
public bool RayCast(out RayCastOutput output, ref RayCastInput input, int childIndex)
{
Transform xf;
Body.GetTransform(out xf);
return Shape.RayCast(out output, ref input, ref xf, childIndex);
}
