void Solve(ref TimeStep step) { // Size the island for the worst case. _island.Reset(_bodyCount, _contactManager._contactCount, _jointCount, _contactManager.ContactListener); // Clear all the island flags. for (Body b = _bodyList; b != null; b = b._next) { b._flags &= ~BodyFlags.Island; } for (Contact c = _contactManager._contactList; c != null; c = c._next) { c._flags &= ~ContactFlags.Island; } for (Joint j = _jointList; j != null; j = j._next) { j._islandFlag = false; } // Build and simulate all awake islands. int stackSize = _bodyCount; if (stackSize > stack.Length) { stack = new Body[Math.Max(stack.Length * 2, stackSize)]; } for (Body seed = _bodyList; seed != null; seed = seed._next) { if ((seed._flags & (BodyFlags.Island)) != BodyFlags.None) { continue; } if (seed.IsAwake() == false || seed.IsActive() == false) { continue; } // The seed can be dynamic or kinematic. if (seed.GetType() == BodyType.Static) { continue; } // Reset island and stack. _island.Clear(); int stackCount = 0; stack[stackCount++] = seed; seed._flags |= BodyFlags.Island; // Perform a depth first search (DFS) on the raint graph. while (stackCount > 0) { // Grab the next body off the stack and add it to the island. Body b = stack[--stackCount]; //Debug.Assert(b.IsActive() == true); _island.Add(b); // Make sure the body is awake. b.SetAwake(true); // To keep islands as small as possible, we don't // propagate islands across static bodies. if (b.GetType() == 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 & ContactFlags.Island) != ContactFlags.None) { continue; } // Is this contact solid and touching? if (!ce.Contact.IsEnabled() || !ce.Contact.IsTouching()) { continue; } // Skip sensors. bool sensorA = contact._fixtureA._isSensor; bool sensorB = contact._fixtureB._isSensor; if (sensorA || sensorB) { continue; } _island.Add(contact); contact._flags |= ContactFlags.Island; Body other = ce.Other; // Was the other body already added to this island? if ((other._flags & BodyFlags.Island) != BodyFlags.None) { continue; } //Debug.Assert(stackCount < stackSize); stack[stackCount++] = other; other._flags |= BodyFlags.Island; } // Search all joints connect to this body. for (JointEdge je = b._jointList; je != null; je = je.Next) { if (je.Joint._islandFlag == true) { continue; } Body other = je.Other; // Don't simulate joints connected to inactive bodies. if (other.IsActive() == false) { continue; } _island.Add(je.Joint); je.Joint._islandFlag = true; if ((other._flags & BodyFlags.Island) != BodyFlags.None) { continue; } //Debug.Assert(stackCount < stackSize); stack[stackCount++] = other; other._flags |= BodyFlags.Island; } } _island.Solve(ref step, Gravity, _allowSleep); // 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.GetType() == BodyType.Static) { b._flags &= ~BodyFlags.Island; } } } // Synchronize fixtures, check for out of range bodies. for (Body b = _bodyList; b != null; b = b.GetNext()) { // If a body was not in an island then it did not move. if ((b._flags & BodyFlags.Island) != BodyFlags.Island) { continue; } if (b.GetType() == BodyType.Static) { continue; } // Update fixtures (for broad-phase). b.SynchronizeFixtures(); } // Look for new contacts. _contactManager.FindNewContacts(); }
private void Solve(TimeStep step) { m_profile.solveInit = 0.0f; m_profile.solveVelocity = 0.0f; m_profile.solvePosition = 0.0f; // Size the island for the worst case. Island island = new Island(m_contactManager.m_contactListener); // Clear all the island flags. foreach (Body b in m_bodyList) { b.m_flags &= ~Body.BodyFlags.e_islandFlag; } foreach (Contact c in m_contactManager.m_contactList) { c.m_flags &= ~ContactFlags.e_islandFlag; } foreach (Joint j in m_jointList) { j.m_islandFlag = false; } // Build and simulate all awake islands. List <Body> stack = new List <Body>(m_bodyList.Count()); foreach (Body seed in m_bodyList) { if (seed.m_flags.HasFlag(Body.BodyFlags.e_islandFlag)) { continue; } if (seed.IsAwake() == false || seed.IsActive() == false) { continue; } // The seed can be dynamic or kinematic. if (seed.GetBodyType() == BodyType._staticBody) { continue; } // Reset island and stack. island.Clear(); int stackCount = 0; stack.Add(seed); stackCount++; seed.m_flags |= Body.BodyFlags.e_islandFlag; // 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]; Utilities.Assert(b.IsActive() == true); island.Add(b); // Make sure the body is awake. b.SetAwake(true); // To keep islands as small as possible, we don't // propagate islands across static bodies. if (b.GetBodyType() == BodyType._staticBody) { continue; } // Search all contacts connected to this body. foreach (ContactEdge ce in b.m_contactList) { Contact contact = ce.contact; // Has this contact already been added to an island? if (contact.m_flags.HasFlag(ContactFlags.e_islandFlag)) { continue; } // Is this contact solid and touching? if (contact.IsEnabled() == false || contact.IsTouching() == false) { continue; } // Skip sensors. bool sensorA = contact.m_fixtureA.m_isSensor; bool sensorB = contact.m_fixtureB.m_isSensor; if (sensorA || sensorB) { continue; } island.Add(contact); contact.m_flags |= ContactFlags.e_islandFlag; Body other = ce.other; // Was the other body already added to this island? if (other.m_flags.HasFlag(Body.BodyFlags.e_islandFlag)) { continue; } Utilities.Assert(stackCount < m_bodyList.Count()); stack.Add(other); stackCount++; other.m_flags |= Body.BodyFlags.e_islandFlag; } // Search all joints connect to this body. foreach (JointEdge je in b.m_jointList) { if (je.joint.m_islandFlag == true) { continue; } Body other = je.other; // Don't simulate joints connected to inactive bodies. if (other.IsActive() == false) { continue; } island.Add(je.joint); je.joint.m_islandFlag = true; if (other.m_flags.HasFlag(Body.BodyFlags.e_islandFlag)) { continue; } stack.Add(other); stackCount++; other.m_flags |= Body.BodyFlags.e_islandFlag; } } Profile profile = new Profile(); island.Solve(profile, step, m_gravity, m_allowSleep); m_profile.solveInit += profile.solveInit; m_profile.solveVelocity += profile.solveVelocity; m_profile.solvePosition += profile.solvePosition; // Post solve cleanup. for (int i = 0; i < island.m_bodies.Count(); ++i) { // Allow static bodies to participate in other islands. Body b = island.m_bodies[i]; if (b.GetBodyType() == BodyType._staticBody) { b.m_flags &= ~Body.BodyFlags.e_islandFlag; } } } { Timer timer = new Timer(); // Synchronize fixtures, check for out of range bodies. foreach (Body b in m_bodyList) { // If a body was not in an island then it did not move. if ((b.m_flags & Body.BodyFlags.e_islandFlag) == 0) { continue; } if (b.GetBodyType() == BodyType._staticBody) { continue; } // Update fixtures (for broad-phase). b.SynchronizeFixtures(); } // Look for new contacts. m_contactManager.FindNewContacts(); m_profile.broadphase = timer.GetMilliseconds(); } }