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, sfloat timestep, sfloat invTimestep)
{
// 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);
sfloat jacLengthSq = math.lengthsq(jacB0);
sfloat invJacLength = Math.RSqrtSafe(jacLengthSq);
sfloat futureAngle;
{
sfloat sinAngle = jacLengthSq * invJacLength;
sfloat 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
sfloat invEffMassDiag0 = math.csum(jacA0 * jacA0 * velocityA.InverseInertia + jacB0 * jacB0 * velocityB.InverseInertia);
sfloat invEffMassDiag1 = math.csum(jacA1 * jacA1 * velocityA.InverseInertia + jacB1 * jacB1 * velocityB.InverseInertia);
sfloat invEffMassOffDiag = math.csum(jacA0 * jacA1 * velocityA.InverseInertia + jacB0 * jacB1 * velocityB.InverseInertia);
sfloat det = invEffMassDiag0 * invEffMassDiag1 - invEffMassOffDiag * invEffMassOffDiag;
sfloat invDet = math.select(jacLengthSq / det, sfloat.Zero, det.IsZero()); // 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
sfloat futureError = JacobianUtilities.CalculateError(futureAngle, MinAngle, MaxAngle);
sfloat solveError = JacobianUtilities.CalculateCorrection(futureError, InitialError, Tau, Damping);
float2 impulse = -effectiveMass * solveError * invTimestep;
velocityA.ApplyAngularImpulse(impulse.x * jacA0 + impulse.y * jacA1);
velocityB.ApplyAngularImpulse(impulse.x * jacB0 + impulse.y * jacB1);
}
// 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);
}
private static void ApplyImpulse(float3 impulse, float3 ang0, float3 ang1, float3 ang2, ref MotionVelocity velocity)
{
velocity.ApplyLinearImpulse(impulse);
velocity.ApplyAngularImpulse(impulse.x * ang0 + impulse.y * ang1 + impulse.z * ang2);
}
