/// Call this after you are done with time steps to clear the forces. you normally
/// call this after each call to Step, unless you are performing sub-steps. By default,
/// forces will be automatically cleared, so you don't need to call this function.
/// @see SetAutoClearForces
public void ClearForces()
{
for (Body body = _bodyList; body != null; body = body.GetNext())
{
body._force = Vector2.zero;
body._torque = 0.0f;
}
}
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();
}
/// 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())
{
Transform xf;
b.GetTransform(out xf);
for (Fixture f = b.GetFixtureList(); f != null; f = f.GetNext())
{
if (b.IsActive() == false)
{
DrawShape(f, xf, new Color(0.5f, 0.5f, 0.3f));
}
else if (b.GetType() == BodyType.Static)
{
DrawShape(f, xf, new Color(0.5f, 0.9f, 0.5f));
}
else if (b.GetType() == BodyType.Kinematic)
{
DrawShape(f, xf, new Color(0.5f, 0.5f, 0.9f));
}
else if (b.IsAwake() == false)
{
DrawShape(f, xf, new Color(0.6f, 0.6f, 0.6f));
}
else
{
DrawShape(f, xf, new Color(0.9f, 0.7f, 0.7f));
}
}
}
}
if ((flags & DebugDrawFlags.Joint) == DebugDrawFlags.Joint)
{
for (Joint j = _jointList; j != null; j = j.GetNext())
{
DrawJoint(j);
}
}
if ((flags & DebugDrawFlags.Pair) == DebugDrawFlags.Pair)
{
Color color = new Color(0.3f, 0.9f, 0.9f);
for (Contact c = _contactManager._contactList; c != null; c = c.GetNext())
{
/*
* Fixture fixtureA = c.GetFixtureA();
* Fixture fixtureB = c.GetFixtureB();
*
* AABB aabbA;
* AABB aabbB;
* fixtureA.GetAABB(out aabbA);
* fixtureB.GetAABB(out aabbB);
*
* Vector2 cA = aabbA.GetCenter();
* Vector2 cB = aabbB.GetCenter();
*
* DebugDraw.DrawSegment(cA, cB, color);
*/
}
}
if ((flags & DebugDrawFlags.AABB) == DebugDrawFlags.AABB)
{
Color color = new Color(0.9f, 0.3f, 0.9f);
BroadPhase bp = _contactManager._broadPhase;
for (Body b = _bodyList; b != null; b = b.GetNext())
{
if (b.IsActive() == false)
{
continue;
}
for (Fixture f = b.GetFixtureList(); f != null; f = f.GetNext())
{
for (int i = 0; i < f._proxyCount; ++i)
{
FixtureProxy proxy = f._proxies[i];
AABB aabb;
bp.GetFatAABB(proxy.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())
{
Transform xf;
b.GetTransform(out xf);
xf.Position = b.GetWorldCenter();
DebugDraw.DrawTransform(ref xf);
}
}
}
