From f39cad748202c81a8cff429de025daebfe9b57ce Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 16 Aug 2021 07:19:12 +0200 Subject: [PATCH] Solve most important constraints at the end for better simulation results --- src/systems/SolveBallAndSocketJointSystem.cpp | 140 +++---- src/systems/SolveHingeJointSystem.cpp | 324 ++++++++-------- src/systems/SolveSliderJointSystem.cpp | 366 +++++++++--------- 3 files changed, 417 insertions(+), 413 deletions(-) diff --git a/src/systems/SolveBallAndSocketJointSystem.cpp b/src/systems/SolveBallAndSocketJointSystem.cpp index 6d8971bd..31a4df8d 100644 --- a/src/systems/SolveBallAndSocketJointSystem.cpp +++ b/src/systems/SolveBallAndSocketJointSystem.cpp @@ -240,28 +240,6 @@ void SolveBallAndSocketJointSystem::solveVelocityConstraint() { const Matrix3x3& i1 = mBallAndSocketJointComponents.mI1[i]; const Matrix3x3& i2 = mBallAndSocketJointComponents.mI2[i]; - // Compute J*v - const Vector3 Jv = v2 + w2.cross(mBallAndSocketJointComponents.mR2World[i]) - v1 - w1.cross(mBallAndSocketJointComponents.mR1World[i]); - - // Compute the Lagrange multiplier lambda - const Vector3 deltaLambda = mBallAndSocketJointComponents.mInverseMassMatrix[i] * (-Jv - mBallAndSocketJointComponents.mBiasVector[i]); - mBallAndSocketJointComponents.mImpulse[i] += deltaLambda; - - // Compute the impulse P=J^T * lambda for the body 1 - const Vector3 linearImpulseBody1 = -deltaLambda; - const Vector3 angularImpulseBody1 = deltaLambda.cross(mBallAndSocketJointComponents.mR1World[i]); - - // Apply the impulse to the body 1 - v1 += mRigidBodyComponents.mInverseMasses[componentIndexBody1] * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; - w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1); - - // Compute the impulse P=J^T * lambda for the body 2 - const Vector3 angularImpulseBody2 = -deltaLambda.cross(mBallAndSocketJointComponents.mR2World[i]); - - // Apply the impulse to the body 2 - v2 += mRigidBodyComponents.mInverseMasses[componentIndexBody2] * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * deltaLambda; - w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2); - // --------------- Limits Constraints --------------- // if (mBallAndSocketJointComponents.mIsConeLimitEnabled[i]) { @@ -292,6 +270,30 @@ void SolveBallAndSocketJointSystem::solveVelocityConstraint() { } } + + // --------------- Joint Constraints --------------- // + + // Compute J*v + const Vector3 Jv = v2 + w2.cross(mBallAndSocketJointComponents.mR2World[i]) - v1 - w1.cross(mBallAndSocketJointComponents.mR1World[i]); + + // Compute the Lagrange multiplier lambda + const Vector3 deltaLambda = mBallAndSocketJointComponents.mInverseMassMatrix[i] * (-Jv - mBallAndSocketJointComponents.mBiasVector[i]); + mBallAndSocketJointComponents.mImpulse[i] += deltaLambda; + + // Compute the impulse P=J^T * lambda for the body 1 + const Vector3 linearImpulseBody1 = -deltaLambda; + const Vector3 angularImpulseBody1 = deltaLambda.cross(mBallAndSocketJointComponents.mR1World[i]); + + // Apply the impulse to the body 1 + v1 += mRigidBodyComponents.mInverseMasses[componentIndexBody1] * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; + w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1); + + // Compute the impulse P=J^T * lambda for the body 2 + const Vector3 angularImpulseBody2 = -deltaLambda.cross(mBallAndSocketJointComponents.mR2World[i]); + + // Apply the impulse to the body 2 + v2 += mRigidBodyComponents.mInverseMasses[componentIndexBody2] * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * deltaLambda; + w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2); } } @@ -342,6 +344,54 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() { const decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; const decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2]; + // --------------- Limits Constraints --------------- // + + if (mBallAndSocketJointComponents.mIsConeLimitEnabled[i]) { + + // Check if the cone limit constraints is violated or not + const Vector3 coneAxisBody1World = q1 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody1[i]; + const Vector3 coneAxisBody2World = q2 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody2[i]; + mBallAndSocketJointComponents.mConeLimitACrossB[i] = coneAxisBody1World.cross(coneAxisBody2World); + decimal coneAngle = computeCurrentConeHalfAngle(coneAxisBody1World, coneAxisBody2World); + decimal coneLimitError = mBallAndSocketJointComponents.mConeLimitHalfAngle[i] - coneAngle; + mBallAndSocketJointComponents.mIsConeLimitViolated[i] = coneLimitError < 0; + + // If the cone limit is violated + if (mBallAndSocketJointComponents.mIsConeLimitViolated[i]) { + + // Compute the inverse of the mass matrix K=JM^-1J^t for the cone limit (1x1 matrix) + decimal inverseMassMatrixConeLimit = mBallAndSocketJointComponents.mConeLimitACrossB[i].dot(mBallAndSocketJointComponents.mI1[i] * mBallAndSocketJointComponents.mConeLimitACrossB[i]) + + mBallAndSocketJointComponents.mConeLimitACrossB[i].dot(mBallAndSocketJointComponents.mI2[i] * mBallAndSocketJointComponents.mConeLimitACrossB[i]); + mBallAndSocketJointComponents.mInverseMassMatrixConeLimit[i] = (inverseMassMatrixConeLimit > decimal(0.0)) ? + decimal(1.0) / inverseMassMatrixConeLimit : decimal(0.0); + + // Compute the Lagrange multiplier lambda for the cone limit constraint + decimal lambdaConeLimit = mBallAndSocketJointComponents.mInverseMassMatrixConeLimit[i] * (-coneLimitError ); + + // Compute the impulse P=J^T * lambda of body 1 + const Vector3 angularImpulseBody1 = lambdaConeLimit * mBallAndSocketJointComponents.mConeLimitACrossB[i]; + + // Compute the pseudo velocity of body 1 + const Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mBallAndSocketJointComponents.mI1[i] * angularImpulseBody1); + + // Update the body position/orientation of body 1 + q1 += Quaternion(0, w1) * q1 * decimal(0.5); + q1.normalize(); + + // Compute the impulse P=J^T * lambda of body 2 + const Vector3 angularImpulseBody2 = -lambdaConeLimit * mBallAndSocketJointComponents.mConeLimitACrossB[i]; + + // Compute the pseudo velocity of body 2 + const Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mBallAndSocketJointComponents.mI2[i] * angularImpulseBody2); + + // Update the body position/orientation of body 2 + q2 += Quaternion(0, w2) * q2 * decimal(0.5); + q2.normalize(); + } + } + + // --------------- Joint Constraints --------------- // + // Recompute the inverse mass matrix K=J^TM^-1J of of the 3 translation constraints decimal inverseMassBodies = inverseMassBody1 + inverseMassBody2; Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, @@ -394,51 +444,5 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() { q2 += Quaternion(0, w2) * q2 * decimal(0.5); q2.normalize(); } - - // --------------- Limits Constraints --------------- // - - if (mBallAndSocketJointComponents.mIsConeLimitEnabled[i]) { - - // Check if the cone limit constraints is violated or not - const Vector3 coneAxisBody1World = q1 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody1[i]; - const Vector3 coneAxisBody2World = q2 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody2[i]; - mBallAndSocketJointComponents.mConeLimitACrossB[i] = coneAxisBody1World.cross(coneAxisBody2World); - decimal coneAngle = computeCurrentConeHalfAngle(coneAxisBody1World, coneAxisBody2World); - decimal coneLimitError = mBallAndSocketJointComponents.mConeLimitHalfAngle[i] - coneAngle; - mBallAndSocketJointComponents.mIsConeLimitViolated[i] = coneLimitError < 0; - - // If the cone limit is violated - if (mBallAndSocketJointComponents.mIsConeLimitViolated[i]) { - - // Compute the inverse of the mass matrix K=JM^-1J^t for the cone limit (1x1 matrix) - decimal inverseMassMatrixConeLimit = mBallAndSocketJointComponents.mConeLimitACrossB[i].dot(mBallAndSocketJointComponents.mI1[i] * mBallAndSocketJointComponents.mConeLimitACrossB[i]) + - mBallAndSocketJointComponents.mConeLimitACrossB[i].dot(mBallAndSocketJointComponents.mI2[i] * mBallAndSocketJointComponents.mConeLimitACrossB[i]); - mBallAndSocketJointComponents.mInverseMassMatrixConeLimit[i] = (inverseMassMatrixConeLimit > decimal(0.0)) ? - decimal(1.0) / inverseMassMatrixConeLimit : decimal(0.0); - - // Compute the Lagrange multiplier lambda for the cone limit constraint - decimal lambdaConeLimit = mBallAndSocketJointComponents.mInverseMassMatrixConeLimit[i] * (-coneLimitError ); - - // Compute the impulse P=J^T * lambda of body 1 - const Vector3 angularImpulseBody1 = lambdaConeLimit * mBallAndSocketJointComponents.mConeLimitACrossB[i]; - - // Compute the pseudo velocity of body 1 - const Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mBallAndSocketJointComponents.mI1[i] * angularImpulseBody1); - - // Update the body position/orientation of body 1 - q1 += Quaternion(0, w1) * q1 * decimal(0.5); - q1.normalize(); - - // Compute the impulse P=J^T * lambda of body 2 - const Vector3 angularImpulseBody2 = -lambdaConeLimit * mBallAndSocketJointComponents.mConeLimitACrossB[i]; - - // Compute the pseudo velocity of body 2 - const Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mBallAndSocketJointComponents.mI2[i] * angularImpulseBody2); - - // Update the body position/orientation of body 2 - q2 += Quaternion(0, w2) * q2 * decimal(0.5); - q2.normalize(); - } - } } } diff --git a/src/systems/SolveHingeJointSystem.cpp b/src/systems/SolveHingeJointSystem.cpp index e547ce14..6926fb12 100644 --- a/src/systems/SolveHingeJointSystem.cpp +++ b/src/systems/SolveHingeJointSystem.cpp @@ -330,57 +330,6 @@ void SolveHingeJointSystem::solveVelocityConstraint() { const decimal inverseMassMatrixLimitMotor = mHingeJointComponents.mInverseMassMatrixLimitMotor[i]; - // --------------- Translation Constraints --------------- // - - // Compute J*v - const Vector3 JvTranslation = v2 + w2.cross(r2World) - v1 - w1.cross(r1World); - - // Compute the Lagrange multiplier lambda - const Vector3 deltaLambdaTranslation = mHingeJointComponents.mInverseMassMatrixTranslation[i] * - (-JvTranslation - mHingeJointComponents.mBiasTranslation[i]); - mHingeJointComponents.mImpulseTranslation[i] += deltaLambdaTranslation; - - // Compute the impulse P=J^T * lambda of body 1 - const Vector3 linearImpulseBody1 = -deltaLambdaTranslation; - Vector3 angularImpulseBody1 = deltaLambdaTranslation.cross(r1World); - - // Apply the impulse to the body 1 - v1 += inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; - w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1); - - // Compute the impulse P=J^T * lambda of body 2 - Vector3 angularImpulseBody2 = -deltaLambdaTranslation.cross(r2World); - - // Apply the impulse to the body 2 - v2 += inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * deltaLambdaTranslation; - w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2); - - // --------------- Rotation Constraints --------------- // - - const Vector3& b2CrossA1 = mHingeJointComponents.mB2CrossA1[i]; - const Vector3& c2CrossA1 = mHingeJointComponents.mC2CrossA1[i]; - - // Compute J*v for the 2 rotation constraints - const Vector2 JvRotation(-b2CrossA1.dot(w1) + b2CrossA1.dot(w2), - -c2CrossA1.dot(w1) + c2CrossA1.dot(w2)); - - // Compute the Lagrange multiplier lambda for the 2 rotation constraints - Vector2 deltaLambdaRotation = mHingeJointComponents.mInverseMassMatrixRotation[i] * - (-JvRotation - mHingeJointComponents.mBiasRotation[i]); - mHingeJointComponents.mImpulseRotation[i] += deltaLambdaRotation; - - // Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 1 - angularImpulseBody1 = -b2CrossA1 * deltaLambdaRotation.x - c2CrossA1 * deltaLambdaRotation.y; - - // Apply the impulse to the body 1 - w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1); - - // Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 2 - angularImpulseBody2 = b2CrossA1 * deltaLambdaRotation.x + c2CrossA1 * deltaLambdaRotation.y; - - // Apply the impulse to the body 2 - w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2); - // --------------- Limits Constraints --------------- // if (mHingeJointComponents.mIsLimitEnabled[i]) { @@ -463,6 +412,57 @@ void SolveHingeJointSystem::solveVelocityConstraint() { // Apply the impulse to the body 2 w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2); } + + // --------------- Joint Rotation Constraints --------------- // + + const Vector3& b2CrossA1 = mHingeJointComponents.mB2CrossA1[i]; + const Vector3& c2CrossA1 = mHingeJointComponents.mC2CrossA1[i]; + + // Compute J*v for the 2 rotation constraints + const Vector2 JvRotation(-b2CrossA1.dot(w1) + b2CrossA1.dot(w2), + -c2CrossA1.dot(w1) + c2CrossA1.dot(w2)); + + // Compute the Lagrange multiplier lambda for the 2 rotation constraints + Vector2 deltaLambdaRotation = mHingeJointComponents.mInverseMassMatrixRotation[i] * + (-JvRotation - mHingeJointComponents.mBiasRotation[i]); + mHingeJointComponents.mImpulseRotation[i] += deltaLambdaRotation; + + // Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 1 + Vector3 angularImpulseBody1 = -b2CrossA1 * deltaLambdaRotation.x - c2CrossA1 * deltaLambdaRotation.y; + + // Apply the impulse to the body 1 + w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1); + + // Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 2 + Vector3 angularImpulseBody2 = b2CrossA1 * deltaLambdaRotation.x + c2CrossA1 * deltaLambdaRotation.y; + + // Apply the impulse to the body 2 + w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2); + + // --------------- Joint Translation Constraints --------------- // + + // Compute J*v + const Vector3 JvTranslation = v2 + w2.cross(r2World) - v1 - w1.cross(r1World); + + // Compute the Lagrange multiplier lambda + const Vector3 deltaLambdaTranslation = mHingeJointComponents.mInverseMassMatrixTranslation[i] * + (-JvTranslation - mHingeJointComponents.mBiasTranslation[i]); + mHingeJointComponents.mImpulseTranslation[i] += deltaLambdaTranslation; + + // Compute the impulse P=J^T * lambda of body 1 + const Vector3 linearImpulseBody1 = -deltaLambdaTranslation; + angularImpulseBody1 = deltaLambdaTranslation.cross(r1World); + + // Apply the impulse to the body 1 + v1 += inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; + w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1); + + // Compute the impulse P=J^T * lambda of body 2 + angularImpulseBody2 = -deltaLambdaTranslation.cross(r2World); + + // Apply the impulse to the body 2 + v2 += inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * deltaLambdaTranslation; + w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2); } } @@ -504,7 +504,6 @@ void SolveHingeJointSystem::solvePositionConstraint() { Matrix3x3 skewSymmetricMatrixU1 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mHingeJointComponents.mR1World[i]); Matrix3x3 skewSymmetricMatrixU2 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mHingeJointComponents.mR2World[i]); - // --------------- Translation Constraints --------------- // Vector3& b2CrossA1 = mHingeJointComponents.mB2CrossA1[i]; Vector3& c2CrossA1 = mHingeJointComponents.mC2CrossA1[i]; @@ -524,116 +523,6 @@ void SolveHingeJointSystem::solvePositionConstraint() { c2CrossA1 = c2.cross(a1); mHingeJointComponents.mC2CrossA1[i] = c2CrossA1; - // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints - const decimal body1InverseMass = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; - const decimal body2InverseMass = mRigidBodyComponents.mInverseMasses[componentIndexBody2]; - decimal inverseMassBodies = body1InverseMass + body2InverseMass; - Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, - 0, inverseMassBodies, 0, - 0, 0, inverseMassBodies) + - skewSymmetricMatrixU1 * mHingeJointComponents.mI1[i] * skewSymmetricMatrixU1.getTranspose() + - skewSymmetricMatrixU2 * mHingeJointComponents.mI2[i] * skewSymmetricMatrixU2.getTranspose(); - mHingeJointComponents.mInverseMassMatrixTranslation[i].setToZero(); - decimal matrixDeterminant = massMatrix.getDeterminant(); - if (std::abs(matrixDeterminant) > MACHINE_EPSILON) { - - if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || - mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { - mHingeJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse(matrixDeterminant); - } - - - Vector3& x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1]; - Vector3& x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2]; - - // Compute position error for the 3 translation constraints - const Vector3 errorTranslation = x2 + mHingeJointComponents.mR2World[i] - x1 - mHingeJointComponents.mR1World[i]; - - // Compute the Lagrange multiplier lambda - const Vector3 lambdaTranslation = mHingeJointComponents.mInverseMassMatrixTranslation[i] * (-errorTranslation); - - // Compute the impulse of body 1 - Vector3 linearImpulseBody1 = -lambdaTranslation; - Vector3 angularImpulseBody1 = lambdaTranslation.cross(mHingeJointComponents.mR1World[i]); - - // Get the inverse mass and inverse inertia tensors of the bodies - decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; - decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2]; - - // Compute the pseudo velocity of body 1 - const Vector3 v1 = inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; - Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mHingeJointComponents.mI1[i] * angularImpulseBody1); - - // Update the body position/orientation of body 1 - x1 += v1; - q1 += Quaternion(0, w1) * q1 * decimal(0.5); - q1.normalize(); - - // Compute the impulse of body 2 - Vector3 angularImpulseBody2 = -lambdaTranslation.cross(mHingeJointComponents.mR2World[i]); - - // Compute the pseudo velocity of body 2 - const Vector3 v2 = inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * lambdaTranslation; - Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mHingeJointComponents.mI2[i] * angularImpulseBody2); - - // Update the body position/orientation of body 2 - x2 += v2; - q2 += Quaternion(0, w2) * q2 * decimal(0.5); - q2.normalize(); - } - - // --------------- Rotation Constraints --------------- // - - // Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix) - Vector3 I1B2CrossA1 = mHingeJointComponents.mI1[i] * b2CrossA1; - Vector3 I1C2CrossA1 = mHingeJointComponents.mI1[i] * c2CrossA1; - Vector3 I2B2CrossA1 = mHingeJointComponents.mI2[i] * b2CrossA1; - Vector3 I2C2CrossA1 = mHingeJointComponents.mI2[i] * c2CrossA1; - const decimal el11 = b2CrossA1.dot(I1B2CrossA1) + - b2CrossA1.dot(I2B2CrossA1); - const decimal el12 = b2CrossA1.dot(I1C2CrossA1) + - b2CrossA1.dot(I2C2CrossA1); - const decimal el21 = c2CrossA1.dot(I1B2CrossA1) + - c2CrossA1.dot(I2B2CrossA1); - const decimal el22 = c2CrossA1.dot(I1C2CrossA1) + - c2CrossA1.dot(I2C2CrossA1); - const Matrix2x2 matrixKRotation(el11, el12, el21, el22); - mHingeJointComponents.mInverseMassMatrixRotation[i].setToZero(); - matrixDeterminant = matrixKRotation.getDeterminant(); - if (std::abs(matrixDeterminant) > MACHINE_EPSILON) { - if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || - mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { - mHingeJointComponents.mInverseMassMatrixRotation[i] = matrixKRotation.getInverse(matrixDeterminant); - } - - // Compute the position error for the 3 rotation constraints - const Vector2 errorRotation = Vector2(a1.dot(b2), a1.dot(c2)); - - // Compute the Lagrange multiplier lambda for the 3 rotation constraints - Vector2 lambdaRotation = mHingeJointComponents.mInverseMassMatrixRotation[i] * (-errorRotation); - - // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 - Vector3 angularImpulseBody1 = -b2CrossA1 * lambdaRotation.x - c2CrossA1 * lambdaRotation.y; - - // Compute the pseudo velocity of body 1 - Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mHingeJointComponents.mI1[i] * angularImpulseBody1); - - // Update the body position/orientation of body 1 - q1 += Quaternion(0, w1) * q1 * decimal(0.5); - q1.normalize(); - - // Compute the impulse of body 2 - Vector3 angularImpulseBody2 = b2CrossA1 * lambdaRotation.x + c2CrossA1 * lambdaRotation.y; - - // Compute the pseudo velocity of body 2 - Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mHingeJointComponents.mI2[i] * angularImpulseBody2); - - // Update the body position/orientation of body 2 - q2 += Quaternion(0, w2) * q2 * decimal(0.5); - q2.normalize(); - - } - // Compute the current angle around the hinge axis const decimal hingeAngle = computeCurrentHingeAngle(jointEntity, q1, q2); @@ -713,6 +602,117 @@ void SolveHingeJointSystem::solvePositionConstraint() { q2.normalize(); } } + + // --------------- Rotation Constraints --------------- // + + // Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix) + Vector3 I1B2CrossA1 = mHingeJointComponents.mI1[i] * b2CrossA1; + Vector3 I1C2CrossA1 = mHingeJointComponents.mI1[i] * c2CrossA1; + Vector3 I2B2CrossA1 = mHingeJointComponents.mI2[i] * b2CrossA1; + Vector3 I2C2CrossA1 = mHingeJointComponents.mI2[i] * c2CrossA1; + const decimal el11 = b2CrossA1.dot(I1B2CrossA1) + + b2CrossA1.dot(I2B2CrossA1); + const decimal el12 = b2CrossA1.dot(I1C2CrossA1) + + b2CrossA1.dot(I2C2CrossA1); + const decimal el21 = c2CrossA1.dot(I1B2CrossA1) + + c2CrossA1.dot(I2B2CrossA1); + const decimal el22 = c2CrossA1.dot(I1C2CrossA1) + + c2CrossA1.dot(I2C2CrossA1); + const Matrix2x2 matrixKRotation(el11, el12, el21, el22); + mHingeJointComponents.mInverseMassMatrixRotation[i].setToZero(); + decimal matrixDeterminant = matrixKRotation.getDeterminant(); + if (std::abs(matrixDeterminant) > MACHINE_EPSILON) { + if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || + mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { + mHingeJointComponents.mInverseMassMatrixRotation[i] = matrixKRotation.getInverse(matrixDeterminant); + } + + // Compute the position error for the 3 rotation constraints + const Vector2 errorRotation = Vector2(a1.dot(b2), a1.dot(c2)); + + // Compute the Lagrange multiplier lambda for the 3 rotation constraints + Vector2 lambdaRotation = mHingeJointComponents.mInverseMassMatrixRotation[i] * (-errorRotation); + + // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 + Vector3 angularImpulseBody1 = -b2CrossA1 * lambdaRotation.x - c2CrossA1 * lambdaRotation.y; + + // Compute the pseudo velocity of body 1 + Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mHingeJointComponents.mI1[i] * angularImpulseBody1); + + // Update the body position/orientation of body 1 + q1 += Quaternion(0, w1) * q1 * decimal(0.5); + q1.normalize(); + + // Compute the impulse of body 2 + Vector3 angularImpulseBody2 = b2CrossA1 * lambdaRotation.x + c2CrossA1 * lambdaRotation.y; + + // Compute the pseudo velocity of body 2 + Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mHingeJointComponents.mI2[i] * angularImpulseBody2); + + // Update the body position/orientation of body 2 + q2 += Quaternion(0, w2) * q2 * decimal(0.5); + q2.normalize(); + } + + // --------------- Translation Constraints --------------- // + + // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints + const decimal body1InverseMass = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; + const decimal body2InverseMass = mRigidBodyComponents.mInverseMasses[componentIndexBody2]; + decimal inverseMassBodies = body1InverseMass + body2InverseMass; + Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, + 0, inverseMassBodies, 0, + 0, 0, inverseMassBodies) + + skewSymmetricMatrixU1 * mHingeJointComponents.mI1[i] * skewSymmetricMatrixU1.getTranspose() + + skewSymmetricMatrixU2 * mHingeJointComponents.mI2[i] * skewSymmetricMatrixU2.getTranspose(); + mHingeJointComponents.mInverseMassMatrixTranslation[i].setToZero(); + matrixDeterminant = massMatrix.getDeterminant(); + if (std::abs(matrixDeterminant) > MACHINE_EPSILON) { + + if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || + mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { + mHingeJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse(matrixDeterminant); + } + + + Vector3& x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1]; + Vector3& x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2]; + + // Compute position error for the 3 translation constraints + const Vector3 errorTranslation = x2 + mHingeJointComponents.mR2World[i] - x1 - mHingeJointComponents.mR1World[i]; + + // Compute the Lagrange multiplier lambda + const Vector3 lambdaTranslation = mHingeJointComponents.mInverseMassMatrixTranslation[i] * (-errorTranslation); + + // Compute the impulse of body 1 + Vector3 linearImpulseBody1 = -lambdaTranslation; + Vector3 angularImpulseBody1 = lambdaTranslation.cross(mHingeJointComponents.mR1World[i]); + + // Get the inverse mass and inverse inertia tensors of the bodies + decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; + decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2]; + + // Compute the pseudo velocity of body 1 + const Vector3 v1 = inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; + Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mHingeJointComponents.mI1[i] * angularImpulseBody1); + + // Update the body position/orientation of body 1 + x1 += v1; + q1 += Quaternion(0, w1) * q1 * decimal(0.5); + q1.normalize(); + + // Compute the impulse of body 2 + Vector3 angularImpulseBody2 = -lambdaTranslation.cross(mHingeJointComponents.mR2World[i]); + + // Compute the pseudo velocity of body 2 + const Vector3 v2 = inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * lambdaTranslation; + Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mHingeJointComponents.mI2[i] * angularImpulseBody2); + + // Update the body position/orientation of body 2 + x2 += v2; + q2 += Quaternion(0, w2) * q2 * decimal(0.5); + q2.normalize(); + } } } diff --git a/src/systems/SolveSliderJointSystem.cpp b/src/systems/SolveSliderJointSystem.cpp index 7077c305..2d1c9d92 100644 --- a/src/systems/SolveSliderJointSystem.cpp +++ b/src/systems/SolveSliderJointSystem.cpp @@ -357,58 +357,6 @@ void SolveSliderJointSystem::solveVelocityConstraint() { decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2]; - // --------------- Translation Constraints --------------- // - - // Compute J*v for the 2 translation constraints - const decimal el1 = -n1.dot(v1) - w1.dot(r1PlusUCrossN1) + - n1.dot(v2) + w2.dot(r2CrossN1); - const decimal el2 = -n2.dot(v1) - w1.dot(r1PlusUCrossN2) + - n2.dot(v2) + w2.dot(r2CrossN2); - const Vector2 JvTranslation(el1, el2); - - // Compute the Lagrange multiplier lambda for the 2 translation constraints - const Vector2 deltaLambda = mSliderJointComponents.mInverseMassMatrixTranslation[i] * (-JvTranslation - mSliderJointComponents.mBiasTranslation[i]); - mSliderJointComponents.mImpulseTranslation[i] += deltaLambda; - - // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1 - const Vector3 linearImpulseBody1 = -n1 * deltaLambda.x - n2 * deltaLambda.y; - Vector3 angularImpulseBody1 = -r1PlusUCrossN1 * deltaLambda.x - - r1PlusUCrossN2 * deltaLambda.y; - - // Apply the impulse to the body 1 - v1 += inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; - w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1); - - // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2 - const Vector3 linearImpulseBody2 = -linearImpulseBody1; - Vector3 angularImpulseBody2 = r2CrossN1 * deltaLambda.x + r2CrossN2 * deltaLambda.y; - - // Apply the impulse to the body 2 - v2 += inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * linearImpulseBody2; - w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2); - - // --------------- Rotation Constraints --------------- // - - // Compute J*v for the 3 rotation constraints - const Vector3 JvRotation = w2 - w1; - - // Compute the Lagrange multiplier lambda for the 3 rotation constraints - Vector3 deltaLambda2 = mSliderJointComponents.mInverseMassMatrixRotation[i] * - (-JvRotation - mSliderJointComponents.getBiasRotation(jointEntity)); - mSliderJointComponents.mImpulseRotation[i] += deltaLambda2; - - // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 - angularImpulseBody1 = -deltaLambda2; - - // Apply the impulse to the body 1 - w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mSliderJointComponents.mI1[i] * angularImpulseBody1); - - // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2 - angularImpulseBody2 = deltaLambda2; - - // Apply the impulse to the body 2 - w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mSliderJointComponents.mI2[i] * angularImpulseBody2); - const Vector3& r2CrossSliderAxis = mSliderJointComponents.mR2CrossSliderAxis[i]; const Vector3& r1PlusUCrossSliderAxis = mSliderJointComponents.mR1PlusUCrossSliderAxis[i]; @@ -510,6 +458,58 @@ void SolveSliderJointSystem::solveVelocityConstraint() { // Apply the impulse to the body 2 v2 += inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * linearImpulseBody2; } + + // --------------- Rotation Constraints --------------- // + + // Compute J*v for the 3 rotation constraints + const Vector3 JvRotation = w2 - w1; + + // Compute the Lagrange multiplier lambda for the 3 rotation constraints + Vector3 deltaLambda2 = mSliderJointComponents.mInverseMassMatrixRotation[i] * + (-JvRotation - mSliderJointComponents.getBiasRotation(jointEntity)); + mSliderJointComponents.mImpulseRotation[i] += deltaLambda2; + + // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 + Vector3 angularImpulseBody1 = -deltaLambda2; + + // Apply the impulse to the body 1 + w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mSliderJointComponents.mI1[i] * angularImpulseBody1); + + // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2 + Vector3 angularImpulseBody2 = deltaLambda2; + + // Apply the impulse to the body 2 + w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mSliderJointComponents.mI2[i] * angularImpulseBody2); + + // --------------- Translation Constraints --------------- // + + // Compute J*v for the 2 translation constraints + const decimal el1 = -n1.dot(v1) - w1.dot(r1PlusUCrossN1) + + n1.dot(v2) + w2.dot(r2CrossN1); + const decimal el2 = -n2.dot(v1) - w1.dot(r1PlusUCrossN2) + + n2.dot(v2) + w2.dot(r2CrossN2); + const Vector2 JvTranslation(el1, el2); + + // Compute the Lagrange multiplier lambda for the 2 translation constraints + const Vector2 deltaLambda = mSliderJointComponents.mInverseMassMatrixTranslation[i] * (-JvTranslation - mSliderJointComponents.mBiasTranslation[i]); + mSliderJointComponents.mImpulseTranslation[i] += deltaLambda; + + // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1 + const Vector3 linearImpulseBody1 = -n1 * deltaLambda.x - n2 * deltaLambda.y; + angularImpulseBody1 = -r1PlusUCrossN1 * deltaLambda.x - + r1PlusUCrossN2 * deltaLambda.y; + + // Apply the impulse to the body 1 + v1 += inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; + w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1); + + // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2 + const Vector3 linearImpulseBody2 = -linearImpulseBody1; + angularImpulseBody2 = r2CrossN1 * deltaLambda.x + r2CrossN2 * deltaLambda.y; + + // Apply the impulse to the body 2 + v2 += inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * linearImpulseBody2; + w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2); } } @@ -591,137 +591,6 @@ void SolveSliderJointSystem::solvePositionConstraint() { const Vector3& r1PlusUCrossN1 = mSliderJointComponents.mR1PlusUCrossN1[i]; const Vector3& r1PlusUCrossN2 = mSliderJointComponents.mR1PlusUCrossN2[i]; - // --------------- Translation Constraints --------------- // - - const Matrix3x3& i1 = mSliderJointComponents.getI1(jointEntity); - const Matrix3x3& i2 = mSliderJointComponents.getI2(jointEntity); - - // Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation - // constraints (2x2 matrix) - const decimal body1MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; - const decimal body2MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody2]; - decimal sumInverseMass = body1MassInverse + body2MassInverse; - Vector3 I1R1PlusUCrossN1 = i1 * r1PlusUCrossN1; - Vector3 I1R1PlusUCrossN2 = i1 * r1PlusUCrossN2; - Vector3 I2R2CrossN1 = i2 * r2CrossN1; - Vector3 I2R2CrossN2 = i2 * r2CrossN2; - const decimal el11 = sumInverseMass + r1PlusUCrossN1.dot(I1R1PlusUCrossN1) + - r2CrossN1.dot(I2R2CrossN1); - const decimal el12 = r1PlusUCrossN1.dot(I1R1PlusUCrossN2) + - r2CrossN1.dot(I2R2CrossN2); - const decimal el21 = r1PlusUCrossN2.dot(I1R1PlusUCrossN1) + - r2CrossN2.dot(I2R2CrossN1); - const decimal el22 = sumInverseMass + r1PlusUCrossN2.dot(I1R1PlusUCrossN2) + - r2CrossN2.dot(I2R2CrossN2); - Matrix2x2 matrixKTranslation(el11, el12, el21, el22); - mSliderJointComponents.mInverseMassMatrixTranslation[i].setToZero(); - decimal matrixKTranslationDeterminant = matrixKTranslation.getDeterminant(); - if (std::abs(matrixKTranslationDeterminant) > MACHINE_EPSILON) { - - if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { - - mSliderJointComponents.mInverseMassMatrixTranslation[i] = matrixKTranslation.getInverse(matrixKTranslationDeterminant); - } - - // Compute the position error for the 2 translation constraints - const Vector2 translationError(u.dot(n1), u.dot(n2)); - - // Compute the Lagrange multiplier lambda for the 2 translation constraints - Vector2 lambdaTranslation = mSliderJointComponents.mInverseMassMatrixTranslation[i] * (-translationError); - - // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1 - const Vector3 linearImpulseBody1 = -n1 * lambdaTranslation.x - n2 * lambdaTranslation.y; - Vector3 angularImpulseBody1 = -r1PlusUCrossN1 * lambdaTranslation.x - - r1PlusUCrossN2 * lambdaTranslation.y; - - // Apply the impulse to the body 1 - const Vector3 v1 = inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; - Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1); - - // Update the body position/orientation of body 1 - x1 += v1; - q1 += Quaternion(0, w1) * q1 * decimal(0.5); - q1.normalize(); - - // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2 - const Vector3 linearImpulseBody2 = n1 * lambdaTranslation.x + n2 * lambdaTranslation.y; - Vector3 angularImpulseBody2 = r2CrossN1 * lambdaTranslation.x + r2CrossN2 * lambdaTranslation.y; - - // Apply the impulse to the body 2 - const Vector3 v2 = inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * linearImpulseBody2; - Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2); - - // Update the body position/orientation of body 2 - x2 += v2; - q2 += Quaternion(0, w2) * q2 * decimal(0.5); - q2.normalize(); - } - - // --------------- Rotation Constraints --------------- // - - // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation - // contraints (3x3 matrix) - mSliderJointComponents.mInverseMassMatrixRotation[i] = mSliderJointComponents.mI1[i] + mSliderJointComponents.mI2[i]; - decimal massMatrixRotationDeterminant = mSliderJointComponents.mInverseMassMatrixRotation[i].getDeterminant(); - if (std::abs(massMatrixRotationDeterminant) > MACHINE_EPSILON) { - - if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { - - mSliderJointComponents.mInverseMassMatrixRotation[i] = mSliderJointComponents.mInverseMassMatrixRotation[i].getInverse(massMatrixRotationDeterminant); - } - - // Calculate difference in rotation - // - // The rotation should be: - // - // q2 = q1 r0 - // - // But because of drift the actual rotation is: - // - // q2 = qError q1 r0 - // <=> qError = q2 r0^-1 q1^-1 - // - // Where: - // q1 = current rotation of body 1 - // q2 = current rotation of body 2 - // qError = error that needs to be reduced to zero - Quaternion qError = q2 * mSliderJointComponents.mInitOrientationDifferenceInv[i] * q1.getInverse(); - - // A quaternion can be seen as: - // - // q = [sin(theta / 2) * v, cos(theta/2)] - // - // Where: - // v = rotation vector - // theta = rotation angle - // - // If we assume theta is small (error is small) then sin(x) = x so an approximation of the error angles is: - const Vector3 errorRotation = decimal(2.0) * qError.getVectorV(); - - // Compute the Lagrange multiplier lambda for the 3 rotation constraints - Vector3 lambdaRotation = mSliderJointComponents.mInverseMassMatrixRotation[i] * (-errorRotation); - - // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 - Vector3 angularImpulseBody1 = -lambdaRotation; - - // Apply the impulse to the body 1 - Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mSliderJointComponents.mI1[i] * angularImpulseBody1); - - // Update the body position/orientation of body 1 - q1 += Quaternion(0, w1) * q1 * decimal(0.5); - q1.normalize(); - - // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2 - Vector3 angularImpulseBody2 = lambdaRotation; - - // Apply the impulse to the body 2 - Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mSliderJointComponents.mI2[i] * angularImpulseBody2); - - // Update the body position/orientation of body 2 - q2 += Quaternion(0, w2) * q2 * decimal(0.5); - q2.normalize(); - } - // --------------- Limits Constraints --------------- // if (mSliderJointComponents.mIsLimitEnabled[i]) { @@ -825,5 +694,136 @@ void SolveSliderJointSystem::solvePositionConstraint() { q2.normalize(); } } + + // --------------- Rotation Constraints --------------- // + + // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation + // contraints (3x3 matrix) + mSliderJointComponents.mInverseMassMatrixRotation[i] = mSliderJointComponents.mI1[i] + mSliderJointComponents.mI2[i]; + decimal massMatrixRotationDeterminant = mSliderJointComponents.mInverseMassMatrixRotation[i].getDeterminant(); + if (std::abs(massMatrixRotationDeterminant) > MACHINE_EPSILON) { + + if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { + + mSliderJointComponents.mInverseMassMatrixRotation[i] = mSliderJointComponents.mInverseMassMatrixRotation[i].getInverse(massMatrixRotationDeterminant); + } + + // Calculate difference in rotation + // + // The rotation should be: + // + // q2 = q1 r0 + // + // But because of drift the actual rotation is: + // + // q2 = qError q1 r0 + // <=> qError = q2 r0^-1 q1^-1 + // + // Where: + // q1 = current rotation of body 1 + // q2 = current rotation of body 2 + // qError = error that needs to be reduced to zero + Quaternion qError = q2 * mSliderJointComponents.mInitOrientationDifferenceInv[i] * q1.getInverse(); + + // A quaternion can be seen as: + // + // q = [sin(theta / 2) * v, cos(theta/2)] + // + // Where: + // v = rotation vector + // theta = rotation angle + // + // If we assume theta is small (error is small) then sin(x) = x so an approximation of the error angles is: + const Vector3 errorRotation = decimal(2.0) * qError.getVectorV(); + + // Compute the Lagrange multiplier lambda for the 3 rotation constraints + Vector3 lambdaRotation = mSliderJointComponents.mInverseMassMatrixRotation[i] * (-errorRotation); + + // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 + Vector3 angularImpulseBody1 = -lambdaRotation; + + // Apply the impulse to the body 1 + Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mSliderJointComponents.mI1[i] * angularImpulseBody1); + + // Update the body position/orientation of body 1 + q1 += Quaternion(0, w1) * q1 * decimal(0.5); + q1.normalize(); + + // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2 + Vector3 angularImpulseBody2 = lambdaRotation; + + // Apply the impulse to the body 2 + Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mSliderJointComponents.mI2[i] * angularImpulseBody2); + + // Update the body position/orientation of body 2 + q2 += Quaternion(0, w2) * q2 * decimal(0.5); + q2.normalize(); + } + + // --------------- Translation Constraints --------------- // + + const Matrix3x3& i1 = mSliderJointComponents.getI1(jointEntity); + const Matrix3x3& i2 = mSliderJointComponents.getI2(jointEntity); + + // Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation + // constraints (2x2 matrix) + const decimal body1MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; + const decimal body2MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody2]; + decimal sumInverseMass = body1MassInverse + body2MassInverse; + Vector3 I1R1PlusUCrossN1 = i1 * r1PlusUCrossN1; + Vector3 I1R1PlusUCrossN2 = i1 * r1PlusUCrossN2; + Vector3 I2R2CrossN1 = i2 * r2CrossN1; + Vector3 I2R2CrossN2 = i2 * r2CrossN2; + const decimal el11 = sumInverseMass + r1PlusUCrossN1.dot(I1R1PlusUCrossN1) + + r2CrossN1.dot(I2R2CrossN1); + const decimal el12 = r1PlusUCrossN1.dot(I1R1PlusUCrossN2) + + r2CrossN1.dot(I2R2CrossN2); + const decimal el21 = r1PlusUCrossN2.dot(I1R1PlusUCrossN1) + + r2CrossN2.dot(I2R2CrossN1); + const decimal el22 = sumInverseMass + r1PlusUCrossN2.dot(I1R1PlusUCrossN2) + + r2CrossN2.dot(I2R2CrossN2); + Matrix2x2 matrixKTranslation(el11, el12, el21, el22); + mSliderJointComponents.mInverseMassMatrixTranslation[i].setToZero(); + decimal matrixKTranslationDeterminant = matrixKTranslation.getDeterminant(); + if (std::abs(matrixKTranslationDeterminant) > MACHINE_EPSILON) { + + if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { + + mSliderJointComponents.mInverseMassMatrixTranslation[i] = matrixKTranslation.getInverse(matrixKTranslationDeterminant); + } + + // Compute the position error for the 2 translation constraints + const Vector2 translationError(u.dot(n1), u.dot(n2)); + + // Compute the Lagrange multiplier lambda for the 2 translation constraints + Vector2 lambdaTranslation = mSliderJointComponents.mInverseMassMatrixTranslation[i] * (-translationError); + + // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1 + const Vector3 linearImpulseBody1 = -n1 * lambdaTranslation.x - n2 * lambdaTranslation.y; + Vector3 angularImpulseBody1 = -r1PlusUCrossN1 * lambdaTranslation.x - + r1PlusUCrossN2 * lambdaTranslation.y; + + // Apply the impulse to the body 1 + const Vector3 v1 = inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; + Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1); + + // Update the body position/orientation of body 1 + x1 += v1; + q1 += Quaternion(0, w1) * q1 * decimal(0.5); + q1.normalize(); + + // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2 + const Vector3 linearImpulseBody2 = n1 * lambdaTranslation.x + n2 * lambdaTranslation.y; + Vector3 angularImpulseBody2 = r2CrossN1 * lambdaTranslation.x + r2CrossN2 * lambdaTranslation.y; + + // Apply the impulse to the body 2 + const Vector3 v2 = inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * linearImpulseBody2; + Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2); + + // Update the body position/orientation of body 2 + x2 += v2; + q2 += Quaternion(0, w2) * q2 * decimal(0.5); + q2.normalize(); + } } }