private bool SolveAngularPosition(LimitState state, float angle, float minAngle, float maxAngle, ref Vector3 axis, ref Vector3 accImpulse, float mass)
{
RigidBody a = this.BodyA, b = this.BodyB;
float error = 0f;
switch (state)
{
case LimitState.Inactive:
return(true);
case LimitState.Locked:
if (angle < minAngle)
{
error = minAngle - angle;
}
else if (angle > maxAngle)
{
error = maxAngle - angle;
}
else
{
error = 0f;
}
break;
case LimitState.Min:
error = minAngle - angle;
break;
case LimitState.Max:
error = maxAngle - angle;
break;
}
if (Math.Abs(error) < this.Manager.AngularErrorTolerance)
{
return(true);
}
Vector3 impulse, oldImpulse = accImpulse;
Vector3.Multiply(ref axis, error * mass * this.Manager.PositionCorrectionFactor, out impulse);
Vector3.Add(ref accImpulse, ref impulse, out accImpulse);
float d;
Vector3.Dot(ref axis, ref accImpulse, out d);
if (state == LimitState.Min && d < 0f || state == LimitState.Max && d > 0f)
{
accImpulse = Vector3.Zero;
}
Vector3.Subtract(ref accImpulse, ref oldImpulse, out impulse);
a.ApplyAngularFlashImpulse(ref impulse);
Vector3.Negate(ref impulse, out impulse);
b.ApplyAngularFlashImpulse(ref impulse);
return(false);
}