private bool DoDeepContact(out TinyStructList<ContactData> contactList)
{
//Find the origin to triangle center offset.
Vector3 center;
Vector3.Add(ref triangle.vA, ref triangle.vB, out center);
Vector3.Add(ref center, ref triangle.vC, out center);
Vector3.Multiply(ref center, 1f / 3f, out center);
ContactData contact;
contactList = new TinyStructList<ContactData>();
if (MPRToolbox.AreLocalShapesOverlapping(convex, triangle, ref center, ref Toolbox.RigidIdentity))
{
float dot;
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 lengthSquared = triangleNormal.LengthSquared();
if (lengthSquared < Toolbox.Epsilon * .01f)
{
//Degenerate triangle! That's no good.
//Just use the direction pointing from A to B, "B" being the triangle. That direction is center - origin, or just center.
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref center, out contact.PenetrationDepth, out contact.Normal, out contact.Position);
}
else
{
//Normalize the normal.
Vector3.Divide(ref triangleNormal, (float)Math.Sqrt(lengthSquared), out triangleNormal);
////The first direction to check is one of the triangle's edge normals. Choose the one that is most aligned with the offset from A to B.
////Project the direction onto the triangle plane.
//Vector3.Dot(ref triangleNormal, ref center, out dot);
//Vector3 trianglePlaneDirection;
//Vector3.Multiply(ref triangleNormal, dot, out trianglePlaneDirection);
//Vector3.Subtract(ref trianglePlaneDirection, ref center, out trianglePlaneDirection);
////To find out which edge to use, compute which region the direction is in.
////This is done by constructing three planes which segment the triangle into three sub-triangles.
////These planes are defined by A, origin, center; B, origin, center; C, origin, center.
////The plane tests against the direction can be reordered to:
////(center x direction) * A
////(center x direction) * B
////(center x direction) * C
//Vector3 OxD;
//Vector3.Cross(ref trianglePlaneDirection, ref center, out OxD);
//Vector3 p;
//float dotA, dotB, dotC;
//Vector3.Dot(ref triangle.vA, ref OxD, out dotA);
//Vector3.Dot(ref triangle.vB, ref OxD, out dotB);
//Vector3.Dot(ref triangle.vC, ref OxD, out dotC);
//if (dotA >= 0 && dotB <= 0)
//{
// //Direction is in the AB edge zone.
// //Compute the edge normal using AB x (AO x AB).
// Vector3 AB, AO;
// Vector3.Subtract(ref triangle.vB, ref triangle.vA, out AB);
// Vector3.Subtract(ref center, ref triangle.vA, out AO);
// Vector3.Cross(ref AO, ref AB, out p);
// Vector3.Cross(ref AB, ref p, out trianglePlaneDirection);
//}
//else if (dotB >= 0 && dotC <= 0)
//{
// //Direction is in the BC edge zone.
// //Compute the edge normal using BC x (BO x BC).
// Vector3 BC, BO;
// Vector3.Subtract(ref triangle.vC, ref triangle.vB, out BC);
// Vector3.Subtract(ref center, ref triangle.vB, out BO);
// Vector3.Cross(ref BO, ref BC, out p);
// Vector3.Cross(ref BC, ref p, out trianglePlaneDirection);
//}
//else // dotC > 0 && dotA < 0
//{
// //Direction is in the CA edge zone.
// //Compute the edge normal using CA x (CO x CA).
// Vector3 CA, CO;
// Vector3.Subtract(ref triangle.vA, ref triangle.vC, out CA);
// Vector3.Subtract(ref center, ref triangle.vC, out CO);
// Vector3.Cross(ref CO, ref CA, out p);
// Vector3.Cross(ref CA, ref p, out trianglePlaneDirection);
//}
//dot = trianglePlaneDirection.LengthSquared();
//if (dot > Toolbox.Epsilon)
//{
// Vector3.Divide(ref trianglePlaneDirection, (float)Math.Sqrt(dot), out trianglePlaneDirection);
// MPRTesting.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref trianglePlaneDirection, out contact.PenetrationDepth, out contact.Normal);
// //Check to see if the normal is facing in the proper direction, considering that this may not be a two-sided triangle.
// Vector3.Dot(ref triangleNormal, ref contact.Normal, out dot);
// if ((triangle.sidedness == TriangleSidedness.Clockwise && dot > 0) || (triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0))
// {
// //Normal was facing the wrong way.
// //Instead of ignoring it entirely, correct the direction to as close as it can get by removing any component parallel to the triangle normal.
// Vector3 previousNormal = contact.Normal;
// Vector3.Dot(ref contact.Normal, ref triangleNormal, out dot);
// Vector3.Multiply(ref contact.Normal, dot, out p);
// Vector3.Subtract(ref contact.Normal, ref p, out contact.Normal);
// float length = contact.Normal.LengthSquared();
// if (length > Toolbox.Epsilon)
// {
// //Renormalize the corrected normal.
// Vector3.Divide(ref contact.Normal, (float)Math.Sqrt(length), out contact.Normal);
// Vector3.Dot(ref contact.Normal, ref previousNormal, out dot);
// contact.PenetrationDepth *= dot;
// }
// else
// {
// contact.PenetrationDepth = float.MaxValue;
// contact.Normal = new Vector3();
// }
// }
//}
//else
//{
// contact.PenetrationDepth = float.MaxValue;
// contact.Normal = new Vector3();
//}
//TODO: This tests all three edge axes with a full MPR raycast. That's not really necessary; the correct edge normal should be discoverable, resulting in a single MPR raycast.
//Find the edge directions that will be tested with MPR.
Vector3 AO, BO, CO;
Vector3 AB, BC, CA;
Vector3.Subtract(ref center, ref triangle.vA, out AO);
Vector3.Subtract(ref center, ref triangle.vB, out BO);
Vector3.Subtract(ref center, ref triangle.vC, out CO);
Vector3.Subtract(ref triangle.vB, ref triangle.vA, out AB);
Vector3.Subtract(ref triangle.vC, ref triangle.vB, out BC);
Vector3.Subtract(ref triangle.vA, ref triangle.vC, out CA);
//We don't have to worry about degenerate triangles here because we've already handled that possibility above.
Vector3 ABnormal, BCnormal, CAnormal;
//Project the center onto the edge to find the direction from the center to the edge AB.
Vector3.Dot(ref AO, ref AB, out dot);
Vector3.Multiply(ref AB, dot / AB.LengthSquared(), out ABnormal);
Vector3.Subtract(ref AO, ref ABnormal, out ABnormal);
ABnormal.Normalize();
//Project the center onto the edge to find the direction from the center to the edge BC.
Vector3.Dot(ref BO, ref BC, out dot);
Vector3.Multiply(ref BC, dot / BC.LengthSquared(), out BCnormal);
Vector3.Subtract(ref BO, ref BCnormal, out BCnormal);
BCnormal.Normalize();
//Project the center onto the edge to find the direction from the center to the edge BC.
Vector3.Dot(ref CO, ref CA, out dot);
Vector3.Multiply(ref CA, dot / CA.LengthSquared(), out CAnormal);
Vector3.Subtract(ref CO, ref CAnormal, out CAnormal);
CAnormal.Normalize();
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref ABnormal, out contact.PenetrationDepth, out contact.Normal);
//Check to see if the normal is facing in the proper direction, considering that this may not be a two-sided triangle.
Vector3.Dot(ref triangleNormal, ref contact.Normal, out dot);
if ((triangle.sidedness == TriangleSidedness.Clockwise && dot > 0) || (triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0))
{
//Normal was facing the wrong way.
//Instead of ignoring it entirely, correct the direction to as close as it can get by removing any component parallel to the triangle normal.
Vector3 previousNormal = contact.Normal;
Vector3.Dot(ref contact.Normal, ref triangleNormal, out dot);
Vector3 p;
Vector3.Multiply(ref contact.Normal, dot, out p);
Vector3.Subtract(ref contact.Normal, ref p, out contact.Normal);
float length = contact.Normal.LengthSquared();
if (length > Toolbox.Epsilon)
{
//Renormalize the corrected normal.
Vector3.Divide(ref contact.Normal, (float)Math.Sqrt(length), out contact.Normal);
Vector3.Dot(ref contact.Normal, ref previousNormal, out dot);
contact.PenetrationDepth *= dot;
}
else
{
contact.PenetrationDepth = float.MaxValue;
contact.Normal = new Vector3();
}
}
Vector3 candidateNormal;
float candidateDepth;
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref BCnormal, out candidateDepth, out candidateNormal);
//Check to see if the normal is facing in the proper direction, considering that this may not be a two-sided triangle.
Vector3.Dot(ref triangleNormal, ref candidateNormal, out dot);
if ((triangle.sidedness == TriangleSidedness.Clockwise && dot > 0) || (triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0))
{
//Normal was facing the wrong way.
//Instead of ignoring it entirely, correct the direction to as close as it can get by removing any component parallel to the triangle normal.
Vector3 previousNormal = candidateNormal;
Vector3.Dot(ref candidateNormal, ref triangleNormal, out dot);
Vector3 p;
Vector3.Multiply(ref candidateNormal, dot, out p);
Vector3.Subtract(ref candidateNormal, ref p, out candidateNormal);
float length = candidateNormal.LengthSquared();
if (length > Toolbox.Epsilon)
{
//Renormalize the corrected normal.
Vector3.Divide(ref candidateNormal, (float)Math.Sqrt(length), out candidateNormal);
Vector3.Dot(ref candidateNormal, ref previousNormal, out dot);
candidateDepth *= dot;
}
else
{
contact.PenetrationDepth = float.MaxValue;
contact.Normal = new Vector3();
}
}
if (candidateDepth < contact.PenetrationDepth)
{
contact.Normal = candidateNormal;
contact.PenetrationDepth = candidateDepth;
}
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref CAnormal, out candidateDepth, out candidateNormal);
//Check to see if the normal is facing in the proper direction, considering that this may not be a two-sided triangle.
Vector3.Dot(ref triangleNormal, ref candidateNormal, out dot);
if ((triangle.sidedness == TriangleSidedness.Clockwise && dot > 0) || (triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0))
{
//Normal was facing the wrong way.
//Instead of ignoring it entirely, correct the direction to as close as it can get by removing any component parallel to the triangle normal.
Vector3 previousNormal = candidateNormal;
Vector3.Dot(ref candidateNormal, ref triangleNormal, out dot);
Vector3 p;
Vector3.Multiply(ref candidateNormal, dot, out p);
Vector3.Subtract(ref candidateNormal, ref p, out candidateNormal);
float length = candidateNormal.LengthSquared();
if (length > Toolbox.Epsilon)
{
//Renormalize the corrected normal.
Vector3.Divide(ref candidateNormal, (float)Math.Sqrt(length), out candidateNormal);
Vector3.Dot(ref candidateNormal, ref previousNormal, out dot);
candidateDepth *= dot;
}
else
{
contact.PenetrationDepth = float.MaxValue;
contact.Normal = new Vector3();
}
}
if (candidateDepth < contact.PenetrationDepth)
{
contact.Normal = candidateNormal;
contact.PenetrationDepth = candidateDepth;
}
//Try the depth along the positive triangle normal.
//If it's clockwise, this direction is unnecessary (the resulting normal would be invalidated by the onesidedness of the triangle).
if (triangle.sidedness != TriangleSidedness.Clockwise)
{
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref triangleNormal, out candidateDepth, out candidateNormal);
if (candidateDepth < contact.PenetrationDepth)
{
contact.Normal = candidateNormal;
contact.PenetrationDepth = candidateDepth;
}
}
//Try the depth along the negative triangle normal.
//If it's counterclockwise, this direction is unnecessary (the resulting normal would be invalidated by the onesidedness of the triangle).
if (triangle.sidedness != TriangleSidedness.Counterclockwise)
{
Vector3.Negate(ref triangleNormal, out triangleNormal);
MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref triangleNormal, out candidateDepth, out candidateNormal);
if (candidateDepth < contact.PenetrationDepth)
{
contact.Normal = candidateNormal;
contact.PenetrationDepth = candidateDepth;
}
}
}
MPRToolbox.RefinePenetration(convex, triangle, ref Toolbox.RigidIdentity, contact.PenetrationDepth, ref contact.Normal, out contact.PenetrationDepth, out contact.Normal, out contact.Position);
//It's possible for the normal to still face the 'wrong' direction according to one sided triangles.
if (triangle.sidedness != TriangleSidedness.DoubleSided)
{
Vector3.Dot(ref triangleNormal, ref contact.Normal, out dot);
if (dot < 0)
{
//Skip the add process.
goto InnerSphere;
}
}
contact.Id = -1;
if (contact.PenetrationDepth < convex.collisionMargin + triangle.collisionMargin)
{
state = CollisionState.ExternalNear; //If it's emerged from the deep contact, we can go back to using the preferred GJK method.
}
contactList.Add(ref contact);
}
InnerSphere:
if (TryInnerSphereContact(out contact))
{
contactList.Add(ref contact);
}
if (contactList.Count > 0)
return true;
state = CollisionState.ExternalSeparated;
return false;
}