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. //#warning Remove extra allocs int stackSize = _bodyCount; Body[] stack = new Body[_bodyCount]; for (Body seed = _bodyList; seed != null; seed = seed._next) { if ((seed._flags & (BodyFlags.Island | BodyFlags.Sleep | BodyFlags.Frozen)) != BodyFlags.None) { continue; } if (seed.IsStatic) { 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]; _island.Add(b); // Make sure the body is awake. b._flags &= ~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? // Is this contact non-solid (involves a sensor). if ((ce.Contact._flags & (ContactFlags.Island | ContactFlags.NonSolid)) != ContactFlags.None) { continue; } // Is this contact touching? if ((ce.Contact._flags & ContactFlags.Touch) == ContactFlags.None) { continue; } _island.Add(ce.Contact); ce.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; } _island.Add(je.Joint); je.Joint._islandFlag = true; Body other = je.Other; 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.IsStatic) { b._flags &= ~BodyFlags.Island; } } } // Synchronize fixtures, check for out of range bodies. for (Body b = _bodyList; b != null; b = b.GetNext()) { if ((b._flags & (BodyFlags.Sleep | BodyFlags.Frozen)) != BodyFlags.None) { continue; } if (b.IsStatic) { continue; } // Update fixtures (for broad-phase). b.SynchronizeFixtures(); } // Look for new contacts. _contactManager.FindNewContacts(); }
/// Call this to draw shapes and other debug draw data. public void DrawDebugData() { if (DebugDraw == null) { return; } DebugDrawFlags flags = DebugDraw.Flags; if ((flags & DebugDrawFlags.Shape) == DebugDrawFlags.Shape) { for (Body b = _bodyList; b != null; b = b.GetNext()) { XForm xf; b.GetXForm(out xf); for (Fixture f = b.GetFixtureList(); f != null; f = f.GetNext()) { if (b.IsStatic) { DrawShape(f, xf, ColorEx.FromScRgb(0.5f, 0.9f, 0.5f)); } else if (b.IsSleeping) { DrawShape(f, xf, ColorEx.FromScRgb(0.5f, 0.5f, 0.9f)); } else { DrawShape(f, xf, ColorEx.FromScRgb(0.9f, 0.9f, 0.9f)); } } } } if ((flags & DebugDrawFlags.Joint) == DebugDrawFlags.Joint) { for (Joint j = _jointList; j != null; j = j.GetNext()) { if (j.JointType != JointType.Mouse) { DrawJoint(j); } } } if ((flags & DebugDrawFlags.Pair) == DebugDrawFlags.Pair) { // TODO } if ((flags & DebugDrawFlags.AABB) == DebugDrawFlags.AABB) { Color color = ColorEx.FromScRgb(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.GetAABB(f._proxyId, out aabb); FixedArray8 <Vector2> vs = new FixedArray8 <Vector2>(); vs[0] = new Vector2(aabb.lowerBound.X, aabb.lowerBound.Y); vs[1] = new Vector2(aabb.upperBound.X, aabb.lowerBound.Y); vs[2] = new Vector2(aabb.upperBound.X, aabb.upperBound.Y); vs[3] = new Vector2(aabb.lowerBound.X, aabb.upperBound.Y); DebugDraw.DrawPolygon(ref vs, 4, color); } } } if ((flags & DebugDrawFlags.CenterOfMass) == DebugDrawFlags.CenterOfMass) { for (Body b = _bodyList; b != null; b = b.GetNext()) { XForm xf; b.GetXForm(out xf); xf.Position = b.GetWorldCenter(); DebugDraw.DrawXForm(ref xf); } } }