private bool DoPlaneTest(out TinyStructList<ContactData> contactList)
{
//Find closest point between object and plane.
Vector3 reverseNormal;
Vector3 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 ac, ref ab, out reverseNormal);
//Convex position dot normal is ALWAYS zero. The thing to look at is the plane's 'd'.
//If the distance along the normal is positive, then the convex is 'behind' that normal.
float dotA;
Vector3.Dot(ref triangle.vA, ref reverseNormal, out dotA);
contactList = new TinyStructList<ContactData>();
switch (triangle.sidedness)
{
case TriangleSidedness.DoubleSided:
if (dotA < 0)
{
//The reverse normal is pointing towards the convex.
//It needs to point away from the convex so that the direction
//will get the proper extreme point.
Vector3.Negate(ref reverseNormal, out reverseNormal);
dotA = -dotA;
}
break;
case TriangleSidedness.Clockwise:
//if (dotA < 0)
//{
// //The reverse normal is pointing towards the convex.
// return false;
//}
break;
case TriangleSidedness.Counterclockwise:
//if (dotA > 0)
//{
// //The reverse normal is pointing away from the convex.
// return false;
//}
//The reverse normal is pointing towards the convex.
//It needs to point away from the convex so that the direction
//will get the proper extreme point.
Vector3.Negate(ref reverseNormal, out reverseNormal);
dotA = -dotA;
break;
}
Vector3 extremePoint;
convex.GetLocalExtremePointWithoutMargin(ref reverseNormal, out extremePoint);
//See if the extreme point is within the face or not.
//It might seem like the easy "depth" test should come first, since a barycentric
//calculation takes a bit more time. However, transferring from plane to depth is 'rare'
//(like all transitions), and putting this test here is logically closer to its requirements'
//computation.
if (GetVoronoiRegion(ref extremePoint) != VoronoiRegion.ABC)
{
state = CollisionState.ExternalSeparated;
return DoExternalSeparated(out contactList);
}
float dotE;
Vector3.Dot(ref extremePoint, ref reverseNormal, out dotE);
float t = (dotA - dotE) / reverseNormal.LengthSquared();
Vector3 offset;
Vector3.Multiply(ref reverseNormal, t, out offset);
//Compare the distance from the plane to the convex object.
float distanceSquared = offset.LengthSquared();
float marginSum = triangle.collisionMargin + convex.collisionMargin;
//TODO: Could just normalize early and avoid computing point plane before it's necessary.
//Exposes a sqrt but...
if (t <= 0 || distanceSquared < marginSum * marginSum)
{
//The convex object is in the margin of the plane.
//All that's left is to create the contact.
var contact = new ContactData();
//Displacement is from A to B. point = A + t * AB, where t = marginA / margin.
if (marginSum > Toolbox.Epsilon) //This can be zero! It would cause a NaN is unprotected.
Vector3.Multiply(ref offset, convex.collisionMargin / marginSum, out contact.Position); //t * AB
else contact.Position = new Vector3();
Vector3.Add(ref extremePoint, ref contact.Position, out contact.Position); //A + t * AB.
float normalLength = reverseNormal.Length();
Vector3.Divide(ref reverseNormal, normalLength, out contact.Normal);
float distance = normalLength * t;
contact.PenetrationDepth = marginSum - distance;
if (contact.PenetrationDepth > marginSum)
{
//Check to see if the inner sphere is touching the plane.
//This does not override other tests; there can be more than one contact from a single triangle.
ContactData alternateContact;
if (TryInnerSphereContact(out alternateContact))// && alternateContact.PenetrationDepth > contact.PenetrationDepth)
{
contactList.Add(ref alternateContact);
}
//The convex object is stuck deep in the plane!
//The most problematic case for this is when
//an object is right on top of a cliff.
//The lower, vertical triangle may occasionally detect
//a contact with the object, but would compute an extremely
//deep depth if the normal plane test was used.
//Verify that the depth is correct by trying another approach.
CollisionState previousState = state;
state = CollisionState.ExternalNear;
TinyStructList<ContactData> alternateContacts;
if (DoExternalNear(out alternateContacts))
{
alternateContacts.Get(0, out alternateContact);
if (alternateContact.PenetrationDepth + .01f < contact.PenetrationDepth) //Bias against the subtest's result, since the plane version will probably have a better position.
{
//It WAS a bad contact.
contactList.Add(ref alternateContact);
//DoDeepContact (which can be called from within DoExternalNear) can generate two contacts, but the second contact would just be an inner sphere (which we already generated).
//DoExternalNear can only generate one contact. So we only need the first contact!
//TODO: This is a fairly fragile connection between the two stages. Consider robustifying. (Also, the TryInnerSphereContact is done twice! This process is very rare for marginful pairs, though)
}
else
{
//Well, it really is just that deep.
contactList.Add(ref contact);
state = previousState;
}
}
else
{
//If the external near test finds that there was no collision at all,
//just return to plane testing. If the point turns up outside the face region
//next time, the system will adapt.
state = previousState;
return false;
}
}
else
{
contactList.Add(ref contact);
}
return true;
}
return false;
}
