public override void Update(float dt)
{
Matrix3x3.Transform(ref localAxisA, ref connectionA.orientationMatrix, out worldAxisA);
Matrix3x3.Transform(ref localAxisB, ref connectionB.orientationMatrix, out worldAxisB);
float dot;
Vector3.Dot(ref worldAxisA, ref worldAxisB, out dot);
//Keep in mind, the dot is the cosine of the angle.
//1: 0 radians
//0: pi/2 radians
//-1: pi radians
if (dot > minimumCosine)
{
isActiveInSolver = false;
error = 0;
accumulatedImpulse = 0;
isLimitActive = false;
return;
}
isLimitActive = true;
//Hinge axis is actually the jacobian entry for angular A (negative angular B).
Vector3.Cross(ref worldAxisA, ref worldAxisB, out hingeAxis);
float lengthSquared = hingeAxis.LengthSquared();
if (lengthSquared > Toolbox.Epsilon)
{
//Vector3.Divide(ref hingeAxis, (float)Math.Sqrt(lengthSquared), out hingeAxis);
}
else
{
//They're parallel; for the sake of continuity, pick some axis which is perpendicular to both that ISN'T the zero vector.
Vector3.Cross(ref worldAxisA, ref Toolbox.UpVector, out hingeAxis);
lengthSquared = hingeAxis.LengthSquared();
if (lengthSquared > Toolbox.Epsilon)
{
//Vector3.Divide(ref hingeAxis, (float)Math.Sqrt(lengthSquared), out hingeAxis);
}
else
{
//That's improbable; b's world axis was apparently parallel with the up vector!
//So just use the right vector (it can't be parallel with both the up and right vectors).
Vector3.Cross(ref worldAxisA, ref Toolbox.RightVector, out hingeAxis);
//hingeAxis.Normalize();
}
}
float errorReduction;
springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out softness);
//Further away from 0 degrees is further negative; if the dot is below the minimum cosine, it means the angle is above the maximum angle.
error = Math.Max(0, minimumCosine - dot - margin);
biasVelocity = MathHelper.Clamp(errorReduction * error, -maxCorrectiveVelocity, maxCorrectiveVelocity);
if (bounciness > 0)
{
//Compute the speed around the axis.
float relativeSpeed;
Vector3 relativeVelocity;
Vector3.Subtract(ref connectionA.angularVelocity, ref connectionB.angularVelocity, out relativeVelocity);
Vector3.Dot(ref relativeVelocity, ref hingeAxis, out relativeSpeed);
if (relativeSpeed < -bounceVelocityThreshold)
{
biasVelocity = Math.Max(biasVelocity, bounciness * relativeSpeed);
}
}
//Connection A's contribution to the mass matrix
float entryA;
Vector3 transformedAxis;
if (connectionA.isDynamic)
{
Matrix3x3.Transform(ref hingeAxis, ref connectionA.inertiaTensorInverse, out transformedAxis);
Vector3.Dot(ref transformedAxis, ref hingeAxis, out entryA);
}
else
entryA = 0;
//Connection B's contribution to the mass matrix
float entryB;
if (connectionB.isDynamic)
{
Matrix3x3.Transform(ref hingeAxis, ref connectionB.inertiaTensorInverse, out transformedAxis);
Vector3.Dot(ref transformedAxis, ref hingeAxis, out entryB);
}
else
entryB = 0;
//Compute the inverse mass matrix
velocityToImpulse = 1 / (softness + entryA + entryB);
}