private void SolveAngularVelocity(LimitState state, ref Vector3 vel, ref Vector3 axis, ref float accImpulse, float mass)
{
RigidBody a = this.BodyA, b = this.BodyB;
if (state == LimitState.Inactive)
{
return;
}
float impulse, oldImpulse = accImpulse;
Vector3.Dot(ref axis, ref vel, out impulse);
impulse *= -mass;
// clamp the impulse
accImpulse += impulse;
if (state == LimitState.Min && accImpulse < 0f || state == LimitState.Max && accImpulse > 0f)
{
accImpulse = 0f;
}
impulse = accImpulse - oldImpulse;
Vector3 i;
Vector3.Multiply(ref axis, impulse, out i);
a.ApplyAngularImpulse(ref i);
Vector3.Negate(ref i, out i);
b.ApplyAngularImpulse(ref i);
}