// 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 a specific contact
public void RemoveContact(ContactConstraint contact)
{
Body A = contact.bodyA;
Body B = contact.bodyB;
// Remove from A
A.ContactList.Remove(contact.edgeA);
// Remove from B
B.ContactList.Remove(contact.edgeB);
A.SetToAwake();
B.SetToAwake();
// Remove contact from the manager
ContactList.Remove(contact);
}
// Run the simulation forward in time by dt (fixed timestep). Variable
// timestep is not supported.
public void Step(double Dt)
{
if (NewBox)
{
ContactManager.Broadphase.UpdatePairs();
NewBox = false;
}
ContactManager.TestCollisions();
foreach (var body in Bodies)
{
body.Flags &= ~BodyFlags.eIsland;
}
Island.AllowSleep = AllowSleep;
Island.EnableFriction = EnableFriction;
Island.Dt = Dt;
Island.Gravity = Gravity;
Island.Iterations = Iterations;
// Build each active Island and then solve each built Island
// int stackSize = Bodies.Count;
foreach (var seed in Bodies)
{
// Seed cannot be apart of an Island already
if ((seed.Flags & BodyFlags.eIsland) > 0)
{
continue;
}
// Seed must be awake
if ((seed.Flags & BodyFlags.eAwake) == 0)
{
continue;
}
// Seed cannot be a static body in order to keep islands
// as small as possible
if ((seed.Flags & BodyFlags.eStatic) > 0)
{
continue;
}
int stackCount = 0;
stack[stackCount++] = seed;
Island.Clear();
// Mark seed as apart of Island
seed.Flags |= BodyFlags.eIsland;
// Perform DFS on constraint graph
while (stackCount > 0)
{
// Decrement stack to implement iterative backtracking
Body body = stack[--stackCount];
Island.Add(body);
// Awaken all bodies connected to the Island
body.SetToAwake();
// Do not search across static bodies to keep Island
// formations as small as possible, however the static
// body itself should be apart of the Island in order
// to properly represent a full contact
if ((body.Flags & BodyFlags.eStatic) > 0)
{
continue;
}
// Search all contacts connected to this body
foreach (var edge in body.ContactList)
{
ContactConstraint contact = edge.constraint;
// Skip contacts that have been added to an Island already
if ((contact.Flags & ContactFlags.eIsland) > 0)
{
continue;
}
// Can safely skip this contact if it didn't actually collide with anything
if ((contact.Flags & ContactFlags.eColliding) == 0)
{
continue;
}
// Skip sensors
if (contact.A.sensor || contact.B.sensor)
{
continue;
}
// Mark Island flag and add to Island
contact.Flags |= ContactFlags.eIsland;
Island.Add(contact);
// Attempt to add the other body in the contact to the Island
// to simulate contact awakening propogation
Body other = edge.other;
if ((other.Flags & BodyFlags.eIsland) > 0)
{
continue;
}
Assert(stackCount < 256);
stack[stackCount++] = other;
other.Flags |= BodyFlags.eIsland;
}
}
Assert(Island.Bodies.Count != 0);
Island.Initialize();
Island.Solve();
// Reset all static Island flags
// This allows static bodies to participate in other Island formations
foreach (var body in Island.Bodies)
{
if ((body.Flags & BodyFlags.eStatic) > 0)
{
body.Flags &= ~BodyFlags.eIsland;
}
}
}
// Update the broadphase AABBs
foreach (var body in Bodies)
{
if ((body.Flags & BodyFlags.eStatic) > 0)
{
continue;
}
body.SynchronizeProxies();
}
// Look for new contacts
ContactManager.FindNewContacts();
// Clear all forces
foreach (var body in Bodies)
{
Vec3.Identity(ref body.Force);
Vec3.Identity(ref body.Torque);
}
}
