Improve robustness of constraint solver (avoid inverse of matrix with zero determinant)

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]),

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) {

src/mathematics/Matrix2x2.cpp

@@ -40,10 +40,7 @@ Matrix2x2& Matrix2x2::operator=(const Matrix2x2& matrix) {
 }
 
 // Return the inverse matrix
-Matrix2x2 Matrix2x2::getInverse() const {
+Matrix2x2 Matrix2x2::getInverse(decimal determinant) const {
-
-    // Compute the determinant of the matrix
-    decimal determinant = getDeterminant();
 
     // Check if the determinant is equal to zero
     assert(std::abs(determinant) > MACHINE_EPSILON);

src/mathematics/Matrix3x3.cpp

@@ -42,10 +42,7 @@ Matrix3x3& Matrix3x3::operator=(const Matrix3x3& matrix) {
 }
 
 // Return the inverse matrix
-Matrix3x3 Matrix3x3::getInverse() const {
+Matrix3x3 Matrix3x3::getInverse(decimal determinant) const {
-
-    // Compute the determinant of the matrix
-    decimal determinant = getDeterminant();
 
     // Check if the determinant is equal to zero
     assert(determinant != decimal(0.0));

src/systems/SolveBallAndSocketJointSystem.cpp

@@ -97,9 +97,12 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() {
 
         // Compute the inverse mass matrix K^-1
         mBallAndSocketJointComponents.mInverseMassMatrix[i].setToZero();
+        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();
+                mBallAndSocketJointComponents.mInverseMassMatrix[i] = massMatrix.getInverse(massMatrixDeterminant);
+            }
         }
 
         const Vector3& x1 = mRigidBodyComponents.mCentersOfMassWorld[componentIndexBody1];
@@ -269,9 +272,12 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() {
                                skewSymmetricMatrixU1 * mBallAndSocketJointComponents.mI1[i] * skewSymmetricMatrixU1.getTranspose() +
                                skewSymmetricMatrixU2 * mBallAndSocketJointComponents.mI2[i] * skewSymmetricMatrixU2.getTranspose();
         mBallAndSocketJointComponents.mInverseMassMatrix[i].setToZero();
+        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();
+                mBallAndSocketJointComponents.mInverseMassMatrix[i] = massMatrix.getInverse(massMatrixDeterminant);
             }
 
             Vector3& x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1];
@@ -310,4 +316,5 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() {
             q2 += Quaternion(0, w2) * q2 * decimal(0.5);
             q2.normalize();
         }
+    }
 }

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();
+        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();
+                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];
+        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();
+                mFixedJointComponents.mInverseMassMatrixRotation[i] = mFixedJointComponents.mInverseMassMatrixRotation[i].getInverse(massMatrixRotationDeterminant);
+            }
         }
 
         // Compute the bias "b" for the 3 rotation constraints
@@ -336,9 +342,12 @@ void SolveFixedJointSystem::solvePositionConstraint() {
                                skewSymmetricMatrixU1 * mFixedJointComponents.mI1[i] * skewSymmetricMatrixU1.getTranspose() +
                                skewSymmetricMatrixU2 * mFixedJointComponents.mI2[i] * skewSymmetricMatrixU2.getTranspose();
         mFixedJointComponents.mInverseMassMatrixTranslation[i].setToZero();
+        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();
+                mFixedJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse(massMatrixDeterminant);
             }
 
             Vector3& x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1];
@@ -373,15 +382,19 @@ void SolveFixedJointSystem::solvePositionConstraint() {
             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];
+        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();
+                mFixedJointComponents.mInverseMassMatrixRotation[i] = mFixedJointComponents.mInverseMassMatrixRotation[i].getInverse(massMatrixRotationDeterminant);
             }
 
             // Calculate difference in rotation
@@ -416,20 +429,21 @@ void SolveFixedJointSystem::solvePositionConstraint() {
             Vector3 lambdaRotation = mFixedJointComponents.mInverseMassMatrixRotation[i] * (-errorRotation);
 
             // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
-        angularImpulseBody1 = -lambdaRotation;
+            Vector3 angularImpulseBody1 = -lambdaRotation;
 
             // Compute the pseudo velocity of body 1
-        w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mFixedJointComponents.mI1[i] * angularImpulseBody1);
+            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
-        w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mFixedJointComponents.mI2[i] * lambdaRotation);
+            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();
         }
+    }
 }

src/systems/SolveHingeJointSystem.cpp

@@ -110,9 +110,12 @@ void SolveHingeJointSystem::initBeforeSolve() {
                                skewSymmetricMatrixU2 * mHingeJointComponents.mI2[i] * skewSymmetricMatrixU2.getTranspose();
         Matrix3x3& inverseMassMatrixTranslation = mHingeJointComponents.mInverseMassMatrixTranslation[i];
         inverseMassMatrixTranslation.setToZero();
+        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();
+                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();
+        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();
+                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,6 +524,25 @@ 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];
 
@@ -570,6 +580,7 @@ void SolveHingeJointSystem::solvePositionConstraint() {
             x2 += v2;
             q2 += Quaternion(0, w2) * q2 * decimal(0.5);
             q2.normalize();
+        }
 
         // --------------- Rotation Constraints --------------- //
 
@@ -588,9 +599,11 @@ void SolveHingeJointSystem::solvePositionConstraint() {
                              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();
+                mHingeJointComponents.mInverseMassMatrixRotation[i] = matrixKRotation.getInverse(matrixDeterminant);
             }
 
             // Compute the position error for the 3 rotation constraints
@@ -600,25 +613,27 @@ void SolveHingeJointSystem::solvePositionConstraint() {
             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;
+            Vector3 angularImpulseBody1 = -b2CrossA1 * lambdaRotation.x - c2CrossA1 * lambdaRotation.y;
 
             // Compute the pseudo velocity of body 1
-        w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mHingeJointComponents.mI1[i] * angularImpulseBody1);
+            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
-        angularImpulseBody2 = b2CrossA1 * lambdaRotation.x + c2CrossA1 * lambdaRotation.y;
+            Vector3 angularImpulseBody2 = b2CrossA1 * lambdaRotation.x + c2CrossA1 * lambdaRotation.y;
 
             // Compute the pseudo velocity of body 2
-        w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mHingeJointComponents.mI2[i] * angularImpulseBody2);
+            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);
 

src/systems/SolveSliderJointSystem.cpp

@@ -185,19 +185,25 @@ void SolveSliderJointSystem::initBeforeSolve() {
                              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();
+                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;
+        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,9 +615,12 @@ void SolveSliderJointSystem::solvePositionConstraint() {
                              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();
+                mSliderJointComponents.mInverseMassMatrixTranslation[i] = matrixKTranslation.getInverse(matrixKTranslationDeterminant);
             }
 
             // Compute the position error for the 2 translation constraints
@@ -646,15 +655,19 @@ void SolveSliderJointSystem::solvePositionConstraint() {
             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();
+                mSliderJointComponents.mInverseMassMatrixRotation[i] = mSliderJointComponents.mInverseMassMatrixRotation[i].getInverse(massMatrixRotationDeterminant);
             }
 
             // Calculate difference in rotation
@@ -689,24 +702,25 @@ void SolveSliderJointSystem::solvePositionConstraint() {
             Vector3 lambdaRotation = mSliderJointComponents.mInverseMassMatrixRotation[i] * (-errorRotation);
 
             // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
-        angularImpulseBody1 = -lambdaRotation;
+            Vector3 angularImpulseBody1 = -lambdaRotation;
 
             // Apply the impulse to the body 1
-        w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mSliderJointComponents.mI1[i] * angularImpulseBody1);
+            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
-        angularImpulseBody2 = lambdaRotation;
+            Vector3 angularImpulseBody2 = lambdaRotation;
 
             // Apply the impulse to the body 2
-        w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mSliderJointComponents.mI2[i] * angularImpulseBody2);
+            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 --------------- //