public final boolean solvePositionConstraints(float baumgarte){
float minSeparation = 0.0f;
for (int i = 0; i < m_constraintCount; ++i){
final ContactConstraint c = m_constraints[i];
final Body bodyA = c.bodyA;
final Body bodyB = c.bodyB;
final float invMassA = bodyA.m_mass * bodyA.m_invMass;
final float invIA = bodyA.m_mass * bodyA.m_invI;
final float invMassB = bodyB.m_mass * bodyB.m_invMass;
final float invIB = bodyB.m_mass * bodyB.m_invI;
// Solve normal constraints
for (int j = 0; j < c.pointCount; ++j){
final PositionSolverManifold psm = psolver;
psm.initialize(c, j);
final Vec2 normal = psm.normal;
final Vec2 point = psm.point;
final float separation = psm.separation;
rA.set(point).subLocal(bodyA.m_sweep.c);
rB.set(point).subLocal(bodyB.m_sweep.c);
// Track max constraint error.
minSeparation = MathUtils.min(minSeparation, separation);
// Prevent large corrections and allow slop.
final float C = MathUtils.clamp(baumgarte * (separation + Settings.linearSlop), -Settings.maxLinearCorrection, 0.0f);
// Compute the effective mass.
final float rnA = Vec2.cross(rA, normal);
final float rnB = Vec2.cross(rB, normal);
final float K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB;
// Compute normal impulse
final float impulse = K > 0.0f ? - C / K : 0.0f;
P.set(normal).mulLocal(impulse);
temp1.set(P).mulLocal(invMassA);
bodyA.m_sweep.c.subLocal(temp1);
bodyA.m_sweep.a -= invIA * Vec2.cross(rA, P);
bodyA.synchronizeTransform();
temp1.set(P).mulLocal(invMassB);
bodyB.m_sweep.c.addLocal(temp1);
bodyB.m_sweep.a += invIB * Vec2.cross(rB, P);
bodyB.synchronizeTransform();
}
}
// We can't expect minSpeparation >= -linearSlop because we don't
// push the separation above -linearSlop.