public void DrawDebugData() { if (m_debugDraw == null) { return; } b2DrawFlags flags = m_debugDraw.GetFlags(); if (flags & b2DrawFlags.e_shapeBit) { for (b2Body b = m_bodyList; b; b = b.Next) { b2Transform xf = b.Transform; for (b2Fixture f = b.FixtureList; f != null; f = f.Next) { if (b.IsActive() == false) { DrawShape(f, xf, new b2Color(0.5f, 0.5f, 0.3f)); } else if (b.GetType() == b2BodyType.b2_staticBody) { DrawShape(f, xf, new b2Color(0.5f, 0.9f, 0.5f)); } else if (b.GetType() == b2BodyType.b2_kinematicBody) { DrawShape(f, xf, new b2Color(0.5f, 0.5f, 0.9f)); } else if (b.IsAwake() == false) { DrawShape(f, xf, new b2Color(0.6f, 0.6f, 0.6f)); } else { DrawShape(f, xf, new b2Color(0.9f, 0.7f, 0.7f)); } } } } if (flags.HasFlag(b2DrawFlags.e_jointBit)) { for (b2Joint j = m_jointList; j != null; j = j.GetNext()) { DrawJoint(j); } } if (flags.HasFlag(b2DrawFlags.e_pairBit)) { b2Color color = new b2Color(0.3f, 0.9f, 0.9f); for (b2Contact c = m_contactManager.ContactList; c != null; c = c.Next) { //b2Fixture fixtureA = c.GetFixtureA(); //b2Fixture fixtureB = c.GetFixtureB(); //b2Vec2 cA = fixtureA.GetAABB().GetCenter(); //b2Vec2 cB = fixtureB.GetAABB().GetCenter(); //m_debugDraw.DrawSegment(cA, cB, color); } } if (flags.HasFlag(b2DrawFlags.e_aabbBit)) { b2Color color(0.9f, 0.3f, 0.9f); b2BroadPhase bp = m_contactManager.BroadPhase; for (b2Body b = m_bodyList; b != null; b = b.Next) { if (b.IsActive() == false) { continue; } for (b2Fixture f = b.FixtureList; f != null; f = f.Next) { for (int i = 0; i < f.ProxyCount; ++i) { b2FixtureProxy proxy = f.Proxies[i]; b2AABB aabb = bp.GetFatAABB(proxy.proxyId); b2Vec2[] vs = new b2Vec2[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); m_debugDraw.DrawPolygon(vs, 4, color); } } } } if (flags.HasFlag(b2DrawFlags.e_centerOfMassBit)) { for (b2Body b = m_bodyList; b != null; b = b.Next) { b2Transform xf = b.Transform; xf.p = b.WorldCenter; m_debugDraw.DrawTransform(xf); } } }
// Find islands, integrate and solveraints, solve positionraints public void Solve(b2TimeStep step) { m_profile.solveInit = 0.0f; m_profile.solveVelocity = 0.0f; m_profile.solvePosition = 0.0f; // Size the island for the worst case. b2Island island = new b2Island(m_bodyCount, m_contactManager.ContactCount, m_jointCount, m_contactManager.ContactListener); // Clear all the island flags. for (b2Body b = m_bodyList; b != null; b = b.Next) { b.BodyFlags &= ~b2BodyFlags.e_islandFlag; } for (b2Contact c = m_contactManager.ContactList; c != null; c = c.Next) { c.ContactFlags &= ~b2ContactFlags.e_islandFlag; } for (b2Joint j = m_jointList; j; j = j.Next) { j.m_islandFlag = false; } // Build and simulate all awake islands. int stackSize = m_bodyCount; b2Body[] stack = new b2Body[stackSize]; for (b2Body seed = m_bodyList; seed != null; seed = seed.Next) { if (seed.BodyFlags & b2BodyFlags.e_islandFlag) { continue; } if (seed.IsAwake() == false || seed.IsActive() == false) { continue; } // The seed can be dynamic or kinematic. if (seed.BodyType == b2BodyType.b2_staticBody) { continue; } // Reset island and stack. island.Clear(); int stackCount = 0; stack[stackCount++] = seed; seed.BodyFlags |= b2BodyFlags.e_islandFlag; // Perform a depth first search (DFS) on theraint graph. while (stackCount > 0) { // Grab the next body off the stack and add it to the island. b2Body b = stack[--stackCount]; 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.BodyType == b2BodyType.b2_staticBody) { continue; } // Search all contacts connected to this body. for (b2ContactEdge ce = b.ContactList; ce != null; ce = ce.next) { b2Contact contact = ce.contact; // Has this contact already been added to an island? if (contact.ContactFlags & b2ContactFlags.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.ContactFlags |= b2ContactType.e_islandFlag; b2Body other = ce.other; // Was the other body already added to this island? if ((other.BodyFlags & b2BodyFlags.e_islandFlag) > 0) { continue; } stack[stackCount++] = other; other.BodyFlags |= b2BodyFlags.e_islandFlag; } // Search all joints connect to this body. for (b2JointEdge je = b.JointList; je; je = je.next) { if (je.joint.IslandFlag == true) { continue; } b2Body 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.BodyFlags & b2BodyFlags.e_islandFlag) > 0) { continue; } stack[stackCount++] = other; other.BodyFlags |= b2BodyFlags.e_islandFlag; } } b2Profile profile = island.Solve(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_bodyCount; ++i) { // Allow static bodies to participate in other islands. b2Body b = island.m_bodies[i]; if (b.BodyType == b2BodyType.b2_staticBody) { b.BodyFlags &= ~b2BodyFlags.e_islandFlag; } } } { b2Timer timer; // Synchronize fixtures, check for out of range bodies. for (b2Body b = m_bodyList; b != null; b = b.Next) { // If a body was not in an island then it did not move. if ((b.BodyFlags & b2BodyType.e_islandFlag) == 0) { continue; } if (b.GetBodyType() == b2BodyType.b2_staticBody) { continue; } // Update fixtures (for broad-phase). b.SynchronizeFixtures(); } // Look for new contacts. m_contactManager.FindNewContacts(); m_profile.broadphase = timer.GetMilliseconds(); } }
public JObject B2n(b2Body body) { JObject bodyValue = new JObject(); string bodyName = GetBodyName(body); if (null != bodyName) bodyValue["name"] = bodyName; switch (body.BodyType) { case b2BodyType.b2_staticBody: bodyValue["type"] = 0; break; case b2BodyType.b2_kinematicBody: bodyValue["type"] = 1; break; case b2BodyType.b2_dynamicBody: bodyValue["type"] = 2; break; } VecToJson("position", body.Position, bodyValue); FloatToJson("angle", body.Angle, bodyValue); VecToJson("linearVelocity", body.LinearVelocity, bodyValue); FloatToJson("angularVelocity", body.AngularVelocity, bodyValue); if (body.LinearDamping != 0) FloatToJson("linearDamping", body.LinearDamping, bodyValue); if (body.AngularDamping != 0) FloatToJson("angularDamping", body.AngularDamping, bodyValue); if (body.GravityScale != 1) FloatToJson("gravityScale", body.GravityScale, bodyValue); if (body.IsBullet()) bodyValue["bullet"] = true; if (!body.IsSleepingAllowed()) bodyValue["allowSleep"] = false; if (body.IsAwake()) bodyValue["awake"] = true; if (!body.IsActive()) bodyValue["active"] = false; if (body.IsFixedRotation()) bodyValue["fixedRotation"] = true; b2MassData massData = new b2MassData(); massData = body.GetMassData(); if (massData.mass != 0) FloatToJson("massData-mass", massData.mass, bodyValue); if (massData.center.x != 0 || massData.center.y != 0) VecToJson("massData-center", body.LocalCenter, bodyValue); if (massData.I != 0) { FloatToJson("massData-I", massData.I, bodyValue); } //int i = 0; JArray arr = new JArray(); b2Body tmp = body; while (tmp != null) { bodyValue.Add("fixture", B2n(tmp)); tmp = body.Next; } bodyValue["fixture"] = arr; JArray customPropertyValue = WriteCustomPropertiesToJson(body); if (customPropertyValue.Count > 0) bodyValue["customProperties"] = customPropertyValue; return bodyValue; }