public static void LocalSurfaceCast(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform localTransformB, ref Vector3 direction, out float t, out Vector3 normal)
{
// Local surface cast is very similar to regular MPR. However, instead of starting at an interior point and targeting the origin,
// the ray starts at the origin (a point known to be in both shape and shapeB), and just goes towards the direction until the surface
// is found. The portal (v1, v2, v3) at termination defines the surface normal, and the distance from the origin to the portal along the direction is used as the 't' result.
//'v0' is no longer explicitly tracked since it is simply the origin.
//Now that the origin ray is known, create a portal through which the ray passes.
//To do this, 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 = direction;
Vector3 v1;
MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v1);
//v1 could be zero in some degenerate cases.
//if (v1.LengthSquared() < Toolbox.Epsilon)
//{
// t = 0;
// normal = n;
// return;
//}
//Find another extreme point in a direction perpendicular to the previous.
Vector3 v2;
Vector3.Cross(ref direction, ref v1, out n);
if (n.LengthSquared() < Toolbox.Epsilon)
{
//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!
float rayLengthSquared = direction.LengthSquared();
if (rayLengthSquared > Toolbox.Epsilon * .01f)
Vector3.Divide(ref direction, (float)Math.Sqrt(rayLengthSquared), out normal);
else
normal = new Vector3();
float rate;
Vector3.Dot(ref normal, ref direction, out rate);
float distance;
Vector3.Dot(ref normal, ref v1, out distance);
if (rate > 0)
t = distance / rate;
else
t = 0;
return;
}
MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, 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 direction, 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;
}
Vector3 v3;
int count = 0;
while (true)
{
//Find a final extreme point using the normal of the plane defined by v0, v1, v2.
MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v3);
if (count > MPRToolbox.OuterIterationLimit)
{
//Can't enclose the origin! That's a bit odd; something is wrong.
t = float.MaxValue;
normal = Toolbox.UpVector;
return;
}
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 direction, out dot);
if (dot < 0)
{
//Replace the point that was on the inside of the plane (v2) with the new extreme point.
v2 = v3;
// 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 direction, out dot);
if (dot < 0)
{
//Replace the point that was on the inside of the plane (v1) with the new extreme point.
v1 = v3;
// 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;
}
//if (!VerifySimplex(ref Toolbox.ZeroVector, ref v1, ref v2, ref v3, ref direction))
// Debug.WriteLine("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;
MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, 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 < surfaceEpsilon || count > MPRToolbox.InnerIterationLimit) // TODO: Could use a dynamic epsilon for possibly better behavior.
{
//normal = n;
//float normalLengthInverse = 1 / normal.Length();
//Vector3.Multiply(ref normal, normalLengthInverse, out normal);
////Find the distance from the origin to the plane.
//t = dot * normalLengthInverse;
float lengthSquared = n.LengthSquared();
if (lengthSquared > Toolbox.Epsilon * .01f)
{
Vector3.Divide(ref n, (float)Math.Sqrt(lengthSquared), out 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 normal, ref direction, out dot);
//supportDot is the distance to the plane.
Vector3.Dot(ref normal, ref v1, out supportDot);
if (dot > 0)
t = supportDot / dot;
else
t = 0;
}
else
{
normal = Vector3.Up;
t = 0;
}
////DEBUG STUFF:
//DEBUGlastRayT = t;
//DEBUGlastRayDirection = direction;
//DEBUGlastDepth = t;
//DEBUGlastNormal = normal;
//DEBUGlastV1 = v1;
//DEBUGlastV2 = v2;
//DEBUGlastV3 = v3;
return;
}
//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 direction, 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
}
else
{
v3 = v4; // Inside v1 & outside v2 ==> eliminate v3
}
}
else
{
Vector3.Dot(ref v3, ref temp1, out dot);
if (dot >= 0)
{
v2 = v4; // Outside v1 & inside v3 ==> eliminate v2
}
else
{
v1 = v4; // Outside v1 & outside v3 ==> eliminate v1
}
}
count++;
//Here's an unoptimized equivalent without the scalar triple product reorder.
#region Equivalent refinement
//Vector3.Cross(ref v1, ref v4, out temp1);
//Vector3.Dot(ref temp1, ref direction, out dot);
//if (dot > 0)
//{
// Vector3.Cross(ref v2, ref v4, out temp2);
// Vector3.Dot(ref temp2, ref direction, out dot);
// if (dot > 0)
// {
// //Inside v1, v4, v0 and inside v2, v4, v0
// v1 = v4;
// }
// else
// {
// //Inside v1, v4, v0 and outside v2, v4, v0
// v3 = v4;
// }
//}
//else
//{
// Vector3.Cross(ref v3, ref v4, out temp2);
// Vector3.Dot(ref temp2, ref direction, out dot);
// if (dot > 0)
// {
// //Outside v1, v4, v0 and inside v3, v4, v0
// v2 = v4;
// }
// else
// {
// //Outside v1, v4, v0 and outside v3, v4, v0
// v1 = v4;
// }
//}
#endregion
//if (!VerifySimplex(ref Toolbox.ZeroVector, ref v1, ref v2, ref v3, ref direction))
// Debug.WriteLine("Break.");
}
}
