// 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 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); }