///<summary>
/// Performs the frame's configuration step.
///</summary>
///<param name="dt">Timestep duration.</param>
public override void Update(float dt)
{
//Compute the 'pre'-jacobians
Matrix3x3.Transform(ref localAnchorA, ref connectionA.orientationMatrix, out worldOffsetA);
Matrix3x3.Transform(ref localAnchorB, ref connectionB.orientationMatrix, out worldOffsetB);
Vector3.Add(ref worldOffsetA, ref connectionA.position, out worldAnchorA);
Vector3.Add(ref worldOffsetB, ref connectionB.position, out worldAnchorB);
Vector3.Subtract(ref worldAnchorB, ref connectionA.position, out rA);
Matrix3x3.Transform(ref localAxis, ref connectionA.orientationMatrix, out worldAxis);
//Compute error
#if !WINDOWS
Vector3 separation = new Vector3();
#else
Vector3 separation;
#endif
separation.X = worldAnchorB.X - worldAnchorA.X;
separation.Y = worldAnchorB.Y - worldAnchorA.Y;
separation.Z = worldAnchorB.Z - worldAnchorA.Z;
Vector3.Dot(ref separation, ref worldAxis, out unadjustedError);
//Compute error
if (unadjustedError < minimum)
{
unadjustedError = minimum - unadjustedError;
}
else if (unadjustedError > maximum)
{
unadjustedError = maximum - unadjustedError;
}
else
{
unadjustedError = 0;
isActiveInSolver = false;
accumulatedImpulse = 0;
isLimitActive = false;
return;
}
isLimitActive = true;
unadjustedError = -unadjustedError;
//Adjust Error
if (unadjustedError > 0)
{
error = MathHelper.Max(0, unadjustedError - margin);
}
else if (unadjustedError < 0)
{
error = MathHelper.Min(0, unadjustedError + margin);
}
//Compute jacobians
jLinearA = worldAxis;
jLinearB.X = -jLinearA.X;
jLinearB.Y = -jLinearA.Y;
jLinearB.Z = -jLinearA.Z;
Vector3.Cross(ref rA, ref jLinearA, out jAngularA);
Vector3.Cross(ref worldOffsetB, ref jLinearB, out jAngularB);
//Compute bias
float errorReductionParameter;
springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReductionParameter, out softness);
biasVelocity = MathHelper.Clamp(errorReductionParameter * error, -maxCorrectiveVelocity, maxCorrectiveVelocity);
if (bounciness > 0)
{
//Compute currently relative velocity for bounciness.
float relativeVelocity, dot;
Vector3.Dot(ref jLinearA, ref connectionA.linearVelocity, out relativeVelocity);
Vector3.Dot(ref jAngularA, ref connectionA.angularVelocity, out dot);
relativeVelocity += dot;
Vector3.Dot(ref jLinearB, ref connectionB.linearVelocity, out dot);
relativeVelocity += dot;
Vector3.Dot(ref jAngularB, ref connectionB.angularVelocity, out dot);
relativeVelocity += dot;
if (unadjustedError > 0 && -relativeVelocity > bounceVelocityThreshold)
{
biasVelocity = Math.Max(biasVelocity, ComputeBounceVelocity(-relativeVelocity));
}
else if (unadjustedError < 0 && relativeVelocity > bounceVelocityThreshold)
{
biasVelocity = Math.Min(biasVelocity, -ComputeBounceVelocity(relativeVelocity));
}
}
//compute mass matrix
float entryA, entryB;
Vector3 intermediate;
if (connectionA.isDynamic)
{
Matrix3x3.Transform(ref jAngularA, ref connectionA.inertiaTensorInverse, out intermediate);
Vector3.Dot(ref intermediate, ref jAngularA, out entryA);
entryA += connectionA.inverseMass;
}
else
{
entryA = 0;
}
if (connectionB.isDynamic)
{
Matrix3x3.Transform(ref jAngularB, ref connectionB.inertiaTensorInverse, out intermediate);
Vector3.Dot(ref intermediate, ref jAngularB, out entryB);
entryB += connectionB.inverseMass;
}
else
{
entryB = 0;
}
massMatrix = 1 / (entryA + entryB + softness);
}