// Helper function
private sfloat CalculateError(quaternion motionBFromA)
{
// Calculate the relative body rotation
quaternion jointBFromA = math.mul(math.mul(math.inverse(MotionBFromJoint), motionBFromA), MotionAFromJoint);
// Find the twist angle of the rotation.
//
// There is no one correct solution for the twist angle. Suppose the joint models a pair of bodies connected by
// three gimbals, one of which is limited by this jacobian. There are multiple configurations of the gimbals that
// give the bodies the same relative orientation, so it is impossible to determine the configuration from the
// bodies' orientations alone, nor therefore the orientation of the limited gimbal.
//
// This code instead makes a reasonable guess, the twist angle of the swing-twist decomposition of the bodies'
// relative orientation. It always works when the limited axis itself is unable to rotate freely, as in a limited
// hinge. It works fairly well when the limited axis can only rotate a small amount, preferably less than 90
// degrees. It works poorly at higher angles, especially near 180 degrees where it is not continuous. For systems
// that require that kind of flexibility, the gimbals should be modeled as separate bodies.
sfloat angle = CalculateTwistAngle(jointBFromA, AxisIndex);
// Angle is in [-2pi, 2pi].
// For comparison against the limits, find k so that angle + 2k * pi is as close to [min, max] as possible.
sfloat centerAngle = (MinAngle + MaxAngle) / (sfloat)2.0f;
bool above = angle > (centerAngle + math.PI);
bool below = angle < (centerAngle - math.PI);
angle = math.select(angle, angle - (sfloat)2.0f * math.PI, above);
angle = math.select(angle, angle + (sfloat)2.0f * math.PI, below);
// Calculate the relative angle about the twist axis
return(JacobianUtilities.CalculateError(angle, MinAngle, MaxAngle));
}
// Build the Jacobian
public void Build(
MTransform aFromConstraint, MTransform bFromConstraint,
MotionVelocity velocityA, MotionVelocity velocityB,
MotionData motionA, MotionData motionB,
Constraint constraint, float tau, float damping)
{
this = default(AngularLimit2DJacobian);
// Copy the constraint data
int freeIndex = constraint.FreeAxis2D;
AxisAinA = aFromConstraint.Rotation[freeIndex];
AxisBinB = bFromConstraint.Rotation[freeIndex];
MinAngle = constraint.Min;
MaxAngle = constraint.Max;
Tau = tau;
Damping = damping;
BFromA = math.mul(math.inverse(motionB.WorldFromMotion.rot), motionA.WorldFromMotion.rot);
// Calculate the initial error
{
float3 axisAinB = math.mul(BFromA, AxisAinA);
float sinAngle = math.length(math.cross(axisAinB, AxisBinB));
float cosAngle = math.dot(axisAinB, AxisBinB);
float angle = math.atan2(sinAngle, cosAngle);
InitialError = JacobianUtilities.CalculateError(angle, MinAngle, MaxAngle);
}
}
private float CalculateError(MTransform worldFromA, MTransform worldFromB, out float3 direction)
{
// Find the direction from pivot A to B and the distance between them
float3 pivotA = Mul(worldFromA, PivotAinA);
float3 pivotB = Mul(worldFromB, PivotBinB);
float3 axis = math.mul(worldFromB.Rotation, AxisInB);
direction = pivotB - pivotA;
float dot = math.dot(direction, axis);
// Project for lower-dimension joints
float distance;
if (Is1D)
{
// In 1D, distance is signed and measured along the axis
distance = -dot;
direction = -axis;
}
else
{
// In 2D / 3D, distance is nonnegative. In 2D it is measured perpendicular to the axis.
direction -= axis * dot;
float futureDistanceSq = math.lengthsq(direction);
float invFutureDistance = math.select(math.rsqrt(futureDistanceSq), 0.0f, futureDistanceSq == 0.0f);
distance = futureDistanceSq * invFutureDistance;
direction *= invFutureDistance;
}
// Find the difference between the future distance and the limit range
return(JacobianUtilities.CalculateError(distance, MinDistance, MaxDistance));
}
public void JacobianUtilitiesCalculateErrorTest()
{
float x = 5.0f;
float min = 0.0f;
float max = 10.0f;
Assert.AreApproximatelyEqual(0.0f, JacobianUtilities.CalculateError(x, min, max));
}
// Solve the Jacobian
public void Solve(ref MotionVelocity velocityA, ref MotionVelocity velocityB, sfloat timestep, sfloat invTimestep)
{
// Predict the motions' transforms at the end of the step
MTransform futureWorldFromA;
MTransform futureWorldFromB;
{
quaternion dqA = Integrator.IntegrateAngularVelocity(velocityA.AngularVelocity, timestep);
quaternion dqB = Integrator.IntegrateAngularVelocity(velocityB.AngularVelocity, timestep);
quaternion futureOrientationA = math.normalize(math.mul(WorldFromA.rot, dqA));
quaternion futureOrientationB = math.normalize(math.mul(WorldFromB.rot, dqB));
futureWorldFromA = new MTransform(futureOrientationA, WorldFromA.pos + velocityA.LinearVelocity * timestep);
futureWorldFromB = new MTransform(futureOrientationB, WorldFromB.pos + velocityB.LinearVelocity * timestep);
}
// Calculate the angulars
CalculateAngulars(PivotAinA, futureWorldFromA.Rotation, out float3 angA0, out float3 angA1, out float3 angA2);
CalculateAngulars(PivotBinB, futureWorldFromB.Rotation, out float3 angB0, out float3 angB1, out float3 angB2);
// Calculate effective mass
float3 EffectiveMassDiag, EffectiveMassOffDiag;
{
// Calculate the inverse effective mass matrix
float3 invEffectiveMassDiag = new float3(
JacobianUtilities.CalculateInvEffectiveMassDiag(angA0, velocityA.InverseInertia, velocityA.InverseMass,
angB0, velocityB.InverseInertia, velocityB.InverseMass),
JacobianUtilities.CalculateInvEffectiveMassDiag(angA1, velocityA.InverseInertia, velocityA.InverseMass,
angB1, velocityB.InverseInertia, velocityB.InverseMass),
JacobianUtilities.CalculateInvEffectiveMassDiag(angA2, velocityA.InverseInertia, velocityA.InverseMass,
angB2, velocityB.InverseInertia, velocityB.InverseMass));
float3 invEffectiveMassOffDiag = new float3(
JacobianUtilities.CalculateInvEffectiveMassOffDiag(angA0, angA1, velocityA.InverseInertia, angB0, angB1, velocityB.InverseInertia),
JacobianUtilities.CalculateInvEffectiveMassOffDiag(angA0, angA2, velocityA.InverseInertia, angB0, angB2, velocityB.InverseInertia),
JacobianUtilities.CalculateInvEffectiveMassOffDiag(angA1, angA2, velocityA.InverseInertia, angB1, angB2, velocityB.InverseInertia));
// Invert to get the effective mass matrix
JacobianUtilities.InvertSymmetricMatrix(invEffectiveMassDiag, invEffectiveMassOffDiag, out EffectiveMassDiag, out EffectiveMassOffDiag);
}
// Predict error at the end of the step and calculate the impulse to correct it
float3 impulse;
{
// Find the difference between the future distance and the limit range, then apply tau and damping
sfloat futureDistanceError = CalculateError(futureWorldFromA, futureWorldFromB, out float3 futureDirection);
sfloat solveDistanceError = JacobianUtilities.CalculateCorrection(futureDistanceError, InitialError, Tau, Damping);
// Calculate the impulse to correct the error
float3 solveError = solveDistanceError * futureDirection;
float3x3 effectiveMass = JacobianUtilities.BuildSymmetricMatrix(EffectiveMassDiag, EffectiveMassOffDiag);
impulse = math.mul(effectiveMass, solveError) * invTimestep;
}
// Apply the impulse
ApplyImpulse(impulse, angA0, angA1, angA2, ref velocityA);
ApplyImpulse(-impulse, angB0, angB1, angB2, ref velocityB);
}
public void JacobianUtilitiesIntegrateOrientationBFromATest()
{
var bFromA = quaternion.identity;
var angularVelocityA = float3.zero;
var angularVelocityB = float3.zero;
var timestep = 1.0f;
Assert.AreEqual(quaternion.identity, JacobianUtilities.IntegrateOrientationBFromA(bFromA, angularVelocityA, angularVelocityB, timestep));
}
public void JacobianUtilitiesCalculateCorrectionTest()
{
float predictedError = 0.2f;
float initialError = 0.1f;
float tau = 0.6f;
float damping = 1.0f;
Assert.AreApproximatelyEqual(0.16f, JacobianUtilities.CalculateCorrection(predictedError, initialError, tau, damping));
}
public void JacobianUtilitiesCalculateTauAndDampingTest()
{
float springFrequency = 1.0f;
float springDampingRatio = 1.0f;
float timestep = 1.0f;
int iterations = 4;
JacobianUtilities.CalculateTauAndDamping(springFrequency, springDampingRatio, timestep, iterations, out float tau, out float damping);
Assert.AreApproximatelyEqual(0.4774722f, tau);
Assert.AreApproximatelyEqual(0.6294564f, damping);
}
public void Solve(ref MotionVelocity velocityA, ref MotionVelocity velocityB, sfloat timestep, sfloat invTimestep)
{
// Predict the relative orientation at the end of the step
quaternion futureBFromA = JacobianUtilities.IntegrateOrientationBFromA(BFromA, velocityA.AngularVelocity, velocityB.AngularVelocity, timestep);
// Find the future axis and angle of rotation between the free axes
float3 jacA0, jacA1, jacA2, jacB0, jacB1, jacB2;
float3 effectiveMass; // first column of 3x3 effective mass matrix, don't need the others because only jac0 can have nonzero error
sfloat futureAngle;
{
// Calculate the relative rotation between joint spaces
quaternion jointOrientation = math.mul(math.inverse(RefBFromA), futureBFromA);
// Find the axis and angle of rotation
jacA0 = jointOrientation.value.xyz;
sfloat sinHalfAngleSq = math.lengthsq(jacA0);
sfloat invSinHalfAngle = Math.RSqrtSafe(sinHalfAngleSq);
sfloat sinHalfAngle = sinHalfAngleSq * invSinHalfAngle;
futureAngle = math.asin(sinHalfAngle) * (sfloat)2.0f;
jacA0 = math.select(jacA0 * invSinHalfAngle, new float3(sfloat.One, sfloat.Zero, sfloat.Zero), invSinHalfAngle.IsZero());
jacA0 = math.select(jacA0, -jacA0, jointOrientation.value.w < sfloat.Zero);
Math.CalculatePerpendicularNormalized(jacA0, out jacA1, out jacA2);
jacB0 = math.mul(futureBFromA, -jacA0);
jacB1 = math.mul(futureBFromA, -jacA1);
jacB2 = math.mul(futureBFromA, -jacA2);
// Calculate the effective mass
float3 invEffectiveMassDiag = new float3(
math.csum(jacA0 * jacA0 * velocityA.InverseInertia + jacB0 * jacB0 * velocityB.InverseInertia),
math.csum(jacA1 * jacA1 * velocityA.InverseInertia + jacB1 * jacB1 * velocityB.InverseInertia),
math.csum(jacA2 * jacA2 * velocityA.InverseInertia + jacB2 * jacB2 * velocityB.InverseInertia));
float3 invEffectiveMassOffDiag = new float3(
math.csum(jacA0 * jacA1 * velocityA.InverseInertia + jacB0 * jacB1 * velocityB.InverseInertia),
math.csum(jacA0 * jacA2 * velocityA.InverseInertia + jacB0 * jacB2 * velocityB.InverseInertia),
math.csum(jacA1 * jacA2 * velocityA.InverseInertia + jacB1 * jacB2 * velocityB.InverseInertia));
JacobianUtilities.InvertSymmetricMatrix(invEffectiveMassDiag, invEffectiveMassOffDiag, out float3 effectiveMassDiag, out float3 effectiveMassOffDiag);
effectiveMass = JacobianUtilities.BuildSymmetricMatrix(effectiveMassDiag, effectiveMassOffDiag).c0;
}
// Calculate the error, adjust by tau and damping, and apply an impulse to correct it
sfloat futureError = JacobianUtilities.CalculateError(futureAngle, MinAngle, MaxAngle);
sfloat solveError = JacobianUtilities.CalculateCorrection(futureError, InitialError, Tau, Damping);
sfloat solveVelocity = -solveError * invTimestep;
float3 impulseA = solveVelocity * (jacA0 * effectiveMass.x + jacA1 * effectiveMass.y + jacA2 * effectiveMass.z);
float3 impulseB = solveVelocity * (jacB0 * effectiveMass.x + jacB1 * effectiveMass.y + jacB2 * effectiveMass.z);
velocityA.ApplyAngularImpulse(impulseA);
velocityB.ApplyAngularImpulse(impulseB);
}
// Solve the Jacobian
public void Solve(ref MotionVelocity velocityA, ref MotionVelocity velocityB, float timestep)
{
// Predict the relative orientation at the end of the step
quaternion futureBFromA = JacobianUtilities.IntegrateOrientationBFromA(BFromA, velocityA.AngularVelocity, velocityB.AngularVelocity, timestep);
// Calculate the jacobian axis and angle
float3 axisAinB = math.mul(futureBFromA, AxisAinA);
float3 jacB0 = math.cross(axisAinB, AxisBinB);
float3 jacA0 = math.mul(math.inverse(futureBFromA), -jacB0);
float jacLengthSq = math.lengthsq(jacB0);
float invJacLength = Math.RSqrtSafe(jacLengthSq);
float futureAngle;
{
float sinAngle = jacLengthSq * invJacLength;
float cosAngle = math.dot(axisAinB, AxisBinB);
futureAngle = math.atan2(sinAngle, cosAngle);
}
// Choose a second jacobian axis perpendicular to A
float3 jacB1 = math.cross(jacB0, axisAinB);
float3 jacA1 = math.mul(math.inverse(futureBFromA), -jacB1);
// Calculate effective mass
float2 effectiveMass; // First column of the 2x2 matrix, we don't need the second column because the second component of error is zero
{
// Calculate the inverse effective mass matrix, then invert it
float invEffMassDiag0 = math.csum(jacA0 * jacA0 * velocityA.InverseInertiaAndMass.xyz + jacB0 * jacB0 * velocityB.InverseInertiaAndMass.xyz);
float invEffMassDiag1 = math.csum(jacA1 * jacA1 * velocityA.InverseInertiaAndMass.xyz + jacB1 * jacB1 * velocityB.InverseInertiaAndMass.xyz);
float invEffMassOffDiag = math.csum(jacA0 * jacA1 * velocityA.InverseInertiaAndMass.xyz + jacB0 * jacB1 * velocityB.InverseInertiaAndMass.xyz);
float det = invEffMassDiag0 * invEffMassDiag1 - invEffMassOffDiag * invEffMassOffDiag;
float invDet = math.select(jacLengthSq / det, 0.0f, det == 0.0f); // scale by jacLengthSq because the jacs were not normalized
effectiveMass = invDet * new float2(invEffMassDiag1, -invEffMassOffDiag);
}
// Normalize the jacobians
jacA0 *= invJacLength;
jacB0 *= invJacLength;
jacA1 *= invJacLength;
jacB1 *= invJacLength;
// Calculate the error, adjust by tau and damping, and apply an impulse to correct it
float futureError = JacobianUtilities.CalculateError(futureAngle, MinAngle, MaxAngle);
float solveError = JacobianUtilities.CalculateCorrection(futureError, InitialError, Tau, Damping);
float2 impulse = -effectiveMass * solveError * (1.0f / timestep);
velocityA.ApplyAngularImpulse(impulse.x * jacA0 + impulse.y * jacA1);
velocityB.ApplyAngularImpulse(impulse.x * jacB0 + impulse.y * jacB1);
}
public void JacobianUtilitiesCalculateTauAndDampingFromConstraintTest()
{
var constraint = new Constraint {
SpringFrequency = 1.0f, SpringDamping = 1.0f
};
float timestep = 1.0f;
int iterations = 4;
float tau;
float damping;
JacobianUtilities.CalculateTauAndDamping(constraint, timestep, iterations, out tau, out damping);
Assert.AreApproximatelyEqual(0.4774722f, tau);
Assert.AreApproximatelyEqual(0.6294564f, damping);
}
// Build the Jacobian
public void Build(
MTransform aFromConstraint, MTransform bFromConstraint,
MotionVelocity velocityA, MotionVelocity velocityB,
MotionData motionA, MotionData motionB,
Constraint constraint, sfloat tau, sfloat damping)
{
BFromA = math.mul(math.inverse(motionB.WorldFromMotion.rot), motionA.WorldFromMotion.rot);
RefBFromA = new quaternion(math.mul(bFromConstraint.Rotation, aFromConstraint.InverseRotation));
MinAngle = constraint.Min;
MaxAngle = constraint.Max;
Tau = tau;
Damping = damping;
quaternion jointOrientation = math.mul(math.inverse(RefBFromA), BFromA);
sfloat initialAngle = math.asin(math.length(jointOrientation.value.xyz)) * (sfloat)2.0f;
InitialError = JacobianUtilities.CalculateError(initialAngle, MinAngle, MaxAngle);
}
// Build the Jacobian
public void Build(
MTransform aFromConstraint, MTransform bFromConstraint,
MotionVelocity velocityA, MotionVelocity velocityB,
MotionData motionA, MotionData motionB,
Constraint constraint, float tau, float damping)
{
this = default(AngularLimit3DJacobian);
BFromA = math.mul(math.inverse(motionB.WorldFromMotion.rot), motionA.WorldFromMotion.rot);
MotionAFromJoint = new quaternion(aFromConstraint.Rotation);
MotionBFromJoint = new quaternion(bFromConstraint.Rotation);
MinAngle = constraint.Min;
MaxAngle = constraint.Max;
Tau = tau;
Damping = damping;
float initialAngle = math.atan2(math.length(BFromA.value.xyz), BFromA.value.w) * 2.0f;
InitialError = JacobianUtilities.CalculateError(initialAngle, MinAngle, MaxAngle);
}
// Solve the Jacobian
public void Solve(ref MotionVelocity velocityA, ref MotionVelocity velocityB, sfloat timestep, sfloat invTimestep)
{
// Predict the relative orientation at the end of the step
quaternion futureMotionBFromA = JacobianUtilities.IntegrateOrientationBFromA(MotionBFromA, velocityA.AngularVelocity, velocityB.AngularVelocity, timestep);
// Calculate the effective mass
float3 axisInMotionB = math.mul(futureMotionBFromA, -AxisInMotionA);
sfloat effectiveMass;
{
sfloat invEffectiveMass = math.csum(AxisInMotionA * AxisInMotionA * velocityA.InverseInertia +
axisInMotionB * axisInMotionB * velocityB.InverseInertia);
effectiveMass = math.select(sfloat.One / invEffectiveMass, sfloat.Zero, invEffectiveMass.IsZero());
}
// Calculate the error, adjust by tau and damping, and apply an impulse to correct it
sfloat futureError = CalculateError(futureMotionBFromA);
sfloat solveError = JacobianUtilities.CalculateCorrection(futureError, InitialError, Tau, Damping);
sfloat impulse = math.mul(effectiveMass, -solveError) * invTimestep;
velocityA.ApplyAngularImpulse(impulse * AxisInMotionA);
velocityB.ApplyAngularImpulse(impulse * axisInMotionB);
}