public static bool SweptShapeContainsPoint(ConvexShape shape, ref Vector3 sweep, ref Vector3 localPoint)
{
//It's possible that the two objects' centers are overlapping, or very very close to it. In this case,
//they are obviously colliding and we can immediately exit.
if (localPoint.LengthSquared() < Toolbox.Epsilon)
{
return true;
}
Vector3 v0;
Vector3.Negate(ref localPoint, out v0); //Since we're in A's local space, A-B is just -B.
//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 = localPoint;
Vector3 v1;
//MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shape, shapeB, ref n, ref localPoint, out v1A, out v1B, out v1);
GetSweptExtremePoint(shape, ref localPoint, ref sweep, ref n, out v1);
//Find another extreme point in a direction perpendicular to the previous.
Vector3 v2;
Vector3.Cross(ref v1, ref v0, 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!
//If the origin is further along this direction than the extreme point, then there is no intersection.
//If the origin is within this extreme point, then there is an intersection.
float dot;
Vector3.Dot(ref v1, ref localPoint, out dot);
if (dot < 0)
{
//Origin is outside.
return false;
}
return true;
}
//MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shape, shapeB, ref n, ref localPoint, out v2A, out v2B, out v2);
GetSweptExtremePoint(shape, ref localPoint, ref sweep, ref n, out v2);
Vector3 temp1, temp2;
//Set n for the first iteration.
Vector3.Subtract(ref v1, ref v0, out temp1);
Vector3.Subtract(ref v2, ref v0, out temp2);
Vector3.Cross(ref temp1, ref temp2, out n);
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(shape, shapeB, ref n, ref localPoint, out v3A, out v3B, out v3);
GetSweptExtremePoint(shape, ref localPoint, ref sweep, ref n, out v3);
if (count > MPRToolbox.OuterIterationLimit)
break;
count++;
//By now, the simplex is a tetrahedron, but it is not known whether or not the origin ray found earlier actually passes through the portal
//defined by v1, v2, v3.
// If the origin is outside the plane defined by v1,v0,v3, then the portal is invalid.
Vector3.Cross(ref v1, ref v3, out temp1);
float dot;
Vector3.Dot(ref temp1, ref v0, 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.Subtract(ref v1, ref v0, out temp1);
Vector3.Subtract(ref v3, ref v0, out temp2);
Vector3.Cross(ref temp1, ref temp2, out n);
continue;
}
// If the origin 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 v0, 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.Subtract(ref v2, ref v0, out temp1);
Vector3.Subtract(ref v3, ref v0, out temp2);
Vector3.Cross(ref temp1, ref temp2, out n);
continue;
}
break;
}
//if (!VerifySimplex(ref v0, ref v1, ref v2, ref v3, ref localPoint.Position))
// Debug.WriteLine("Break.");
// Refine the portal.
while (true)
{
//Test the origin against the plane defined by v1, v2, v3. If it's inside, we're done.
//Compute the outward facing normal.
Vector3.Subtract(ref v3, ref v2, out temp1);
Vector3.Subtract(ref v1, ref v2, out temp2);
Vector3.Cross(ref temp1, ref temp2, out n);
float dot;
Vector3.Dot(ref n, ref v1, out dot);
if (dot >= 0)
{
return true;
}
//We haven't yet found the origin. Find the support point in the portal's outward facing direction.
Vector3 v4;
//MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shape, shapeB, ref n, ref localPoint, out v4A, out v4B, out v4);
GetSweptExtremePoint(shape, ref localPoint, ref sweep, ref n, out v4);
//If the origin is further along the direction than the extreme point, it's not inside the shape.
float dot2;
Vector3.Dot(ref v4, ref n, out dot2);
if (dot2 < 0)
{
//The origin is outside!
return false;
}
//If the plane which generated the normal is very close to the extreme point, then we're at the surface
//and we have not found the origin; it's either just BARELY inside, or it is outside. Assume it's outside.
if (dot2 - dot < surfaceEpsilon || count > MPRToolbox.InnerIterationLimit) // TODO: Could use a dynamic epsilon for possibly better behavior.
{
//DEBUGlastPosition = position;
return false;
}
count++;
//Still haven't exited, so refine the portal.
//Test origin against the three planes that separate the new portal candidates: (v1,v4,v0) (v2,v4,v0) (v3,v4,v0)
Vector3.Cross(ref v4, ref v0, 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
}
}
//if (!VerifySimplex(ref v0, ref v1, ref v2, ref v3, ref localPoint.Position))
// Debug.WriteLine("Break.");
}
}
