// 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);
}
}