public void Solve(ref FSTimeStep step, ref FVector2 gravity)
{
float h = step.dt;
// Integrate velocities and apply damping. Initialize the body state.
for (int i = 0; i < BodyCount; ++i)
{
FSBody b = Bodies[i];
FVector2 c = b.Sweep.C;
float a = b.Sweep.A;
FVector2 v = b.LinearVelocity;
float w = b.AngularVelocity;
// Store positions for continuous collision.
b.Sweep.C0 = b.Sweep.C;
b.Sweep.A0 = b.Sweep.A;
if (b.BodyType == BodyType.Dynamic)
{
// Integrate velocities.
v += h * (b.GravityScale * gravity + b.InvMass * b.Force);
w += h * b.InvI * b.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 *= MathUtils.Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f);
w *= MathUtils.Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f);
}
_positions[i].c = c;
_positions[i].a = a;
_velocities[i].v = v;
_velocities[i].w = w;
}
// Solver data
SolverData solverData = new SolverData();
solverData.step = step;
solverData.positions = _positions;
solverData.velocities = _velocities;
// Initialize velocity constraints.
//b2ContactSolverDef contactSolverDef;
//contactSolverDef.step = step;
//contactSolverDef.contacts = m_contacts;
//contactSolverDef.count = m_contactCount;
//contactSolverDef.positions = m_positions;
//contactSolverDef.velocities = m_velocities;
//contactSolverDef.allocator = m_allocator;
_contactSolver.Reset(step, ContactCount, _contacts, _positions, _velocities);
_contactSolver.InitializeVelocityConstraints();
if (FSSettings.EnableWarmstarting)
{
_contactSolver.WarmStart();
}
#if (!SILVERLIGHT)
if (FSSettings.EnableDiagnostics)
{
_watch.Start();
_tmpTime = 0;
}
#endif
for (int i = 0; i < JointCount; ++i)
{
if (_joints[i].Enabled)
{
_joints[i].InitVelocityConstraints(ref solverData);
}
}
#if (!SILVERLIGHT)
if (FSSettings.EnableDiagnostics)
{
_tmpTime += _watch.ElapsedTicks;
}
#endif
// Solve velocity constraints.
for (int i = 0; i < FSSettings.VelocityIterations; ++i)
{
#if (!SILVERLIGHT)
if (FSSettings.EnableDiagnostics)
{
_watch.Start();
}
#endif
for (int j = 0; j < JointCount; ++j)
{
FarseerJoint joint = _joints[j];
if (!joint.Enabled)
{
continue;
}
joint.SolveVelocityConstraints(ref solverData);
//TODO: Move up before solve?
joint.Validate(step.inv_dt);
}
#if (!SILVERLIGHT)
if (FSSettings.EnableDiagnostics)
{
_watch.Stop();
_tmpTime += _watch.ElapsedTicks;
_watch.Reset();
}
#endif
_contactSolver.SolveVelocityConstraints();
}
// Store impulses for warm starting.
_contactSolver.StoreImpulses();
// Integrate positions
for (int i = 0; i < BodyCount; ++i)
{
FVector2 c = _positions[i].c;
float a = _positions[i].a;
FVector2 v = _velocities[i].v;
float w = _velocities[i].w;
// Check for large velocities
FVector2 translation = h * v;
if (FVector2.Dot(translation, translation) > FSSettings.MaxTranslationSquared)
{
float ratio = FSSettings.MaxTranslation / translation.Length();
v *= ratio;
}
float rotation = h * w;
if (rotation * rotation > FSSettings.MaxRotationSquared)
{
float ratio = FSSettings.MaxRotation / Math.Abs(rotation);
w *= ratio;
}
// Integrate
c += h * v;
a += h * w;
_positions[i].c = c;
_positions[i].a = a;
_velocities[i].v = v;
_velocities[i].w = w;
}
// Solve position constraints
bool positionSolved = false;
for (int i = 0; i < FSSettings.PositionIterations; ++i)
{
bool contactsOkay = _contactSolver.SolvePositionConstraints();
bool jointsOkay = true;
#if (!SILVERLIGHT)
if (FSSettings.EnableDiagnostics)
{
_watch.Start();
}
#endif
for (int j = 0; j < JointCount; ++j)
{
FarseerJoint joint = _joints[j];
if (!joint.Enabled)
{
continue;
}
bool jointOkay = joint.SolvePositionConstraints(ref solverData);
jointsOkay = jointsOkay && jointOkay;
}
#if (!SILVERLIGHT)
if (FSSettings.EnableDiagnostics)
{
_watch.Stop();
_tmpTime += _watch.ElapsedTicks;
_watch.Reset();
}
#endif
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
positionSolved = true;
break;
}
}
#if (!SILVERLIGHT)
if (FSSettings.EnableDiagnostics)
{
JointUpdateTime = _tmpTime;
}
#endif
// Copy state buffers back to the bodies
for (int i = 0; i < BodyCount; ++i)
{
FSBody body = Bodies[i];
body.Sweep.C = _positions[i].c;
body.Sweep.A = _positions[i].a;
body.LinearVelocity = _velocities[i].v;
body.AngularVelocity = _velocities[i].w;
body.SynchronizeTransform();
}
Report(_contactSolver._velocityConstraints);
if (FSSettings.AllowSleep)
{
float minSleepTime = FSSettings.MaxFloat;
for (int i = 0; i < BodyCount; ++i)
{
FSBody b = Bodies[i];
if (b.BodyType == BodyType.Static)
{
continue;
}
if ((b.Flags & BodyFlags.AutoSleep) == 0 ||
b.AngularVelocityInternal * b.AngularVelocityInternal > AngTolSqr ||
FVector2.Dot(b.LinearVelocityInternal, b.LinearVelocityInternal) > LinTolSqr)
{
b.SleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b.SleepTime += h;
minSleepTime = Math.Min(minSleepTime, b.SleepTime);
}
}
if (minSleepTime >= FSSettings.TimeToSleep && positionSolved)
{
for (int i = 0; i < BodyCount; ++i)
{
FSBody b = Bodies[i];
b.Awake = false;
}
}
}
}
internal void SolveTOI(ref FSTimeStep subStep, int toiIndexA, int toiIndexB)
{
Debug.Assert(toiIndexA < BodyCount);
Debug.Assert(toiIndexB < BodyCount);
// Initialize the body state.
for (int i = 0; i < BodyCount; ++i)
{
FSBody b = Bodies[i];
_positions[i].c = b.Sweep.C;
_positions[i].a = b.Sweep.A;
_velocities[i].v = b.LinearVelocity;
_velocities[i].w = b.AngularVelocity;
}
//b2ContactSolverDef contactSolverDef;
//contactSolverDef.contacts = _contacts;
//contactSolverDef.count = _contactCount;
//contactSolverDef.allocator = _allocator;
//contactSolverDef.step = subStep;
//contactSolverDef.positions = _positions;
//contactSolverDef.velocities = _velocities;
//b2ContactSolver contactSolver(&contactSolverDef);
_contactSolver.Reset(subStep, ContactCount, _contacts, _positions, _velocities);
// Solve position constraints.
for (int i = 0; i < FSSettings.TOIPositionIterations; ++i)
{
bool contactsOkay = _contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB);
if (contactsOkay)
{
break;
}
}
// Leap of faith to new safe state.
Bodies[toiIndexA].Sweep.C0 = _positions[toiIndexA].c;
Bodies[toiIndexA].Sweep.A0 = _positions[toiIndexA].a;
Bodies[toiIndexB].Sweep.C0 = _positions[toiIndexB].c;
Bodies[toiIndexB].Sweep.A0 = _positions[toiIndexB].a;
// No warm starting is needed for TOI events because warm
// starting impulses were applied in the discrete solver.
_contactSolver.InitializeVelocityConstraints();
// Solve velocity constraints.
for (int i = 0; i < FSSettings.TOIVelocityIterations; ++i)
{
_contactSolver.SolveVelocityConstraints();
}
// Don't store the TOI contact forces for warm starting
// because they can be quite large.
float h = subStep.dt;
// Integrate positions.
for (int i = 0; i < BodyCount; ++i)
{
FVector2 c = _positions[i].c;
float a = _positions[i].a;
FVector2 v = _velocities[i].v;
float w = _velocities[i].w;
// Check for large velocities
FVector2 translation = h * v;
if (FVector2.Dot(translation, translation) > FSSettings.MaxTranslationSquared)
{
float ratio = FSSettings.MaxTranslation / translation.Length();
v *= ratio;
}
float rotation = h * w;
if (rotation * rotation > FSSettings.MaxRotationSquared)
{
float ratio = FSSettings.MaxRotation / Math.Abs(rotation);
w *= ratio;
}
// Integrate
c += h * v;
a += h * w;
_positions[i].c = c;
_positions[i].a = a;
_velocities[i].v = v;
_velocities[i].w = w;
// Sync bodies
FSBody body = Bodies[i];
body.Sweep.C = c;
body.Sweep.A = a;
body.LinearVelocity = v;
body.AngularVelocity = w;
body.SynchronizeTransform();
}
Report(_contactSolver._velocityConstraints);
}