public override void Update(float dt)
{
//First, refresh all existing contacts. This is an incremental manifold.
ContactRefresher.ContactRefresh(contacts, supplementData, ref convex.worldTransform, ref triangle.worldTransform, contactIndicesToRemove);
RemoveQueuedContacts();
//Compute the local triangle vertices.
//TODO: this could be quicker and cleaner.
localTriangleShape.collisionMargin = triangle.Shape.collisionMargin;
localTriangleShape.sidedness = triangle.Shape.sidedness;
Matrix3x3 orientation;
Matrix3x3.CreateFromQuaternion(ref triangle.worldTransform.Orientation, out orientation);
Matrix3x3.Transform(ref triangle.Shape.vA, ref orientation, out localTriangleShape.vA);
Matrix3x3.Transform(ref triangle.Shape.vB, ref orientation, out localTriangleShape.vB);
Matrix3x3.Transform(ref triangle.Shape.vC, ref orientation, out localTriangleShape.vC);
Vector3.Add(ref localTriangleShape.vA, ref triangle.worldTransform.Position, out localTriangleShape.vA);
Vector3.Add(ref localTriangleShape.vB, ref triangle.worldTransform.Position, out localTriangleShape.vB);
Vector3.Add(ref localTriangleShape.vC, ref triangle.worldTransform.Position, out localTriangleShape.vC);
Vector3.Subtract(ref localTriangleShape.vA, ref convex.worldTransform.Position, out localTriangleShape.vA);
Vector3.Subtract(ref localTriangleShape.vB, ref convex.worldTransform.Position, out localTriangleShape.vB);
Vector3.Subtract(ref localTriangleShape.vC, ref convex.worldTransform.Position, out localTriangleShape.vC);
Matrix3x3.CreateFromQuaternion(ref convex.worldTransform.Orientation, out orientation);
Matrix3x3.TransformTranspose(ref localTriangleShape.vA, ref orientation, out localTriangleShape.vA);
Matrix3x3.TransformTranspose(ref localTriangleShape.vB, ref orientation, out localTriangleShape.vB);
Matrix3x3.TransformTranspose(ref localTriangleShape.vC, ref orientation, out localTriangleShape.vC);
//Now, generate a contact between the two shapes.
ContactData contact;
TinyStructList<ContactData> contactList;
if (pairTester.GenerateContactCandidate(out contactList))
{
for (int i = 0; i < contactList.Count; i++)
{
contactList.Get(i, out contact);
//Put the contact into world space.
Matrix3x3.Transform(ref contact.Position, ref orientation, out contact.Position);
Vector3.Add(ref contact.Position, ref convex.worldTransform.Position, out contact.Position);
Matrix3x3.Transform(ref contact.Normal, ref orientation, out contact.Normal);
//Check if the contact is unique before proceeding.
if (IsContactUnique(ref contact))
{
//Check if adding the new contact would overflow the manifold.
if (contacts.Count == 4)
{
//Adding that contact would overflow the manifold. Reduce to the best subset.
bool addCandidate;
ContactReducer.ReduceContacts(contacts, ref contact, contactIndicesToRemove, out addCandidate);
RemoveQueuedContacts();
if (addCandidate)
Add(ref contact);
}
else
{
//Won't overflow the manifold, so just toss it in PROVIDED that it isn't too close to something else.
Add(ref contact);
}
}
}
}
else
{
//Clear out the contacts, it's separated.
for (int i = contacts.Count - 1; i >= 0; i--)
Remove(i);
}
}