internal void SolveTOI(ref TimeStep subStep)
{
_contactSolver.Reset(_contacts, ContactCount, subStep.dtRatio, false);
// Solve position constraints.
const float kTOIBaumgarte = 0.75f;
for (int i = 0; i < Settings.TOIPositionIterations; ++i)
{
bool contactsOkay = _contactSolver.SolvePositionConstraintsTOI(kTOIBaumgarte);
if (contactsOkay)
{
break;
}
if (i == Settings.TOIPositionIterations - 1)
{
i += 0;
}
}
// Leap of faith to new safe state.
for (int i = 0; i < BodyCount; ++i)
{
Body body = Bodies[i];
body.Sweep.A0 = body.Sweep.A;
body.Sweep.C0 = body.Sweep.C;
}
// 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 < Settings.TOIVelocityIterations; ++i)
{
_contactSolver.SolveVelocityConstraints();
}
// Don't store the TOI contact forces for warm starting
// because they can be quite large.
// Integrate positions.
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
if (b.BodyType == BodyType.Static)
{
continue;
}
// Check for large velocities.
float translationx = subStep.dt * b.LinearVelocityInternal.X;
float translationy = subStep.dt * b.LinearVelocityInternal.Y;
float dot = translationx * translationx + translationy * translationy;
if (dot > Settings.MaxTranslationSquared)
{
float norm = 1f / (float)Math.Sqrt(dot);
float value = Settings.MaxTranslation * subStep.inv_dt;
b.LinearVelocityInternal.X = value * (translationx * norm);
b.LinearVelocityInternal.Y = value * (translationy * norm);
}
float rotation = subStep.dt * b.AngularVelocity;
if (rotation * rotation > Settings.MaxRotationSquared)
{
if (rotation < 0.0)
{
b.AngularVelocityInternal = -subStep.inv_dt * Settings.MaxRotation;
}
else
{
b.AngularVelocityInternal = subStep.inv_dt * Settings.MaxRotation;
}
}
// Integrate
b.Sweep.C.X += subStep.dt * b.LinearVelocityInternal.X;
b.Sweep.C.Y += subStep.dt * b.LinearVelocityInternal.Y;
b.Sweep.A += subStep.dt * b.AngularVelocityInternal;
// Compute new transform
b.SynchronizeTransform();
// Note: shapes are synchronized later.
}
Report(_contactSolver.Constraints);
}