// Add a new contact constraint for a pair of objects
// unless the contact constraint already exists
public void AddContact(Box A, Box B)
{
Body bodyA = A.body;
Body bodyB = B.body;
if (!bodyA.CanCollide(bodyB))
{
return;
}
// Search for existing matching contact
// Return if found duplicate to avoid duplicate constraints
// Mark pre-existing duplicates as active
foreach (var edge in A.body.ContactList)
{
if (edge.other == bodyB)
{
Box shapeA = edge.constraint.A;
Box shapeB = edge.constraint.B;
// @TODO: Verify this against Box2D; not sure if this is all we need here
if ((A == shapeA) && (B == shapeB))
{
return;
}
}
}
// Create new contact
ContactConstraint contact = new ContactConstraint()
{
A = A,
B = B,
bodyA = A.body,
bodyB = B.body,
Flags = 0,
friction = MixFriction(A, B),
restitution = MixRestitution(A, B),
};
contact.manifold.SetPair(A, B);
ContactList.Add(contact);
// Connect A
contact.edgeA.constraint = contact;
contact.edgeA.other = bodyB;
bodyA.ContactList.Add(contact.edgeA);
// Connect B
contact.edgeB.constraint = contact;
contact.edgeB.other = bodyA;
bodyB.ContactList.Add(contact.edgeB);
bodyA.SetToAwake();
bodyB.SetToAwake();
}
// 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);
}
}
}
}
