public static bool GetContact(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform transformA, ref RigidTransform transformB, ref Vector3 penetrationAxis, out ContactData contact)
{
RigidTransform localTransformB;
MinkowskiToolbox.GetLocalTransform(ref transformA, ref transformB, out localTransformB);
if (MPRToolbox.AreLocalShapesOverlapping(shapeA, shapeB, ref localTransformB))
{
//First, try to use the heuristically found direction. This comes from either the GJK shallow contact separating axis or from the relative velocity.
Vector3 rayCastDirection;
float lengthSquared = penetrationAxis.LengthSquared();
if (lengthSquared > Toolbox.Epsilon)
{
Vector3.Divide(ref penetrationAxis, (float)Math.Sqrt(lengthSquared), out rayCastDirection);// (Vector3.Normalize(localDirection) + Vector3.Normalize(collidableB.worldTransform.Position - collidableA.worldTransform.Position)) / 2;
MPRToolbox.LocalSurfaceCast(shapeA, shapeB, ref localTransformB, ref rayCastDirection, out contact.PenetrationDepth, out contact.Normal);
}
else
{
contact.PenetrationDepth = float.MaxValue;
contact.Normal = Toolbox.UpVector;
}
//Try the offset between the origins as a second option. Sometimes this is a better choice than the relative velocity.
//TODO: Could use the position-finding MPR iteration to find the A-B direction hit by continuing even after the origin has been found (optimization).
Vector3 normalCandidate;
float depthCandidate;
lengthSquared = localTransformB.Position.LengthSquared();
if (lengthSquared > Toolbox.Epsilon)
{
Vector3.Divide(ref localTransformB.Position, (float)Math.Sqrt(lengthSquared), out rayCastDirection);
MPRToolbox.LocalSurfaceCast(shapeA, shapeB, ref localTransformB, ref rayCastDirection, out depthCandidate, out normalCandidate);
if (depthCandidate < contact.PenetrationDepth)
{
contact.Normal = normalCandidate;
contact.PenetrationDepth = depthCandidate;
}
}
//if (contact.PenetrationDepth > 1)
// Debug.WriteLine("Break.");
//Correct the penetration depth.
RefinePenetration(shapeA, shapeB, ref localTransformB, contact.PenetrationDepth, ref contact.Normal, out contact.PenetrationDepth, out contact.Normal, out contact.Position);
////Correct the penetration depth.
//MPRTesting.LocalSurfaceCast(shape, shapeB, ref localPoint, ref contact.Normal, out contact.PenetrationDepth, out rayCastDirection);
////The local casting can optionally continue. Eventually, it will converge to the local minimum.
//while (true)
//{
// MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localPoint, ref contact.Normal, out depthCandidate, out normalCandidate);
// if (contact.PenetrationDepth - depthCandidate <= Toolbox.BigEpsilon)
// break;
// contact.PenetrationDepth = depthCandidate;
// contact.Normal = normalCandidate;
//}
contact.Id = -1;
//we're still in local space! transform it all back.
Matrix3x3 orientation;
Matrix3x3.CreateFromQuaternion(ref transformA.Orientation, out orientation);
Matrix3x3.Transform(ref contact.Normal, ref orientation, out contact.Normal);
//Vector3.Negate(ref contact.Normal, out contact.Normal);
Matrix3x3.Transform(ref contact.Position, ref orientation, out contact.Position);
Vector3.Add(ref contact.Position, ref transformA.Position, out contact.Position);
return true;
}
contact = new ContactData();
return false;
}
