private static bool LocalSweepCast(ConvexShape shape, ConvexShape shapeB, float sweepLength, float rayLengthSquared, ref Vector3 localDirection, ref Vector3 sweep, ref RigidTransform localTransformB, out RayHit hit)
{
//By now, the ray is known to be within the swept shape and facing the right direction for a normal raycast.
//First guess a portal.
//This implementation is similar to that of the original XenoCollide.
//'n' will be the direction used to find supports throughout the algorithm.
Vector3 n = localDirection;
Vector3 v1, v1A;
GetSweptExtremePoint(shape, shapeB, ref localTransformB, ref sweep, ref n, out v1A, out v1);
//v1 could be zero in some degenerate cases.
//if (v1.LengthSquared() < Toolbox.Epsilon)
//{
// hit.T = 0;
// Vector3.Normalize(ref n, out hit.Normal);
// Vector3.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal);
// //hit.Location = hit.T * localDirection;
// Vector3.Transform(ref hit.Location, ref transform.Orientation, out hit.Location);
// Vector3.Add(ref hit.Location, ref transform.Position, out hit.Location);
// hit.Location += sweepA * hit.T;
// return true;
//}
//Find another extreme point in a direction perpendicular to the previous.
Vector3 v2, v2A;
Vector3.Cross(ref localDirection, ref v1, out n);
hit.Location = new Vector3();
if (n.LengthSquared() < Toolbox.Epsilon * .01f)
{
//v1 and v0 could be parallel.
//This isn't a bad thing- it means the direction is exactly aligned with the extreme point offset.
//In other words, if the raycast is followed out to the surface, it will arrive at the extreme point!
if (rayLengthSquared > Toolbox.Epsilon * .01f)
Vector3.Divide(ref localDirection, (float)Math.Sqrt(rayLengthSquared), out hit.Normal);
else
hit.Normal = new Vector3();
float rate;
Vector3.Dot(ref hit.Normal, ref localDirection, out rate);
float distance;
Vector3.Dot(ref hit.Normal, ref v1, out distance);
if (rate > 0)
hit.T = sweepLength - distance / rate;
else
hit.T = sweepLength;
if (hit.T < 0)
hit.T = 0;
//Vector3.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal);
////hit.Location = hit.T * localDirection;
//Vector3.Transform(ref hit.Location, ref transform.Orientation, out hit.Location);
//Vector3.Add(ref hit.Location, ref transform.Position, out hit.Location);
//hit.Location += sweepA * hit.T;
return hit.T <= 1;
}
GetSweptExtremePoint(shape, shapeB, ref localTransformB, ref sweep, ref n, out v2A, out v2);
Vector3 temp1, temp2;
//Set n for the first iteration.
Vector3.Cross(ref v1, ref v2, out n);
//It's possible that v1 and v2 were constructed in such a way that 'n' is not properly calibrated
//relative to the direction vector.
float dot;
Vector3.Dot(ref n, ref localDirection, out dot);
if (dot > 0)
{
//It's not properly calibrated. Flip the winding (and the previously calculated normal).
Vector3.Negate(ref n, out n);
temp1 = v1;
v1 = v2;
v2 = temp1;
temp1 = v1A;
v1A = v2A;
v2A = temp1;
}
Vector3 v3, v3A;
int count = 0;
while (true)
{
//Find a final extreme point using the normal of the plane defined by v0, v1, v2.
GetSweptExtremePoint(shape, shapeB, ref localTransformB, ref sweep, ref n, out v3A, out v3);
if (count > MPRToolbox.OuterIterationLimit)
{
//Can't enclose the origin! That's a bit odd. Something is wrong; the preparation for this raycast
//guarantees that the origin is enclosed. Could be a numerical problem.
hit.T = float.MaxValue;
hit.Normal = new Vector3();
hit.Location = new Vector3();
return false;
}
count++;
//By now, the simplex is a tetrahedron, but it is not known whether or not the ray actually passes through the portal
//defined by v1, v2, v3.
// If the direction is outside the plane defined by v1,v0,v3, then the portal is invalid.
Vector3.Cross(ref v1, ref v3, out temp1);
Vector3.Dot(ref temp1, ref localDirection, out dot);
if (dot < 0)
{
//Replace the point that was on the inside of the plane (v2) with the new extreme point.
v2 = v3;
v2A = v3A;
// Calculate the normal of the plane that will be used to find a new extreme point.
Vector3.Cross(ref v1, ref v3, out n);
continue;
}
// If the direction is outside the plane defined by v3,v0,v2, then the portal is invalid.
Vector3.Cross(ref v3, ref v2, out temp1);
Vector3.Dot(ref temp1, ref localDirection, out dot);
if (dot < 0)
{
//Replace the point that was on the inside of the plane (v1) with the new extreme point.
v1 = v3;
v1A = v3A;
// Calculate the normal of the plane that will be used to find a new extreme point.
Vector3.Cross(ref v2, ref v3, out n);
continue;
}
break;
}
// Refine the portal.
count = 0;
while (true)
{
//Compute the outward facing normal.
Vector3.Subtract(ref v1, ref v2, out temp1);
Vector3.Subtract(ref v3, ref v2, out temp2);
Vector3.Cross(ref temp1, ref temp2, out n);
//Keep working towards the surface. Find the next extreme point.
Vector3 v4, v4A;
GetSweptExtremePoint(shape, shapeB, ref localTransformB, ref sweep, ref n, out v4A, out v4);
//If the plane which generated the normal is very close to the extreme point, then we're at the surface.
Vector3.Dot(ref n, ref v1, out dot);
float supportDot;
Vector3.Dot(ref v4, ref n, out supportDot);
if (supportDot - dot < rayCastSurfaceEpsilon || count > MPRToolbox.InnerIterationLimit) // TODO: Could use a dynamic epsilon for possibly better behavior.
{
//The portal is now on the surface. The algorithm can now compute the TOI and exit.
float lengthSquared = n.LengthSquared();
if (lengthSquared > Toolbox.Epsilon * .00001f)
{
Vector3.Divide(ref n, (float)Math.Sqrt(lengthSquared), out hit.Normal);
//The plane is very close to the surface, and the ray is known to pass through it.
//dot is the rate.
Vector3.Dot(ref hit.Normal, ref localDirection, out dot);
//supportDot is the distance to the plane.
Vector3.Dot(ref hit.Normal, ref v1, out supportDot);
hit.T = sweepLength - supportDot / dot;
}
else
{
Vector3.Normalize(ref localDirection, out hit.Normal);
hit.T = sweepLength;
}
//Sometimes, when the objects are intersecting, the T parameter can be negative.
//In this case, just go with t = 0.
if (hit.T < 0)
hit.T = 0;
//Vector3.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal);
//Vector3.Transform(ref hit.Location, ref transform.Orientation, out hit.Location);
//Vector3.Add(ref hit.Location, ref transform.Position, out hit.Location);
//hit.Location += sweepA * (hit.T);
//Compute the barycentric coordinates of the ray hit location.
//Vector3 mdHitLocation = t * localDirection;
//float v1Weight, v2Weight, v3Weight;
//Toolbox.GetBarycentricCoordinates(ref mdHitLocation, ref v1, ref v2, ref v3, out v1Weight, out v2Weight, out v3Weight);
//hit.Location = v1Weight * v1A + v2Weight * v2A + v3Weight * v3A;
//hit.Location += sweepA * hit.T;
//Vector3.Transform(ref hit.Location, ref transform.Orientation, out hit.Location);
//Vector3.Add(ref hit.Location, ref transform.Position, out hit.Location);
return hit.T <= 1;
}
//Still haven't exited, so refine the portal.
//Test direction against the three planes that separate the new portal candidates: (v1,v4,v0) (v2,v4,v0) (v3,v4,v0)
//This may look a little weird at first.
//'inside' here means 'on the positive side of the plane.'
//There are three total planes being tested, one for each of v1, v2, and v3.
//The planes are created from consistently wound vertices, so it's possible to determine
//where the ray passes through the portal based upon its relationship to two of the three planes.
//The third vertex which is found to be opposite the face which contains the ray is replaced with the extreme point.
//This v4 x direction is just a minor reordering of a scalar triple product: (v1 x v4) * direction.
//It eliminates the need for extra cross products for the inner if.
Vector3.Cross(ref v4, ref localDirection, out temp1);
Vector3.Dot(ref v1, ref temp1, out dot);
if (dot >= 0)
{
Vector3.Dot(ref v2, ref temp1, out dot);
if (dot >= 0)
{
v1 = v4; // Inside v1 & inside v2 ==> eliminate v1
v1A = v4A;
}
else
{
v3 = v4; // Inside v1 & outside v2 ==> eliminate v3
v3A = v4A;
}
}
else
{
Vector3.Dot(ref v3, ref temp1, out dot);
if (dot >= 0)
{
v2 = v4; // Outside v1 & inside v3 ==> eliminate v2
v2A = v4A;
}
else
{
v1 = v4; // Outside v1 & outside v3 ==> eliminate v1
v1A = v4A;
}
}
count++;
}
}
