internal override void InitVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
// Compute the effective mass matrix.
Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
_u = b2._sweep.C + r2 - b1._sweep.C - r1;
// Handle singularity.
float length = _u.magnitude;
if (length > Settings.LinearSlop)
{
_u *= 1.0f / length;
}
else
{
_u = Vector2.zero;
}
float cr1u = r1.Cross(_u);
float cr2u = r2.Cross(_u);
float invMass = b1._invMass + b1._invI * cr1u * cr1u + b2._invMass + b2._invI * cr2u * cr2u;
Box2DXDebug.Assert(invMass > Settings.FLT_EPSILON);
_mass = 1.0f / invMass;
if (_frequencyHz > 0.0f)
{
float C = length - _length;
// Frequency
float omega = 2.0f * Settings.Pi * _frequencyHz;
// Damping coefficient
float d = 2.0f * _mass * _dampingRatio * omega;
// Spring stiffness
float k = _mass * omega * omega;
// magic formulas
_gamma = 1.0f / (step.Dt * (d + step.Dt * k));
_bias = C * step.Dt * k * _gamma;
_mass = 1.0f / (invMass + _gamma);
}
if (step.WarmStarting)
{
//Scale the inpulse to support a variable timestep.
_impulse *= step.DtRatio;
Vector2 P = _impulse * _u;
b1._linearVelocity -= b1._invMass * P;
b1._angularVelocity -= b1._invI * r1.Cross(P);
b2._linearVelocity += b2._invMass * P;
b2._angularVelocity += b2._invI * r2.Cross(P);
}
else
{
_impulse = 0.0f;
}
}