public void Solve(ref MotionVelocity velocityA, ref MotionVelocity velocityB, float timestep, float 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
float 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;
float sinHalfAngleSq = math.lengthsq(jacA0);
float invSinHalfAngle = Math.RSqrtSafe(sinHalfAngleSq);
float sinHalfAngle = sinHalfAngleSq * invSinHalfAngle;
futureAngle = math.asin(sinHalfAngle) * 2.0f;
jacA0 = math.select(jacA0 * invSinHalfAngle, new float3(1, 0, 0), invSinHalfAngle == 0.0f);
jacA0 = math.select(jacA0, -jacA0, jointOrientation.value.w < 0.0f);
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.InverseInertiaAndMass.xyz + jacB0 * jacB0 * velocityB.InverseInertiaAndMass.xyz),
math.csum(jacA1 * jacA1 * velocityA.InverseInertiaAndMass.xyz + jacB1 * jacB1 * velocityB.InverseInertiaAndMass.xyz),
math.csum(jacA2 * jacA2 * velocityA.InverseInertiaAndMass.xyz + jacB2 * jacB2 * velocityB.InverseInertiaAndMass.xyz));
float3 invEffectiveMassOffDiag = new float3(
math.csum(jacA0 * jacA1 * velocityA.InverseInertiaAndMass.xyz + jacB0 * jacB1 * velocityB.InverseInertiaAndMass.xyz),
math.csum(jacA0 * jacA2 * velocityA.InverseInertiaAndMass.xyz + jacB0 * jacB2 * velocityB.InverseInertiaAndMass.xyz),
math.csum(jacA1 * jacA2 * velocityA.InverseInertiaAndMass.xyz + jacB1 * jacB2 * velocityB.InverseInertiaAndMass.xyz));
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
float futureError = JacobianUtilities.CalculateError(futureAngle, MinAngle, MaxAngle);
float solveError = JacobianUtilities.CalculateCorrection(futureError, InitialError, Tau, Damping);
float 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);
}