// Sequential position solver for position constraints.
public bool SolvePositionConstraintsTOI(float baumgarte, Body toiBodyA, Body toiBodyB)
{
float minSeparation = 0.0f;
for (int i = 0; i < _constraintCount; ++i)
{
ContactConstraint c = Constraints[i];
Body bodyA = c.BodyA;
Body bodyB = c.BodyB;
float massA = 0.0f;
if (bodyA == toiBodyA || bodyA == toiBodyB)
{
massA = bodyA.Mass;
}
float massB = 0.0f;
if (bodyB == toiBodyA || bodyB == toiBodyB)
{
massB = bodyB.Mass;
}
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)
{
Vector2 normal;
Vector2 point;
float separation;
PositionSolverManifold.Solve(ref c, j, out normal, out point, out separation);
Vector2 rA; //= point - bodyA.Sweep.C);
Vector2.Subtract(ref point, ref bodyA.Sweep.C, out rA);
Vector2 rB; // = point - bodyB.Sweep.C;
Vector2.Subtract(ref point, ref bodyB.Sweep.C, out rB);
// Track max constraint error.
minSeparation = Math.Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float C = MathUtils.Clamp(baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f);
// Compute the effective mass.
float rnA = rA.X * normal.Y - rA.Y * normal.X;
float rnB = rB.X * normal.Y - rB.Y * normal.X;
float K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB;
// Compute normal impulse
float impulse = K > 0.0f ? -C / K : 0.0f;
Vector2 P = impulse * normal;
bodyA.Sweep.C -= invMassA * P;
bodyA.Sweep.A -= invIA * (rA.X * P.Y - rA.Y * P.X);
bodyA.SynchronizeTransform();
bodyB.Sweep.C += invMassB * P;
bodyB.Sweep.A += invIB * (rB.X * P.Y - rB.Y * P.X);
bodyB.SynchronizeTransform();
}
}
// We can't expect minSpeparation >= -b2_linearSlop because we don't
// push the separation above -b2_linearSlop.
return(minSeparation >= -1.5f * Settings.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)
{
Vector2 normal;
Vector2 point;
float separation;
PositionSolverManifold.Solve(ref c, j, out normal, out point, out 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.LinearSlop),
-Settings.MaxLinearCorrection, 0.0f);
// Compute the effective mass.
float rnA = rA.X * normal.Y - rA.Y * normal.X;
float rnB = rB.X * normal.Y - rB.Y * normal.X;
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.LinearSlop);
}
