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);
}
}
}
public void SolveTOI(b2TimeStep subStep)
{
int i;
int j;
m_contactSolver.Initialize(subStep, m_contacts, m_contactCount, m_allocator);
b2ContactSolver contactSolver = m_contactSolver;
// No warm starting is needed for TOI events because warm
// starting impulses were applied in the discrete solver.
// Warm starting for joints is off for now, but we need to
// call this function to compute Jacobians.
for (i = 0; i < m_jointCount; ++i)
{
m_joints[i].InitVelocityConstraints(subStep);
}
// Solve velocity constraints.
for (i = 0; i < subStep.velocityIterations; ++i)
{
contactSolver.SolveVelocityConstraints();
for (j = 0; j < m_jointCount; ++j)
{
m_joints[j].SolveVelocityConstraints(subStep);
}
}
// Don't store the TOI contact forces for warm starting
// because they can be quite large.
// Integrate positions.
for (i = 0; i < m_bodyCount; ++i)
{
b2Body b = m_bodies[i];
if (b.GetType() == b2Body.b2_staticBody)
{
continue;
}
// Check for large velocities.
// b2Vec2 translation = subStep.dt * b.m_linearVelocity;
float translationX = subStep.dt * b.m_linearVelocity.x;
float translationY = subStep.dt * b.m_linearVelocity.y;
//if (b2Dot(translation, translation) > b2_maxTranslationSquared)
if ((translationX * translationX + translationY * translationY) > b2Settings.b2_maxTranslationSquared)
{
b.m_linearVelocity.Normalize();
b.m_linearVelocity.x *= b2Settings.b2_maxTranslation * subStep.inv_dt;
b.m_linearVelocity.y *= b2Settings.b2_maxTranslation * subStep.inv_dt;
}
float rotation = subStep.dt * b.m_angularVelocity;
if (rotation * rotation > b2Settings.b2_maxRotationSquared)
{
if (b.m_angularVelocity < 0.0f)
{
b.m_angularVelocity = -b2Settings.b2_maxRotation * subStep.inv_dt;
}
else
{
b.m_angularVelocity = b2Settings.b2_maxRotation * subStep.inv_dt;
}
}
// Store positions for continuous collision.
b.m_sweep.c0.SetV(b.m_sweep.c);
b.m_sweep.a0 = b.m_sweep.a;
// Integrate
b.m_sweep.c.x += subStep.dt * b.m_linearVelocity.x;
b.m_sweep.c.y += subStep.dt * b.m_linearVelocity.y;
b.m_sweep.a += subStep.dt * b.m_angularVelocity;
// Compute new transform
b.SynchronizeTransform();
// Note: shapes are synchronized later.
}
// Solve position constraints.
float k_toiBaumgarte = 0.75f;
for (i = 0; i < subStep.positionIterations; ++i)
{
bool contactsOkay = contactSolver.SolvePositionConstraints(k_toiBaumgarte);
bool jointsOkay = true;
for (j = 0; j < m_jointCount; ++j)
{
bool jointOkay = m_joints[j].SolvePositionConstraints(b2Settings.b2_contactBaumgarte);
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
break;
}
}
Report(contactSolver.m_constraints);
}
