public static bool RayCast(Ray ray, ConvexShape shape, ref RigidTransform shapeTransform, float maximumLength,
out RayHit hit)
{
//Transform the ray into the object's local space.
Vector3.Subtract(ref ray.Position, ref shapeTransform.Position, out ray.Position);
Quaternion conjugate;
Quaternion.Conjugate(ref shapeTransform.Orientation, out conjugate);
Vector3.Transform(ref ray.Position, ref conjugate, out ray.Position);
Vector3.Transform(ref ray.Direction, ref conjugate, out ray.Direction);
Vector3 extremePointToRayOrigin, extremePoint;
hit.T = 0;
hit.Location = ray.Position;
hit.Normal = Toolbox.ZeroVector;
Vector3 closestOffset = hit.Location;
RaySimplex simplex = new RaySimplex();
float vw, closestPointDotDirection;
int count = 0;
//This epsilon has a significant impact on performance and accuracy. Changing it to use BigEpsilon instead increases speed by around 30-40% usually, but jigging is more evident.
while (closestOffset.LengthSquared() >= Toolbox.Epsilon * simplex.GetErrorTolerance(ref ray.Position))
{
if (++count > MaximumGJKIterations)
{
//It's taken too long to find a hit. Numerical problems are probable; quit.
hit = new RayHit();
return false;
}
shape.GetLocalExtremePoint(closestOffset, out extremePoint);
Vector3.Subtract(ref hit.Location, ref extremePoint, out extremePointToRayOrigin);
Vector3.Dot(ref closestOffset, ref extremePointToRayOrigin, out vw);
//If the closest offset and the extreme point->ray origin direction point the same way,
//then we might be able to conservatively advance the point towards the surface.
if (vw > 0)
{
Vector3.Dot(ref closestOffset, ref ray.Direction, out closestPointDotDirection);
if (closestPointDotDirection >= 0)
{
hit = new RayHit();
return false;
}
hit.T = hit.T - vw / closestPointDotDirection;
if (hit.T > maximumLength)
{
//If we've gone beyond where the ray can reach, there's obviously no hit.
hit = new RayHit();
return false;
}
//Shift the ray up.
Vector3.Multiply(ref ray.Direction, hit.T, out hit.Location);
Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location);
hit.Normal = closestOffset;
}
RaySimplex shiftedSimplex;
simplex.AddNewSimplexPoint(ref extremePoint, ref hit.Location, out shiftedSimplex);
//Compute the offset from the simplex surface to the origin.
shiftedSimplex.GetPointClosestToOrigin(ref simplex, out closestOffset);
}
//Transform the hit data into world space.
Vector3.Transform(ref hit.Normal, ref shapeTransform.Orientation, out hit.Normal);
Vector3.Transform(ref hit.Location, ref shapeTransform.Orientation, out hit.Location);
Vector3.Add(ref hit.Location, ref shapeTransform.Position, out hit.Location);
return true;
}