/// <summary>
/// Describes whether this instance solve position constraints
/// </summary>
/// <param name="baumgarte">The baumgarte</param>
/// <returns>The bool</returns>
public bool SolvePositionConstraints(float baumgarte)
{
float minSeparation = 0.0f;
for (int i = 0; i < ConstraintCount; ++i)
{
ContactConstraint c = Constraints[i];
Body bodyA = c.BodyA;
Body bodyB = c.BodyB;
float invMassA = bodyA.Mass * bodyA.InvMass;
float invIa = bodyA.Mass * bodyA.InvI;
float invMassB = bodyB.Mass * bodyB.InvMass;
float invIb = bodyB.Mass * bodyB.InvI;
SPositionSolverManifold.Initialize(c);
Vec2 normal = SPositionSolverManifold.Normal;
// Solver normal constraints
for (int j = 0; j < c.PointCount; ++j)
{
Vec2 point = SPositionSolverManifold.Points[j];
float separation = SPositionSolverManifold.Separations[j];
Vec2 rA = point - bodyA.Sweep.C;
Vec2 rB = point - bodyB.Sweep.C;
// Track max constraint error.
minSeparation = Math.Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float clamp = baumgarte * Math.Clamp(separation + Settings.LinearSlop,
-Settings.MaxLinearCorrection,
0.0f);
// Compute normal impulse
float impulse = -c.Points[j].EqualizedMass * clamp;
Vec2 p = impulse * normal;
bodyA.Sweep.C -= invMassA * p;
bodyA.Sweep.A -= invIa * Vec2.Cross(rA, p);
bodyA.SynchronizeTransform();
bodyB.Sweep.C += invMassB * p;
bodyB.Sweep.A += invIb * Vec2.Cross(rB, p);
bodyB.SynchronizeTransform();
}
}
// We can't expect minSpeparation >= -Settings.LinearSlop because we don't
// push the separation above -Settings.LinearSlop.
return(minSeparation >= -1.5f * Settings.LinearSlop);
}
/// <summary>
/// Solves the step
/// </summary>
/// <param name="step">The step</param>
/// <param name="gravity">The gravity</param>
/// <param name="allowSleep">The allow sleep</param>
public void Solve(TimeStep step, Vec2 gravity, bool allowSleep)
{
// Integrate velocities and apply damping.
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
if (b.IsStatic())
{
continue;
}
// Integrate velocities.
b.LinearVelocity += step.Dt * (gravity + b.InvMass * b.Force);
b.AngularVelocity += step.Dt * b.InvI * b.Torque;
// Reset forces.
b.Force.Set(0.0f, 0.0f);
b.Torque = 0.0f;
// 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
b.LinearVelocity *= Math.Clamp(1.0f - step.Dt * b.LinearDamping, 0.0f, 1.0f);
b.AngularVelocity *= Math.Clamp(1.0f - step.Dt * b.AngularDamping, 0.0f, 1.0f);
}
ContactSolver contactSolver = new ContactSolver(step, Contacts, ContactCount);
// Initialize velocity constraints.
contactSolver.InitVelocityConstraints(step);
for (int i = 0; i < JointCount; ++i)
{
Joints[i].InitVelocityConstraints(step);
}
// Solve velocity constraints.
for (int i = 0; i < step.VelocityIterations; ++i)
{
for (int j = 0; j < JointCount; ++j)
{
Joints[j].SolveVelocityConstraints(step);
}
contactSolver.SolveVelocityConstraints();
}
// Post-solve (store impulses for warm starting).
contactSolver.FinalizeVelocityConstraints();
// Integrate positions.
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
if (b.IsStatic())
{
continue;
}
// Check for large velocities.
Vec2 translation = step.Dt * b.LinearVelocity;
if (Vec2.Dot(translation, translation) > Settings.MaxTranslationSquared)
{
translation.Normalize();
b.LinearVelocity = Settings.MaxTranslation * step.InvDt * translation;
}
float rotation = step.Dt * b.AngularVelocity;
if (rotation * rotation > Settings.MaxRotationSquared)
{
if (rotation < 0.0)
{
b.AngularVelocity = -step.InvDt * Settings.MaxRotation;
}
else
{
b.AngularVelocity = step.InvDt * Settings.MaxRotation;
}
}
// Store positions for continuous collision.
b.Sweep.C0 = b.Sweep.C;
b.Sweep.A0 = b.Sweep.A;
// Integrate
b.Sweep.C += step.Dt * b.LinearVelocity;
b.Sweep.A += step.Dt * b.AngularVelocity;
// Compute new transform
b.SynchronizeTransform();
// Note: shapes are synchronized later.
}
// Iterate over constraints.
for (int i = 0; i < step.PositionIterations; ++i)
{
bool contactsOkay = contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte);
bool jointsOkay = true;
for (int j = 0; j < JointCount; ++j)
{
bool jointOkay = Joints[j].SolvePositionConstraints(Settings.ContactBaumgarte);
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
break;
}
}
Report(contactSolver.Constraints);
if (allowSleep)
{
float minSleepTime = Settings.FltMax;
#if !TARGET_FLOAT32_IS_FIXED
float linTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance;
float angTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance;
#endif
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
if (b.InvMass == 0.0f)
{
continue;
}
if ((b.Flags & BodyFlags.AllowSleep) == 0)
{
b.SleepTime = 0.0f;
minSleepTime = 0.0f;
}
if ((b.Flags & BodyFlags.AllowSleep) == 0 ||
#if TARGET_FLOAT32_IS_FIXED
Common.Math.Abs(b._angularVelocity) > Settings.AngularSleepTolerance ||
Common.Math.Abs(b._linearVelocity.X) > Settings.LinearSleepTolerance ||
Common.Math.Abs(b._linearVelocity.Y) > Settings.LinearSleepTolerance)
#else
b.AngularVelocity *b.AngularVelocity > angTolSqr ||
Vec2.Dot(b.LinearVelocity, b.LinearVelocity) > linTolSqr)
#endif
{
b.SleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b.SleepTime += step.Dt;
minSleepTime = Math.Min(minSleepTime, b.SleepTime);
}
}