internal override bool SolvePositionConstraints(ref SolverData data)
{
if (Frequency > 0.0f)
{
// There is no position correction for soft distance constraints.
return(true);
}
FSBody b1 = BodyA;
FSBody b2 = BodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
FVector2 r1 = MathUtils.Mul(ref xf1.q, LocalAnchorA - b1.LocalCenter);
FVector2 r2 = MathUtils.Mul(ref xf2.q, LocalAnchorB - b2.LocalCenter);
FVector2 d = b2.Sweep.C + r2 - b1.Sweep.C - r1;
float length = d.Length();
if (length < MaxLength && length > MinLength)
{
return(true);
}
if (length == 0.0f)
{
return(true);
}
d /= length;
float C = length - MaxLength;
C = MathUtils.Clamp(C, -FSSettings.MaxLinearCorrection, FSSettings.MaxLinearCorrection);
float impulse = -_mass * C;
_u = d;
FVector2 P = impulse * _u;
b1.Sweep.C -= b1.InvMass * P;
b1.Sweep.A -= b1.InvI * MathUtils.Cross(r1, P);
b2.Sweep.C += b2.InvMass * P;
b2.Sweep.A += b2.InvI * MathUtils.Cross(r2, P);
b1.SynchronizeTransform();
b2.SynchronizeTransform();
return(Math.Abs(C) < FSSettings.LinearSlop);
}