public override void Update(float dt)
{
//Transform the axes into world space.
basis.rotationMatrix = connectionA.orientationMatrix;
basis.ComputeWorldSpaceAxes();
Matrix3x3.Transform(ref localTestAxis, ref connectionB.orientationMatrix, out worldTestAxis);
//Compute the plane normals.
Vector3 minPlaneNormal, maxPlaneNormal;
//Rotate basisA y axis around the basisA primary axis.
Matrix rotation;
Matrix.CreateFromAxisAngle(ref basis.primaryAxis, minimumAngle + MathHelper.PiOver2, out rotation);
Vector3.TransformNormal(ref basis.xAxis, ref rotation, out minPlaneNormal);
Matrix.CreateFromAxisAngle(ref basis.primaryAxis, maximumAngle - MathHelper.PiOver2, out rotation);
Vector3.TransformNormal(ref basis.xAxis, ref rotation, out maxPlaneNormal);
//Compute the errors along the two normals.
float planePositionMin, planePositionMax;
Vector3.Dot(ref minPlaneNormal, ref worldTestAxis, out planePositionMin);
Vector3.Dot(ref maxPlaneNormal, ref worldTestAxis, out planePositionMax);
float span = GetDistanceFromMinimum(maximumAngle);
//Early out and compute the determine the plane normal.
if (span >= MathHelper.Pi)
{
if (planePositionMax > 0 || planePositionMin > 0)
{
//It's in a perfectly valid configuration, so skip.
isActiveInSolver = false;
minIsActive = false;
maxIsActive = false;
error = Vector2.Zero;
accumulatedImpulse = Vector2.Zero;
isLimitActive = false;
return;
}
if (planePositionMax > planePositionMin)
{
//It's quicker to escape out to the max plane than the min plane.
error.X = 0;
error.Y = -planePositionMax;
accumulatedImpulse.X = 0;
minIsActive = false;
maxIsActive = true;
}
else
{
//It's quicker to escape out to the min plane than the max plane.
error.X = -planePositionMin;
error.Y = 0;
accumulatedImpulse.Y = 0;
minIsActive = true;
maxIsActive = false;
}
//There's never a non-degenerate situation where having both planes active with a span
//greater than pi is useful.
}
else
{
if (planePositionMax > 0 && planePositionMin > 0)
{
//It's in a perfectly valid configuration, so skip.
isActiveInSolver = false;
minIsActive = false;
maxIsActive = false;
error = Vector2.Zero;
accumulatedImpulse = Vector2.Zero;
isLimitActive = false;
return;
}
if (planePositionMin <= 0 && planePositionMax <= 0)
{
//Escape upward.
//Activate both planes.
error.X = -planePositionMin;
error.Y = -planePositionMax;
minIsActive = true;
maxIsActive = true;
}
else if (planePositionMin <= 0)
{
//It's quicker to escape out to the min plane than the max plane.
error.X = -planePositionMin;
error.Y = 0;
accumulatedImpulse.Y = 0;
minIsActive = true;
maxIsActive = false;
}
else
{
//It's quicker to escape out to the max plane than the min plane.
error.X = 0;
error.Y = -planePositionMax;
accumulatedImpulse.X = 0;
minIsActive = false;
maxIsActive = true;
}
}
isLimitActive = true;
//****** VELOCITY BIAS ******//
//Compute the correction velocity
float errorReduction;
springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out softness);
//Compute the jacobians
if (minIsActive)
{
Vector3.Cross(ref minPlaneNormal, ref worldTestAxis, out jacobianMinA);
if (jacobianMinA.LengthSquared() < Toolbox.Epsilon)
{
//The plane normal is aligned with the test axis.
//Use the basis's free axis.
jacobianMinA = basis.primaryAxis;
}
jacobianMinA.Normalize();
jacobianMinB.X = -jacobianMinA.X;
jacobianMinB.Y = -jacobianMinA.Y;
jacobianMinB.Z = -jacobianMinA.Z;
}
if (maxIsActive)
{
Vector3.Cross(ref maxPlaneNormal, ref worldTestAxis, out jacobianMaxA);
if (jacobianMaxA.LengthSquared() < Toolbox.Epsilon)
{
//The plane normal is aligned with the test axis.
//Use the basis's free axis.
jacobianMaxA = basis.primaryAxis;
}
jacobianMaxA.Normalize();
jacobianMaxB.X = -jacobianMaxA.X;
jacobianMaxB.Y = -jacobianMaxA.Y;
jacobianMaxB.Z = -jacobianMaxA.Z;
}
//Error is always positive
if (minIsActive)
{
biasVelocity.X = MathHelper.Min(MathHelper.Max(0, error.X - margin) * errorReduction, maxCorrectiveVelocity);
if (bounciness > 0)
{
float relativeVelocity;
float dot;
//Find the velocity contribution from each connection
Vector3.Dot(ref connectionA.angularVelocity, ref jacobianMinA, out relativeVelocity);
Vector3.Dot(ref connectionB.angularVelocity, ref jacobianMinB, out dot);
relativeVelocity += dot;
if (-relativeVelocity > bounceVelocityThreshold)
biasVelocity.X = MathHelper.Max(biasVelocity.X, -bounciness * relativeVelocity);
}
}
if (maxIsActive)
{
biasVelocity.Y = MathHelper.Min(MathHelper.Max(0, error.Y - margin) * errorReduction, maxCorrectiveVelocity);
if (bounciness > 0)
{
//Find the velocity contribution from each connection
if (maxIsActive)
{
float relativeVelocity;
Vector3.Dot(ref connectionA.angularVelocity, ref jacobianMaxA, out relativeVelocity);
float dot;
Vector3.Dot(ref connectionB.angularVelocity, ref jacobianMaxB, out dot);
relativeVelocity += dot;
if (-relativeVelocity > bounceVelocityThreshold)
biasVelocity.Y = MathHelper.Max(biasVelocity.Y, -bounciness * relativeVelocity);
}
}
}
//****** EFFECTIVE MASS MATRIX ******//
//Connection A's contribution to the mass matrix
float minEntryA, minEntryB;
float maxEntryA, maxEntryB;
Vector3 transformedAxis;
if (connectionA.isDynamic)
{
if (minIsActive)
{
Matrix3x3.Transform(ref jacobianMinA, ref connectionA.inertiaTensorInverse, out transformedAxis);
Vector3.Dot(ref transformedAxis, ref jacobianMinA, out minEntryA);
}
else
minEntryA = 0;
if (maxIsActive)
{
Matrix3x3.Transform(ref jacobianMaxA, ref connectionA.inertiaTensorInverse, out transformedAxis);
Vector3.Dot(ref transformedAxis, ref jacobianMaxA, out maxEntryA);
}
else
maxEntryA = 0;
}
else
{
minEntryA = 0;
maxEntryA = 0;
}
//Connection B's contribution to the mass matrix
if (connectionB.isDynamic)
{
if (minIsActive)
{
Matrix3x3.Transform(ref jacobianMinB, ref connectionB.inertiaTensorInverse, out transformedAxis);
Vector3.Dot(ref transformedAxis, ref jacobianMinB, out minEntryB);
}
else
minEntryB = 0;
if (maxIsActive)
{
Matrix3x3.Transform(ref jacobianMaxB, ref connectionB.inertiaTensorInverse, out transformedAxis);
Vector3.Dot(ref transformedAxis, ref jacobianMaxB, out maxEntryB);
}
else
maxEntryB = 0;
}
else
{
minEntryB = 0;
maxEntryB = 0;
}
//Compute the inverse mass matrix
//Notice that the mass matrix isn't linked, it's two separate ones.
velocityToImpulse.X = 1 / (softness + minEntryA + minEntryB);
velocityToImpulse.Y = 1 / (softness + maxEntryA + maxEntryB);
}
