public static bool AreShapesIntersecting(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform transformA, ref RigidTransform transformB,
ref Vector3 localSeparatingAxis)
{
RigidTransform localtransformB;
MinkowskiToolbox.GetLocalTransform(ref transformA, ref transformB, out localtransformB);
//Warm start the simplex.
var simplex = new SimpleSimplex();
Vector3 extremePoint;
MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref localSeparatingAxis, ref localtransformB, out extremePoint);
simplex.AddNewSimplexPoint(ref extremePoint);
Vector3 closestPoint;
int count = 0;
while (count++ < MaximumGJKIterations)
{
if (simplex.GetPointClosestToOrigin(out closestPoint) || //Also reduces the simplex.
closestPoint.LengthSquared() <= simplex.GetErrorTolerance() * Toolbox.BigEpsilon)
{
//Intersecting, or so close to it that it will be difficult/expensive to figure out the separation.
return true;
}
//Use the closest point as a direction.
Vector3 direction;
Vector3.Negate(ref closestPoint, out direction);
MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref direction, ref localtransformB, out extremePoint);
//Since this is a boolean test, we don't need to refine the simplex if it becomes apparent that we cannot reach the origin.
//If the most extreme point at any given time does not go past the origin, then we can quit immediately.
float dot;
Vector3.Dot(ref extremePoint, ref closestPoint, out dot); //extreme point dotted against the direction pointing backwards towards the CSO.
if (dot > 0)
{
// If it's positive, that means that the direction pointing towards the origin produced an extreme point 'in front of' the origin, eliminating the possibility of any intersection.
localSeparatingAxis = direction;
return false;
}
simplex.AddNewSimplexPoint(ref extremePoint);
}
return false;
}
