public override void Update(float dt)
{
basisA.rotationMatrix = connectionA.orientationMatrix;
basisB.rotationMatrix = connectionB.orientationMatrix;
basisA.ComputeWorldSpaceAxes();
basisB.ComputeWorldSpaceAxes();
Quaternion rotation;
Toolbox.GetQuaternionBetweenNormalizedVectors(ref basisB.primaryAxis, ref basisA.primaryAxis, out rotation);
//Transform b's 'Y' axis so that it is perpendicular with a's 'X' axis for measurement.
Vector3 twistMeasureAxis;
Vector3.Transform(ref basisB.xAxis, ref rotation, out twistMeasureAxis);
//By dotting the measurement vector with a 2d plane's axes, we can get a local X and Y value.
float y, x;
Vector3.Dot(ref twistMeasureAxis, ref basisA.yAxis, out y);
Vector3.Dot(ref twistMeasureAxis, ref basisA.xAxis, out x);
var angle = (float) Math.Atan2(y, x);
float distanceFromCurrent, distanceFromMaximum;
if (IsAngleValid(angle, out distanceFromCurrent, out distanceFromMaximum))
{
isActiveInSolver = false;
accumulatedImpulse = 0;
error = 0;
isLimitActive = false;
return;
}
isLimitActive = true;
//Compute the jacobian.
if (error > 0)
{
Vector3.Add(ref basisA.primaryAxis, ref basisB.primaryAxis, out jacobianB);
if (jacobianB.LengthSquared() < Toolbox.Epsilon)
{
//A nasty singularity can show up if the axes are aligned perfectly.
//In a 'real' situation, this is impossible, so just ignore it.
isActiveInSolver = false;
return;
}
jacobianB.Normalize();
jacobianA.X = -jacobianB.X;
jacobianA.Y = -jacobianB.Y;
jacobianA.Z = -jacobianB.Z;
}
else
{
//Reverse the jacobian so that the solver loop is easier.
Vector3.Add(ref basisA.primaryAxis, ref basisB.primaryAxis, out jacobianA);
if (jacobianA.LengthSquared() < Toolbox.Epsilon)
{
//A nasty singularity can show up if the axes are aligned perfectly.
//In a 'real' situation, this is impossible, so just ignore it.
isActiveInSolver = false;
return;
}
jacobianA.Normalize();
jacobianB.X = -jacobianA.X;
jacobianB.Y = -jacobianA.Y;
jacobianB.Z = -jacobianA.Z;
}
//****** VELOCITY BIAS ******//
//Compute the correction velocity.
error = ComputeAngleError(distanceFromCurrent, distanceFromMaximum);
float errorReduction;
springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out softness);
//biasVelocity = MathHelper.Clamp(-error * myCorrectionStrength / dt, -myMaxCorrectiveVelocity, myMaxCorrectiveVelocity);
biasVelocity = MathHelper.Min(MathHelper.Max(0, Math.Abs(error) - margin) * errorReduction, maxCorrectiveVelocity);
if (bounciness > 0)
{
float relativeVelocity;
float dot;
//Find the velocity contribution from each connection
Vector3.Dot(ref connectionA.angularVelocity, ref jacobianA, out relativeVelocity);
Vector3.Dot(ref connectionB.angularVelocity, ref jacobianB, out dot);
relativeVelocity += dot;
if (-relativeVelocity > bounceVelocityThreshold)
biasVelocity = MathHelper.Max(biasVelocity, -bounciness * relativeVelocity);
}
//The nice thing about this approach is that the jacobian entry doesn't flip.
//Instead, the error can be negative due to the use of Atan2.
//This is important for limits which have a unique high and low value.
//****** EFFECTIVE MASS MATRIX ******//
//Connection A's contribution to the mass matrix
float entryA;
Vector3 transformedAxis;
if (connectionA.isDynamic)
{
Matrix3x3.Transform(ref jacobianA, ref connectionA.inertiaTensorInverse, out transformedAxis);
Vector3.Dot(ref transformedAxis, ref jacobianA, out entryA);
}
else
entryA = 0;
//Connection B's contribution to the mass matrix
float entryB;
if (connectionB.isDynamic)
{
Matrix3x3.Transform(ref jacobianB, ref connectionB.inertiaTensorInverse, out transformedAxis);
Vector3.Dot(ref transformedAxis, ref jacobianB, out entryB);
}
else
entryB = 0;
//Compute the inverse mass matrix
velocityToImpulse = 1 / (softness + entryA + entryB);
}
