// Remove contacts without broadphase overlap
// Solves contact manifolds
public void TestCollisions()
{
for (int h = 0; h < ContactList.Count; h++)
{
var constraint = ContactList[h];
Box A = constraint.A;
Box B = constraint.B;
Body bodyA = A.body;
Body bodyB = B.body;
constraint.Flags &= ~ContactFlags.eIsland;
if (!bodyA.IsAwake() && !bodyB.IsAwake())
{
continue;
}
if (!bodyA.CanCollide(bodyB))
{
RemoveContact(constraint);
continue;
}
// Check if contact should persist
if (!Broadphase.TestOverlap(A.broadPhaseIndex, B.broadPhaseIndex))
{
RemoveContact(constraint);
continue;
}
Manifold manifold = constraint.manifold;
Manifold oldManifold = constraint.manifold;
Vec3 ot0 = oldManifold.tangentVectors;
Vec3 ot1 = oldManifold.bitangentVectors;
constraint.SolveCollision();
AABB.ComputeBasis(manifold.normal, ref manifold.tangentVectors, ref manifold.bitangentVectors);
for (int i = 0; i < manifold.contactCount; ++i)
{
Contact c = manifold.contacts[i];
c.tangentImpulse = c.bitangentImpulse = c.normalImpulse = 0;
byte oldWarmStart = c.warmStarted;
c.warmStarted = 0;
for (int j = 0; j < oldManifold.contactCount; ++j)
{
Contact oc = oldManifold.contacts[j];
if (c.fp.key == oc.fp.key)
{
c.normalImpulse = oc.normalImpulse;
// Attempt to re-project old friction solutions
Vec3 friction = ot0 * oc.tangentImpulse + ot1 * oc.bitangentImpulse;
c.tangentImpulse = Vec3.Dot(friction, manifold.tangentVectors);
c.bitangentImpulse = Vec3.Dot(friction, manifold.bitangentVectors);
c.warmStarted = Math.Max(oldWarmStart, (byte)(oldWarmStart + 1));
break;
}
}
}
if (ContactListener != null)
{
var now_colliding = constraint.Flags & ContactFlags.eColliding;
var was_colliding = constraint.Flags & ContactFlags.eWasColliding;
if (now_colliding > 0 && was_colliding == 0)
{
ContactListener.BeginContact(constraint);
}
else if (now_colliding == 0 && was_colliding > 0)
{
ContactListener.EndContact(constraint);
}
}
}
}
