Guard against non-invertible inertia matrix (#144)

Fixes issues/142.
notgiven688 · May 30, 2024 · b10c16e · b10c16e
1 parent bbc6c41
commit b10c16e
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 5 deletions.
diff --git a/src/Jitter2/Dynamics/RigidBody.cs b/src/Jitter2/Dynamics/RigidBody.cs
@@ -528,7 +528,11 @@ public void SetMassInertia()
             mass += shapes[i].Mass;
         }
 
-        JMatrix.Inverse(inertia, out inverseInertia);
+        if (!JMatrix.Inverse(inertia, out inverseInertia))
+        {
+            throw new ArgumentException("Inertia matrix is not invertible.", nameof(inertia));
+        }
+
         this.inverseMass = 1.0f / mass;
 
         UpdateWorldInertia();

diff --git a/src/Jitter2/LinearMath/JMatrix.cs b/src/Jitter2/LinearMath/JMatrix.cs
@@ -333,9 +333,9 @@ public readonly float Determinant()
     [MethodImpl(MethodImplOptions.AggressiveInlining)]
     public static bool Inverse(in JMatrix matrix, out JMatrix result)
     {
-        float det = matrix.Determinant();
+        float idet = 1.0f / matrix.Determinant();
 
-        if (float.IsNaN(det) || float.IsInfinity(det))
+        if (!float.IsNormal(idet))
         {
             result = new JMatrix();
             return false;
@@ -353,8 +353,6 @@ public static bool Inverse(in JMatrix matrix, out JMatrix result)
         float num32 = matrix.M12 * matrix.M31 - matrix.M32 * matrix.M11;
         float num33 = matrix.M11 * matrix.M22 - matrix.M21 * matrix.M12;
 
-        float idet = 1.0f / det;
-
         result.M11 = num11 * idet;
         result.M12 = num12 * idet;
         result.M13 = num13 * idet;