private void Solve(TimeStep step)
{
Profile.SolveInit = 0;
Profile.SolveVelocity = 0;
Profile.SolvePosition = 0;
// Size the island for the worst case.
island.Init(BodyCount, ContactManager.ContactCount, JointCount, ContactManager.ContactListener);
// Clear all the island flags.
for (Body b = BodyList; b != null; b = b.Next)
{
b.Flags &= ~Body.TypeFlags.Island;
}
for (Contact c = ContactManager.ContactList; c != null; c = c.Next)
{
c.Flags &= ~Contact.ContactFlags.Island;
}
for (Joint j = JointList; j != null; j = j.Next)
{
j.IslandFlag = false;
}
// Build and simulate all awake islands.
int stackSize = BodyCount;
if (stack.Length < stackSize)
{
stack = new Body[stackSize];
}
for (Body seed = BodyList; seed != null; seed = seed.Next)
{
if ((seed.Flags & Body.TypeFlags.Island) == Body.TypeFlags.Island)
{
continue;
}
if (seed.Awake == false || seed.Active == false)
{
continue;
}
// The seed can be dynamic or kinematic.
if (seed.Type == BodyType.Static)
{
continue;
}
// Reset island and stack.
island.Clear();
int stackCount = 0;
stack[stackCount++] = seed;
seed.Flags |= Body.TypeFlags.Island;
// Perform a depth first search (DFS) on the constraint graph.
while (stackCount > 0)
{
// Grab the next body off the stack and add it to the island.
Body b = stack[--stackCount];
Debug.Assert(b.Active == true);
island.Add(b);
// Make sure the body is awake.
b.Awake = true;
// To keep islands as small as possible, we don't
// propagate islands across static bodies.
if (b.Type == BodyType.Static)
{
continue;
}
// Search all contacts connected to this body.
for (ContactEdge ce = b.ContactList; ce != null; ce = ce.Next)
{
Contact contact = ce.Contact;
// Has this contact already been added to an island?
if ((contact.Flags & Contact.ContactFlags.Island) == Contact.ContactFlags.Island)
{
continue;
}
// Is this contact solid and touching?
if (contact.Enabled == false || contact.Touching == false)
{
continue;
}
// Skip sensors.
bool sensorA = contact.FixtureA.IsSensor;
bool sensorB = contact.FixtureB.IsSensor;
if (sensorA || sensorB)
{
continue;
}
island.Add(contact);
contact.Flags |= Contact.ContactFlags.Island;
Body other = ce.Other;
// Was the other body already added to this island?
if ((other.Flags & Body.TypeFlags.Island) == Body.TypeFlags.Island)
{
continue;
}
Debug.Assert(stackCount < stackSize);
stack[stackCount++] = other;
other.Flags |= Body.TypeFlags.Island;
}
// Search all joints connect to this body.
for (JointEdge je = b.JointList; je != null; je = je.Next)
{
if (je.Joint.IslandFlag)
{
continue;
}
Body other = je.Other;
// Don't simulate joints connected to inactive bodies.
if (other.Active == false)
{
continue;
}
island.Add(je.Joint);
je.Joint.IslandFlag = true;
if ((other.Flags & Body.TypeFlags.Island) == Body.TypeFlags.Island)
{
continue;
}
Debug.Assert(stackCount < stackSize);
stack[stackCount++] = other;
other.Flags |= Body.TypeFlags.Island;
}
}
island.Solve(islandProfile, step, m_gravity, SleepingAllowed);
Profile.SolveInit += islandProfile.SolveInit;
Profile.SolveVelocity += islandProfile.SolveVelocity;
Profile.SolvePosition += islandProfile.SolvePosition;
// Post solve cleanup.
for (int i = 0; i < island.BodyCount; ++i)
{
// Allow static bodies to participate in other islands.
Body b = island.Bodies[i];
if (b.Type == BodyType.Static)
{
b.Flags &= ~Body.TypeFlags.Island;
}
}
}
broadphaseTimer.Reset();
// Synchronize fixtures, check for out of range bodies.
for (Body b = BodyList; b != null; b = b.Next)
{
// If a body was not in an island then it did not move.
if ((b.Flags & Body.TypeFlags.Island) == 0)
{
continue;
}
if (b.Type == BodyType.Static)
{
continue;
}
// Update fixtures (for broad-phase).
b.SynchronizeFixtures();
}
// Look for new contacts.
ContactManager.FindNewContacts();
Profile.Broadphase = broadphaseTimer.Milliseconds;
}