From 2e1046f2b830443d1b415ef610ef419cfe38c2d8 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Fri, 29 Jan 2021 22:08:39 +0100 Subject: [PATCH] Improve robustness of constraint solver (avoid inverse of matrix with zero determinant) --- .../reactphysics3d/mathematics/Matrix2x2.h | 9 + .../reactphysics3d/mathematics/Matrix3x3.h | 9 + src/mathematics/Matrix2x2.cpp | 5 +- src/mathematics/Matrix3x3.cpp | 5 +- src/systems/SolveBallAndSocketJointSystem.cpp | 91 ++++---- src/systems/SolveFixedJointSystem.cpp | 200 +++++++++-------- src/systems/SolveHingeJointSystem.cpp | 171 ++++++++------- src/systems/SolveSliderJointSystem.cpp | 202 ++++++++++-------- 8 files changed, 377 insertions(+), 315 deletions(-) diff --git a/include/reactphysics3d/mathematics/Matrix2x2.h b/include/reactphysics3d/mathematics/Matrix2x2.h index 66c651f6..165e972c 100644 --- a/include/reactphysics3d/mathematics/Matrix2x2.h +++ b/include/reactphysics3d/mathematics/Matrix2x2.h @@ -92,6 +92,9 @@ class Matrix2x2 { /// Return the inverse matrix Matrix2x2 getInverse() const; + /// Return the inverse matrix + Matrix2x2 getInverse(decimal determinant) const; + /// Return the matrix with absolute values Matrix2x2 getAbsoluteMatrix() const; @@ -240,6 +243,12 @@ RP3D_FORCE_INLINE Matrix2x2 Matrix2x2::zero() { return Matrix2x2(0.0, 0.0, 0.0, 0.0); } +// Return the inverse matrix +RP3D_FORCE_INLINE Matrix2x2 Matrix2x2::getInverse() const { + + return getInverse(getDeterminant()); +} + // Return the matrix with absolute values RP3D_FORCE_INLINE Matrix2x2 Matrix2x2::getAbsoluteMatrix() const { return Matrix2x2(std::abs(mRows[0][0]), std::abs(mRows[0][1]), diff --git a/include/reactphysics3d/mathematics/Matrix3x3.h b/include/reactphysics3d/mathematics/Matrix3x3.h index 0a2661a0..7ccfb8ec 100644 --- a/include/reactphysics3d/mathematics/Matrix3x3.h +++ b/include/reactphysics3d/mathematics/Matrix3x3.h @@ -95,6 +95,9 @@ class Matrix3x3 { /// Return the inverse matrix Matrix3x3 getInverse() const; + /// Return the inverse matrix + Matrix3x3 getInverse(decimal determinant) const; + /// Return the matrix with absolute values Matrix3x3 getAbsoluteMatrix() const; @@ -253,6 +256,12 @@ RP3D_FORCE_INLINE Matrix3x3 Matrix3x3::zero() { return Matrix3x3(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); } +// Return the inverse matrix +RP3D_FORCE_INLINE Matrix3x3 Matrix3x3::getInverse() const { + + return getInverse(getDeterminant()); +} + // Return a skew-symmetric matrix using a given vector that can be used // to compute cross product with another vector using matrix multiplication RP3D_FORCE_INLINE Matrix3x3 Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(const Vector3& vector) { diff --git a/src/mathematics/Matrix2x2.cpp b/src/mathematics/Matrix2x2.cpp index a6ca1c13..36b927b4 100644 --- a/src/mathematics/Matrix2x2.cpp +++ b/src/mathematics/Matrix2x2.cpp @@ -40,10 +40,7 @@ Matrix2x2& Matrix2x2::operator=(const Matrix2x2& matrix) { } // Return the inverse matrix -Matrix2x2 Matrix2x2::getInverse() const { - - // Compute the determinant of the matrix - decimal determinant = getDeterminant(); +Matrix2x2 Matrix2x2::getInverse(decimal determinant) const { // Check if the determinant is equal to zero assert(std::abs(determinant) > MACHINE_EPSILON); diff --git a/src/mathematics/Matrix3x3.cpp b/src/mathematics/Matrix3x3.cpp index 18f96a1b..47805eae 100644 --- a/src/mathematics/Matrix3x3.cpp +++ b/src/mathematics/Matrix3x3.cpp @@ -42,10 +42,7 @@ Matrix3x3& Matrix3x3::operator=(const Matrix3x3& matrix) { } // Return the inverse matrix -Matrix3x3 Matrix3x3::getInverse() const { - - // Compute the determinant of the matrix - decimal determinant = getDeterminant(); +Matrix3x3 Matrix3x3::getInverse(decimal determinant) const { // Check if the determinant is equal to zero assert(determinant != decimal(0.0)); diff --git a/src/systems/SolveBallAndSocketJointSystem.cpp b/src/systems/SolveBallAndSocketJointSystem.cpp index 599b8f83..a5afc20b 100644 --- a/src/systems/SolveBallAndSocketJointSystem.cpp +++ b/src/systems/SolveBallAndSocketJointSystem.cpp @@ -97,9 +97,12 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() { // Compute the inverse mass matrix K^-1 mBallAndSocketJointComponents.mInverseMassMatrix[i].setToZero(); - if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || - mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { - mBallAndSocketJointComponents.mInverseMassMatrix[i] = massMatrix.getInverse(); + decimal massMatrixDeterminant = massMatrix.getDeterminant(); + if (std::abs(massMatrixDeterminant) > MACHINE_EPSILON) { + if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || + mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { + mBallAndSocketJointComponents.mInverseMassMatrix[i] = massMatrix.getInverse(massMatrixDeterminant); + } } const Vector3& x1 = mRigidBodyComponents.mCentersOfMassWorld[componentIndexBody1]; @@ -269,45 +272,49 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() { skewSymmetricMatrixU1 * mBallAndSocketJointComponents.mI1[i] * skewSymmetricMatrixU1.getTranspose() + skewSymmetricMatrixU2 * mBallAndSocketJointComponents.mI2[i] * skewSymmetricMatrixU2.getTranspose(); mBallAndSocketJointComponents.mInverseMassMatrix[i].setToZero(); - if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || - mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { - mBallAndSocketJointComponents.mInverseMassMatrix[i] = massMatrix.getInverse(); + decimal massMatrixDeterminant = massMatrix.getDeterminant(); + if (std::abs(massMatrixDeterminant) > MACHINE_EPSILON) { + + if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || + mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { + mBallAndSocketJointComponents.mInverseMassMatrix[i] = massMatrix.getInverse(massMatrixDeterminant); + } + + Vector3& x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1]; + Vector3& x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2]; + + // Compute the constraint error (value of the C(x) function) + const Vector3 constraintError = (x2 + r2World - x1 - r1World); + + // Compute the Lagrange multiplier lambda + // TODO : Do not solve the system by computing the inverse each time and multiplying with the + // right-hand side vector but instead use a method to directly solve the linear system. + const Vector3 lambda = mBallAndSocketJointComponents.mInverseMassMatrix[i] * (-constraintError); + + // Compute the impulse of body 1 + const Vector3 linearImpulseBody1 = -lambda; + const Vector3 angularImpulseBody1 = lambda.cross(r1World); + + // Compute the pseudo velocity of body 1 + const Vector3 v1 = inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; + const Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mBallAndSocketJointComponents.mI1[i] * angularImpulseBody1); + + // Update the body center of mass and orientation of body 1 + x1 += v1; + q1 += Quaternion(0, w1) * q1 * decimal(0.5); + q1.normalize(); + + // Compute the impulse of body 2 + const Vector3 angularImpulseBody2 = -lambda.cross(r2World); + + // Compute the pseudo velocity of body 2 + const Vector3 v2 = inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * lambda; + const Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mBallAndSocketJointComponents.mI2[i] * angularImpulseBody2); + + // Update the body position/orientation of body 2 + x2 += v2; + q2 += Quaternion(0, w2) * q2 * decimal(0.5); + q2.normalize(); } - - Vector3& x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1]; - Vector3& x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2]; - - // Compute the constraint error (value of the C(x) function) - const Vector3 constraintError = (x2 + r2World - x1 - r1World); - - // Compute the Lagrange multiplier lambda - // TODO : Do not solve the system by computing the inverse each time and multiplying with the - // right-hand side vector but instead use a method to directly solve the linear system. - const Vector3 lambda = mBallAndSocketJointComponents.mInverseMassMatrix[i] * (-constraintError); - - // Compute the impulse of body 1 - const Vector3 linearImpulseBody1 = -lambda; - const Vector3 angularImpulseBody1 = lambda.cross(r1World); - - // Compute the pseudo velocity of body 1 - const Vector3 v1 = inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; - const Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mBallAndSocketJointComponents.mI1[i] * angularImpulseBody1); - - // Update the body center of mass and orientation of body 1 - x1 += v1; - q1 += Quaternion(0, w1) * q1 * decimal(0.5); - q1.normalize(); - - // Compute the impulse of body 2 - const Vector3 angularImpulseBody2 = -lambda.cross(r2World); - - // Compute the pseudo velocity of body 2 - const Vector3 v2 = inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * lambda; - const Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mBallAndSocketJointComponents.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/SolveFixedJointSystem.cpp b/src/systems/SolveFixedJointSystem.cpp index a36902d2..9ce77d30 100644 --- a/src/systems/SolveFixedJointSystem.cpp +++ b/src/systems/SolveFixedJointSystem.cpp @@ -93,9 +93,12 @@ void SolveFixedJointSystem::initBeforeSolve() { // Compute the inverse mass matrix K^-1 for the 3 translation constraints mFixedJointComponents.mInverseMassMatrixTranslation[i].setToZero(); - if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || - mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { - mFixedJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse(); + decimal massMatrixDeterminant = massMatrix.getDeterminant(); + if (std::abs(massMatrixDeterminant) > MACHINE_EPSILON) { + if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || + mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { + mFixedJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse(massMatrixDeterminant); + } } // Get the bodies positions and orientations @@ -113,9 +116,12 @@ void SolveFixedJointSystem::initBeforeSolve() { // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation contraints (3x3 matrix) mFixedJointComponents.mInverseMassMatrixRotation[i] = mFixedJointComponents.mI1[i] + mFixedJointComponents.mI2[i]; - if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || - mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { - mFixedJointComponents.mInverseMassMatrixRotation[i] = mFixedJointComponents.mInverseMassMatrixRotation[i].getInverse(); + decimal massMatrixRotationDeterminant = mFixedJointComponents.mInverseMassMatrixRotation[i].getDeterminant(); + if (std::abs(massMatrixRotationDeterminant) > MACHINE_EPSILON) { + if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || + mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { + mFixedJointComponents.mInverseMassMatrixRotation[i] = mFixedJointComponents.mInverseMassMatrixRotation[i].getInverse(massMatrixRotationDeterminant); + } } // Compute the bias "b" for the 3 rotation constraints @@ -336,100 +342,108 @@ void SolveFixedJointSystem::solvePositionConstraint() { skewSymmetricMatrixU1 * mFixedJointComponents.mI1[i] * skewSymmetricMatrixU1.getTranspose() + skewSymmetricMatrixU2 * mFixedJointComponents.mI2[i] * skewSymmetricMatrixU2.getTranspose(); mFixedJointComponents.mInverseMassMatrixTranslation[i].setToZero(); - if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || - mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { - mFixedJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse(); + decimal massMatrixDeterminant = massMatrix.getDeterminant(); + if (std::abs(massMatrixDeterminant) > MACHINE_EPSILON) { + + if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || + mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { + mFixedJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse(massMatrixDeterminant); + } + + Vector3& x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1]; + Vector3& x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2]; + // Compute position error for the 3 translation constraints + const Vector3 errorTranslation = x2 + r2World - x1 - r1World; + + // Compute the Lagrange multiplier lambda + const Vector3 lambdaTranslation = mFixedJointComponents.mInverseMassMatrixTranslation[i] * (-errorTranslation); + + // Compute the impulse of body 1 + Vector3 linearImpulseBody1 = -lambdaTranslation; + Vector3 angularImpulseBody1 = lambdaTranslation.cross(r1World); + + // Compute the pseudo velocity of body 1 + const Vector3 v1 = inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; + Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mFixedJointComponents.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(r2World); + + // Compute the pseudo velocity of body 2 + const Vector3 v2 = inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * lambdaTranslation; + Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mFixedJointComponents.mI2[i] * angularImpulseBody2); + + // Update the body position/orientation of body 2 + x2 += v2; + q2 += Quaternion(0, w2) * q2 * decimal(0.5); + q2.normalize(); } - Vector3& x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1]; - Vector3& x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2]; - // Compute position error for the 3 translation constraints - const Vector3 errorTranslation = x2 + r2World - x1 - r1World; - - // Compute the Lagrange multiplier lambda - const Vector3 lambdaTranslation = mFixedJointComponents.mInverseMassMatrixTranslation[i] * (-errorTranslation); - - // Compute the impulse of body 1 - Vector3 linearImpulseBody1 = -lambdaTranslation; - Vector3 angularImpulseBody1 = lambdaTranslation.cross(r1World); - - // Compute the pseudo velocity of body 1 - const Vector3 v1 = inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; - Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mFixedJointComponents.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(r2World); - - // Compute the pseudo velocity of body 2 - const Vector3 v2 = inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * lambdaTranslation; - Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mFixedJointComponents.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 of the mass matrix K=JM^-1J^t for the 3 rotation // contraints (3x3 matrix) mFixedJointComponents.mInverseMassMatrixRotation[i] = mFixedJointComponents.mI1[i] + mFixedJointComponents.mI2[i]; - if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || - mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { - mFixedJointComponents.mInverseMassMatrixRotation[i] = mFixedJointComponents.mInverseMassMatrixRotation[i].getInverse(); + decimal massMatrixRotationDeterminant = mFixedJointComponents.mInverseMassMatrixRotation[i].getDeterminant(); + if (std::abs(massMatrixRotationDeterminant) > MACHINE_EPSILON) { + + if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || + mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { + mFixedJointComponents.mInverseMassMatrixRotation[i] = mFixedJointComponents.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 * mFixedJointComponents.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 = mFixedJointComponents.mInverseMassMatrixRotation[i] * (-errorRotation); + + // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 + Vector3 angularImpulseBody1 = -lambdaRotation; + + // Compute the pseudo velocity of body 1 + Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mFixedJointComponents.mI1[i] * angularImpulseBody1); + + // Update the body position/orientation of body 1 + q1 += Quaternion(0, w1) * q1 * decimal(0.5); + q1.normalize(); + + // Compute the pseudo velocity of body 2 + Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mFixedJointComponents.mI2[i] * lambdaRotation); + + // Update the body position/orientation of body 2 + q2 += Quaternion(0, w2) * q2 * decimal(0.5); + q2.normalize(); } - - // 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 * mFixedJointComponents.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 = mFixedJointComponents.mInverseMassMatrixRotation[i] * (-errorRotation); - - // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 - angularImpulseBody1 = -lambdaRotation; - - // Compute the pseudo velocity of body 1 - w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mFixedJointComponents.mI1[i] * angularImpulseBody1); - - // Update the body position/orientation of body 1 - q1 += Quaternion(0, w1) * q1 * decimal(0.5); - q1.normalize(); - - // Compute the pseudo velocity of body 2 - w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mFixedJointComponents.mI2[i] * lambdaRotation); - - // 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 c2a859cb..e547ce14 100644 --- a/src/systems/SolveHingeJointSystem.cpp +++ b/src/systems/SolveHingeJointSystem.cpp @@ -110,9 +110,12 @@ void SolveHingeJointSystem::initBeforeSolve() { skewSymmetricMatrixU2 * mHingeJointComponents.mI2[i] * skewSymmetricMatrixU2.getTranspose(); Matrix3x3& inverseMassMatrixTranslation = mHingeJointComponents.mInverseMassMatrixTranslation[i]; inverseMassMatrixTranslation.setToZero(); - if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || - mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { - mHingeJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse(); + decimal massMatrixDeterminant = massMatrix.getDeterminant(); + if (std::abs(massMatrixDeterminant) > MACHINE_EPSILON) { + if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || + mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { + mHingeJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse(massMatrixDeterminant); + } } // Get the bodies positions and orientations @@ -141,9 +144,12 @@ void SolveHingeJointSystem::initBeforeSolve() { const decimal el22 = c2CrossA1.dot(i1C2CrossA1) + c2CrossA1.dot(i2C2CrossA1); const Matrix2x2 matrixKRotation(el11, el12, el21, el22); mHingeJointComponents.mInverseMassMatrixRotation[i].setToZero(); - if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || - mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { - mHingeJointComponents.mInverseMassMatrixRotation[i] = matrixKRotation.getInverse(); + decimal matrixKRotationDeterminant = matrixKRotation.getDeterminant(); + if (std::abs(matrixKRotationDeterminant) > MACHINE_EPSILON) { + if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || + mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { + mHingeJointComponents.mInverseMassMatrixRotation[i] = matrixKRotation.getInverse(matrixKRotationDeterminant); + } } // If warm-starting is not enabled @@ -500,21 +506,6 @@ void SolveHingeJointSystem::solvePositionConstraint() { // --------------- 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(); - if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || - mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { - mHingeJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse(); - } - Vector3& b2CrossA1 = mHingeJointComponents.mB2CrossA1[i]; Vector3& c2CrossA1 = mHingeJointComponents.mC2CrossA1[i]; @@ -533,43 +524,63 @@ void SolveHingeJointSystem::solvePositionConstraint() { c2CrossA1 = c2.cross(a1); mHingeJointComponents.mC2CrossA1[i] = c2CrossA1; - Vector3& x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1]; - Vector3& x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2]; + // 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) { - // Compute position error for the 3 translation constraints - const Vector3 errorTranslation = x2 + mHingeJointComponents.mR2World[i] - x1 - mHingeJointComponents.mR1World[i]; + if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || + mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { + mHingeJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse(matrixDeterminant); + } - // 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]); + Vector3& x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1]; + Vector3& x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2]; - // Get the inverse mass and inverse inertia tensors of the bodies - decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; - decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2]; + // Compute position error for the 3 translation constraints + const Vector3 errorTranslation = x2 + mHingeJointComponents.mR2World[i] - x1 - mHingeJointComponents.mR1World[i]; - // Compute the pseudo velocity of body 1 - const Vector3 v1 = inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; - Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mHingeJointComponents.mI1[i] * angularImpulseBody1); + // Compute the Lagrange multiplier lambda + const Vector3 lambdaTranslation = mHingeJointComponents.mInverseMassMatrixTranslation[i] * (-errorTranslation); - // 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 1 + Vector3 linearImpulseBody1 = -lambdaTranslation; + Vector3 angularImpulseBody1 = lambdaTranslation.cross(mHingeJointComponents.mR1World[i]); - // Compute the impulse of body 2 - Vector3 angularImpulseBody2 = -lambdaTranslation.cross(mHingeJointComponents.mR2World[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 2 - const Vector3 v2 = inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * lambdaTranslation; - Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mHingeJointComponents.mI2[i] * angularImpulseBody2); + // 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 2 - x2 += v2; - q2 += Quaternion(0, w2) * q2 * decimal(0.5); - q2.normalize(); + // 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 --------------- // @@ -588,37 +599,41 @@ void SolveHingeJointSystem::solvePositionConstraint() { c2CrossA1.dot(I2C2CrossA1); const Matrix2x2 matrixKRotation(el11, el12, el21, el22); mHingeJointComponents.mInverseMassMatrixRotation[i].setToZero(); - if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || - mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { - mHingeJointComponents.mInverseMassMatrixRotation[i] = matrixKRotation.getInverse(); + 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 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 - angularImpulseBody1 = -b2CrossA1 * lambdaRotation.x - c2CrossA1 * lambdaRotation.y; - - // Compute the pseudo velocity of body 1 - 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 - angularImpulseBody2 = b2CrossA1 * lambdaRotation.x + c2CrossA1 * lambdaRotation.y; - - // Compute the pseudo velocity of body 2 - 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); diff --git a/src/systems/SolveSliderJointSystem.cpp b/src/systems/SolveSliderJointSystem.cpp index 9eab7e03..e2726174 100644 --- a/src/systems/SolveSliderJointSystem.cpp +++ b/src/systems/SolveSliderJointSystem.cpp @@ -185,19 +185,25 @@ void SolveSliderJointSystem::initBeforeSolve() { r2CrossN2.dot(I2R2CrossN2); Matrix2x2 matrixKTranslation(el11, el12, el21, el22); mSliderJointComponents.mInverseMassMatrixTranslation[i].setToZero(); - if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || - mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { + 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(); + mSliderJointComponents.mInverseMassMatrixTranslation[i] = matrixKTranslation.getInverse(matrixKTranslationDeterminant); + } } // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation // contraints (3x3 matrix) mSliderJointComponents.mInverseMassMatrixRotation[i] = i1 + i2; - if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || - mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { + 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(); + mSliderJointComponents.mInverseMassMatrixRotation[i] = mSliderJointComponents.mInverseMassMatrixRotation[i].getInverse(massMatrixRotationDeterminant); + } } // Compute the bias "b" of the rotation constraint @@ -609,105 +615,113 @@ void SolveSliderJointSystem::solvePositionConstraint() { r2CrossN2.dot(I2R2CrossN2); Matrix2x2 matrixKTranslation(el11, el12, el21, el22); mSliderJointComponents.mInverseMassMatrixTranslation[i].setToZero(); - if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { + decimal matrixKTranslationDeterminant = matrixKTranslation.getDeterminant(); + if (std::abs(matrixKTranslationDeterminant) > MACHINE_EPSILON) { - mSliderJointComponents.mInverseMassMatrixTranslation[i] = matrixKTranslation.getInverse(); + 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(); } - // 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]; - if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { + decimal massMatrixRotationDeterminant = mSliderJointComponents.mInverseMassMatrixRotation[i].getDeterminant(); + if (std::abs(massMatrixRotationDeterminant) > MACHINE_EPSILON) { - mSliderJointComponents.mInverseMassMatrixRotation[i] = mSliderJointComponents.mInverseMassMatrixRotation[i].getInverse(); + 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(); } - // 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 - angularImpulseBody1 = -lambdaRotation; - - // Apply the impulse to the body 1 - 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 - angularImpulseBody2 = lambdaRotation; - - // Apply the impulse to the body 2 - 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]) {