private bool TryInnerSphereContact(out ContactData contact)
{
Vector3 closestPoint;
Toolbox.GetClosestPointOnTriangleToPoint(ref triangle.vA, ref triangle.vB, ref triangle.vC, ref Toolbox.ZeroVector, out closestPoint);
float length = closestPoint.LengthSquared();
float minimumRadius = convex.minimumRadius * (MotionSettings.CoreShapeScaling + .01f);
if (length < minimumRadius * minimumRadius)
{
Vector3 triangleNormal, ab, ac;
Vector3.Subtract(ref triangle.vB, ref triangle.vA, out ab);
Vector3.Subtract(ref triangle.vC, ref triangle.vA, out ac);
Vector3.Cross(ref ab, ref ac, out triangleNormal);
float dot;
Vector3.Dot(ref closestPoint, ref triangleNormal, out dot);
if ((triangle.sidedness == TriangleSidedness.Clockwise && dot > 0) || (triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0))
{
//Normal was facing the wrong way.
contact = new ContactData();
return false;
}
length = (float)Math.Sqrt(length);
contact.Position = closestPoint;
if (length > Toolbox.Epsilon) //Watch out for NaN's!
{
Vector3.Divide(ref closestPoint, length, out contact.Normal);
}
else
{
//The direction is undefined. Use the triangle's normal.
//One sided triangles can only face in the appropriate direction.
float normalLength = triangleNormal.LengthSquared();
if (triangleNormal.LengthSquared() > Toolbox.Epsilon)
{
Vector3.Divide(ref triangleNormal, (float)Math.Sqrt(normalLength), out triangleNormal);
if (triangle.sidedness == TriangleSidedness.Clockwise)
contact.Normal = triangleNormal;
else
Vector3.Negate(ref triangleNormal, out contact.Normal);
}
else
{
//Degenerate triangle!
contact = new ContactData();
return false;
}
}
//Compute the actual depth of the contact.
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref contact.Normal, out contact.PenetrationDepth, out triangleNormal); //Trash the 'corrected' normal. We want to use the spherical normal.
contact.Id = -1;
return true;
}
contact = new ContactData();
return false;
}
