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;
// Solve normal constraints
for (int j = 0; j < c.pointCount; ++j)
{
PositionSolverManifold psm = new PositionSolverManifold(ref c, j);
Vector2 normal = psm._normal;
Vector2 point = psm._point;
float separation = psm._separation;
Vector2 rA = point - bodyA._sweep.c;
Vector2 rB = point - bodyB._sweep.c;
// Track max constraint error.
minSeparation = Math.Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float C = MathUtils.Clamp(baumgarte * (separation + Settings.b2_linearSlop), -Settings.b2_maxLinearCorrection, 0.0f);
// Compute the effective mass.
float rnA = MathUtils.Cross(rA, normal);
float rnB = MathUtils.Cross(rB, normal);
float K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB;
// Compute normal impulse
float impulse = K > 0.0f ? -C / K : 0.0f;
#if MATH_OVERLOADS
Vector2 P = impulse * normal;
bodyA._sweep.c -= invMassA * P;
bodyA._sweep.a -= invIA * MathUtils.Cross(rA, P);
bodyB._sweep.c += invMassB * P;
bodyB._sweep.a += invIB * MathUtils.Cross(rB, P);
#else
Vector2 P = new Vector2(impulse * normal.X, impulse * normal.Y);
bodyA._sweep.c.X -= invMassA * P.X;
bodyA._sweep.c.Y -= invMassA * P.Y;
bodyA._sweep.a -= invIA * (rA.X * P.Y - rA.Y * P.X);
bodyB._sweep.c.X += invMassB * P.X;
bodyB._sweep.c.Y += invMassB * P.Y;
bodyB._sweep.a += invIB * (rB.X * P.Y - rB.Y * P.X);
#endif
bodyA.SynchronizeTransform();
bodyB.SynchronizeTransform();
}
}
// We can't expect minSpeparation >= -Settings.b2_linearSlop because we don't
// push the separation above -Settings.b2_linearSlop.
return minSeparation >= -1.5f * Settings.b2_linearSlop;
}
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;
// Solve normal constraints
for (int j = 0; j < c.pointCount; ++j)
{
PositionSolverManifold psm = new PositionSolverManifold(ref c, j);
Vector2 normal = psm._normal;
Vector2 point = psm._point;
float separation = psm._separation;
Vector2 rA = point - bodyA._sweep.c;
Vector2 rB = point - bodyB._sweep.c;
// Track max constraint error.
minSeparation = Math.Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float C = MathUtils.Clamp(baumgarte * (separation + Settings.b2_linearSlop), -Settings.b2_maxLinearCorrection, 0.0f);
// Compute the effective mass.
float rnA = MathUtils.Cross(rA, normal);
float rnB = MathUtils.Cross(rB, normal);
float K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB;
// Compute normal impulse
float impulse = K > 0.0f ? -C / K : 0.0f;
#if MATH_OVERLOADS
Vector2 P = impulse * normal;
bodyA._sweep.c -= invMassA * P;
bodyA._sweep.a -= invIA * MathUtils.Cross(rA, P);
bodyB._sweep.c += invMassB * P;
bodyB._sweep.a += invIB * MathUtils.Cross(rB, P);
#else
Vector2 P = new Vector2(impulse * normal.X, impulse * normal.Y);
bodyA._sweep.c.X -= invMassA * P.X;
bodyA._sweep.c.Y -= invMassA * P.Y;
bodyA._sweep.a -= invIA * (rA.X * P.Y - rA.Y * P.X);
bodyB._sweep.c.X += invMassB * P.X;
bodyB._sweep.c.Y += invMassB * P.Y;
bodyB._sweep.a += invIB * (rB.X * P.Y - rB.Y * P.X);
#endif
bodyA.SynchronizeTransform();
bodyB.SynchronizeTransform();
}
}
// We can't expect minSpeparation >= -Settings.b2_linearSlop because we don't
// push the separation above -Settings.b2_linearSlop.
return(minSeparation >= -1.5f * Settings.b2_linearSlop);
}
