// Find islands, integrate and solve constraints, solve position constraints private void Solve(TimeStep step) { // Size the island for the worst case. Island island = new Island(_bodyCount, _contactManager._contactCount, _jointCount, _contactManager._contactListener); // Clear all the island flags. for (Body b = _bodyList; b != null; b = b._next) { b._flags &= ~Body.BodyFlags.Island; } for (Contact c = _contactManager._contactList; c != null; c = c.Next) { c.Flags &= ~ContactFlag.IslandFlag; } for (Joint j = _jointList; j != null; j = j._next) { j._islandFlag = false; } // Build and simulate all awake islands. int stackSize = _bodyCount; Body[] stack = new Body[stackSize]; for (Body seed = _bodyList; seed != null; seed = seed._next) { if ((seed._flags & (Body.BodyFlags.Island | Body.BodyFlags.Sleep)) != 0) { continue; } if (seed.IsStatic()) { continue; } // Reset island and stack. island.Clear(); int stackCount = 0; stack[stackCount++] = seed; seed._flags |= Body.BodyFlags.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]; island.Add(ref b); // Make sure the body is awake. b._flags &= ~Body.BodyFlags.Sleep; // To keep islands as small as possible, we don't // propagate islands across static bodies. if (b.IsStatic()) { continue; } // Search all contacts connected to this body. for (ContactEdge ce = b._contactList; ce != null; ce = ce.Next) { // Has this contact already been added to an island? if ((ce.Contact.Flags & ContactFlag.IslandFlag) != 0) { continue; } // Is this contact touching? if (ce.Contact.IsSolid() == false || ce.Contact.IsTouching() == false) { continue; } island.Add(ref ce.Contact); ce.Contact.Flags |= ContactFlag.IslandFlag; Body other = ce.Other; // Was the other body already added to this island? if ((other._flags & Body.BodyFlags.Island) != 0) { continue; } Box2DXDebug.Assert(stackCount < stackSize); stack[stackCount++] = other; other._flags |= Body.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; } island.Add(je.Joint); je.Joint._islandFlag = true; Body other = je.Other; if ((other._flags & Body.BodyFlags.Island) != 0) { continue; } Box2DXDebug.Assert(stackCount < stackSize); stack[stackCount++] = other; other._flags |= Body.BodyFlags.Island; } } island.Solve(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.IsStatic()) { b._flags &= ~Body.BodyFlags.Island; } } } stack = null; // Synchronize shapes, check for out of range bodies. for (Body b = _bodyList; b != null; b = b.GetNext()) { if ((b._flags & Body.BodyFlags.Sleep) != 0) { continue; } if (b.IsStatic()) { continue; } // Update fixtures (for broad-phase). b.SynchronizeFixtures(); } // Look for new contacts. _contactManager.FindNewContacts(); }
public void DrawDebugData() { if (_debugDraw == null) { return; } DebugDraw.DrawFlags flags = _debugDraw.Flags; if ((flags & DebugDraw.DrawFlags.Shape) != 0) { for (Body b = _bodyList; b != null; b = b.GetNext()) { Transform xf = b.GetTransform(); for (Fixture f = b.GetFixtureList(); f != null; f = f.GetNext()) { if (b.IsStatic()) { DrawShape(f, xf, new Color(0.5f, 0.9f, 0.5f)); } else if (b.IsSleeping()) { DrawShape(f, xf, new Color(0.5f, 0.5f, 0.9f)); } else { DrawShape(f, xf, new Color(0.9f, 0.9f, 0.9f)); } } } } if ((flags & DebugDraw.DrawFlags.Joint) != 0) { for (Joint j = _jointList; j != null; j = j.GetNext()) { if (j.GetType() != JointType.MouseJoint) { DrawJoint(j); } } } if ((flags & DebugDraw.DrawFlags.Pair) != 0) { // TODO_ERIN } if ((flags & DebugDraw.DrawFlags.Aabb) != 0) { Color color = new Color(0.9f, 0.3f, 0.9f); BroadPhase bp = _contactManager._broadPhase; for (Body b = _bodyList; b != null; b = b.GetNext()) { for (Fixture f = b.GetFixtureList(); f != null; f = f.GetNext()) { AABB aabb = bp.GetFatAABB(f.ProxyId); Vec2[] vs = new Vec2[4]; vs[0].Set(aabb.LowerBound.X, aabb.LowerBound.Y); vs[1].Set(aabb.UpperBound.X, aabb.LowerBound.Y); vs[2].Set(aabb.UpperBound.X, aabb.UpperBound.Y); vs[3].Set(aabb.LowerBound.X, aabb.UpperBound.Y); _debugDraw.DrawPolygon(vs, 4, color); } } } if ((flags & DebugDraw.DrawFlags.CenterOfMass) != 0) { for (Body b = _bodyList; b != null; b = b.GetNext()) { Transform xf = b.GetTransform(); xf.Position = b.GetWorldCenter(); _debugDraw.DrawXForm(xf); } } }