public void Solve(Profile profile, TimeStep step, Vec2 gravity, bool allowSleep) {
Timer timer = new Timer();
float h = step.dt;
// Integrate velocities and apply damping. Initialize the body state.
for (int i = 0; i < m_bodies.Count(); i++)
{
Body b = m_bodies[i];
Vec2 c = b.m_sweep.c;
float a = b.m_sweep.a;
Vec2 v = b.m_linearVelocity;
float w = b.m_angularVelocity;
// Store positions for continuous collision.
b.m_sweep.c0 = b.m_sweep.c;
b.m_sweep.a0 = b.m_sweep.a;
if (b.m_type == BodyType._dynamicBody)
{
// Integrate velocities.
v += h * (b.m_gravityScale * gravity + b.m_invMass * b.m_force);
w += h * b.m_invI * b.m_torque;
// Apply damping.
// ODE: dv/dt + c * v = 0
// Solution: v(t) = v0 * exp(-c * t)
// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
// v2 = exp(-c * dt) * v1
// Taylor expansion:
// v2 = (1.0f - c * dt) * v1
v *= Utilities.Clamp(1.0f - h * b.m_linearDamping, 0.0f, 1.0f);
w *= Utilities.Clamp(1.0f - h * b.m_angularDamping, 0.0f, 1.0f);
}
Position pos = new Position();
pos.c = c;
pos.a = a;
m_positions.Add(pos);
Velocity vel = new Velocity();
vel.v = v;
vel.w = w;
m_velocities.Add(vel);
}
timer.Reset();
// Solver data
SolverData solverData;
solverData.step = step;
solverData.positions = m_positions;
solverData.velocities = m_velocities;
// Initialize velocity constraints.
ContactSolverDef contactSolverDef;
contactSolverDef.step = step;
contactSolverDef.contacts = m_contacts;
contactSolverDef.positions = m_positions;
contactSolverDef.velocities = m_velocities;
ContactSolver contactSolver = new ContactSolver(contactSolverDef);
contactSolver.InitializeVelocityConstraints();
if (step.warmStarting)
{
contactSolver.WarmStart();
}
for (int i = 0; i < m_joints.Count(); ++i)
{
m_joints[i].InitVelocityConstraints(solverData);
}
profile.solveInit = timer.GetMilliseconds();
// Solve velocity constraints
timer.Reset();
for (int i = 0; i < step.velocityIterations; ++i)
{
for (int j = 0; j < m_joints.Count(); ++j)
{
m_joints[j].SolveVelocityConstraints(solverData);
}
contactSolver.SolveVelocityConstraints();
}
// Store impulses for warm starting
contactSolver.StoreImpulses();
profile.solveVelocity = timer.GetMilliseconds();
// Integrate positions
for (int i = 0; i < m_bodies.Count(); ++i)
{
Vec2 c = m_positions[i].c;
float a = m_positions[i].a;
Vec2 v = m_velocities[i].v;
float w = m_velocities[i].w;
// Check for large velocities
Vec2 translation = h * v;
if (Utilities.Dot(translation, translation) > Settings._maxTranslationSquared)
{
float ratio = Settings._maxTranslation / translation.Length();
v *= ratio;
}
float rotation = h * w;
if (rotation * rotation > Settings._maxRotationSquared)
{
float ratio = Settings._maxRotation / Math.Abs(rotation);
w *= ratio;
}
// Integrate
c += h * v;
a += h * w;
m_positions[i].c = c;
m_positions[i].a = a;
m_velocities[i].v = v;
m_velocities[i].w = w;
}
// Solve position constraints
timer.Reset();
bool positionSolved = false;
for (int i = 0; i < step.positionIterations; ++i)
{
bool contactsOkay = contactSolver.SolvePositionConstraints();
bool jointsOkay = true;
for (int j = 0; j < m_joints.Count; ++j)
{
bool jointOkay = m_joints[j].SolvePositionConstraints(solverData);
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
positionSolved = true;
break;
}
}
// Copy state buffers back to the bodies
for (int i = 0; i < m_bodies.Count(); ++i)
{
Body body = m_bodies[i];
body.m_sweep.c = m_positions[i].c;
body.m_sweep.a = m_positions[i].a;
body.m_linearVelocity = m_velocities[i].v;
body.m_angularVelocity = m_velocities[i].w;
body.SynchronizeTransform();
}
profile.solvePosition = timer.GetMilliseconds();
Report(contactSolver.m_velocityConstraints);
if (allowSleep)
{
float minSleepTime = Single.MaxValue;
const float linTolSqr = Settings._linearSleepTolerance * Settings._linearSleepTolerance;
const float angTolSqr = Settings._angularSleepTolerance * Settings._angularSleepTolerance;
for (int i = 0; i < m_bodies.Count(); ++i)
{
Body b = m_bodies[i];
if (b.GetBodyType() == BodyType._staticBody)
{
continue;
}
if ((b.m_flags & Body.BodyFlags.e_autoSleepFlag) == 0 ||
b.m_angularVelocity * b.m_angularVelocity > angTolSqr ||
Utilities.Dot(b.m_linearVelocity, b.m_linearVelocity) > linTolSqr)
{
b.m_sleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b.m_sleepTime += h;
minSleepTime = Math.Min(minSleepTime, b.m_sleepTime);
}
}
if (minSleepTime >= Settings._timeToSleep && positionSolved)
{
for (int i = 0; i < m_bodies.Count(); ++i)
{
Body b = m_bodies[i];
b.SetAwake(false);
}
}
}
}
