internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Transform xf1;
b1.GetTransform(out xf1);
// Compute the effective mass matrix.
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = LocalAnchorB;
_u = r2 - b1.Sweep.c - r1;
// Handle singularity.
float length = _u.Length();
if (length > Settings.LinearSlop)
{
_u *= 1.0f / length;
}
else
{
_u = Vector2.Zero;
}
float cr1u = MathUtils.Cross(r1, _u);
float cr2u = MathUtils.Cross(r2, _u);
float invMass = b1.InvMass + b1.InvI * cr1u * cr1u + 0 * cr2u * cr2u;
Debug.Assert(invMass > Settings.Epsilon);
_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
if (Frequency > 0.0f)
{
float C = length - Length;
// Frequency
float omega = 2.0f * Settings.Pi * Frequency;
// Damping coefficient
float d = 2.0f * _mass * DampingRatio * omega;
// Spring stiffness
float k = _mass * omega * omega;
// magic formulas
_gamma = step.dt * (d + step.dt * k);
_gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
_bias = C * step.dt * k * _gamma;
_mass = invMass + _gamma;
_mass = _mass != 0.0f ? 1.0f / _mass : 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Scale the impulse to support a variable time step.
_impulse *= step.dtRatio;
Vector2 P = _impulse * _u;
b1.LinearVelocityInternal -= b1.InvMass * P;
b1.AngularVelocityInternal -= b1.InvI * MathUtils.Cross(r1, P);
}
else
{
_impulse = 0.0f;
}
}