Merge pull request #1384 from jnyfah/dev

Inverting a 4x4 matrix with `try_inverse_mut` doesn't leave `self` unchanged if the inversion fails.
dimforge · May 5, 2024 · afc03cc · afc03cc
2 parents 9bc7f8b + 343eb21
commit afc03cc
Show file tree

Hide file tree

Showing 2 changed files with 50 additions and 33 deletions.
diff --git a/src/linalg/inverse.rs b/src/linalg/inverse.rs
@@ -145,13 +145,44 @@ where
 {
     let m = m.as_slice();
 
-    out[(0, 0)] = m[5].clone() * m[10].clone() * m[15].clone()
+    let cofactor00 = m[5].clone() * m[10].clone() * m[15].clone()
         - m[5].clone() * m[11].clone() * m[14].clone()
         - m[9].clone() * m[6].clone() * m[15].clone()
         + m[9].clone() * m[7].clone() * m[14].clone()
         + m[13].clone() * m[6].clone() * m[11].clone()
         - m[13].clone() * m[7].clone() * m[10].clone();
 
+    let cofactor01 = -m[4].clone() * m[10].clone() * m[15].clone()
+        + m[4].clone() * m[11].clone() * m[14].clone()
+        + m[8].clone() * m[6].clone() * m[15].clone()
+        - m[8].clone() * m[7].clone() * m[14].clone()
+        - m[12].clone() * m[6].clone() * m[11].clone()
+        + m[12].clone() * m[7].clone() * m[10].clone();
+
+    let cofactor02 = m[4].clone() * m[9].clone() * m[15].clone()
+        - m[4].clone() * m[11].clone() * m[13].clone()
+        - m[8].clone() * m[5].clone() * m[15].clone()
+        + m[8].clone() * m[7].clone() * m[13].clone()
+        + m[12].clone() * m[5].clone() * m[11].clone()
+        - m[12].clone() * m[7].clone() * m[9].clone();
+
+    let cofactor03 = -m[4].clone() * m[9].clone() * m[14].clone()
+        + m[4].clone() * m[10].clone() * m[13].clone()
+        + m[8].clone() * m[5].clone() * m[14].clone()
+        - m[8].clone() * m[6].clone() * m[13].clone()
+        - m[12].clone() * m[5].clone() * m[10].clone()
+        + m[12].clone() * m[6].clone() * m[9].clone();
+
+    let det = m[0].clone() * cofactor00.clone()
+        + m[1].clone() * cofactor01.clone()
+        + m[2].clone() * cofactor02.clone()
+        + m[3].clone() * cofactor03.clone();
+
+    if det.is_zero() {
+        return false;
+    }
+    out[(0, 0)] = cofactor00;
+
     out[(1, 0)] = -m[1].clone() * m[10].clone() * m[15].clone()
         + m[1].clone() * m[11].clone() * m[14].clone()
         + m[9].clone() * m[2].clone() * m[15].clone()
@@ -173,12 +204,7 @@ where
         - m[9].clone() * m[2].clone() * m[7].clone()
         + m[9].clone() * m[3].clone() * m[6].clone();
 
-    out[(0, 1)] = -m[4].clone() * m[10].clone() * m[15].clone()
-        + m[4].clone() * m[11].clone() * m[14].clone()
-        + m[8].clone() * m[6].clone() * m[15].clone()
-        - m[8].clone() * m[7].clone() * m[14].clone()
-        - m[12].clone() * m[6].clone() * m[11].clone()
-        + m[12].clone() * m[7].clone() * m[10].clone();
+    out[(0, 1)] = cofactor01;
 
     out[(1, 1)] = m[0].clone() * m[10].clone() * m[15].clone()
         - m[0].clone() * m[11].clone() * m[14].clone()
@@ -201,12 +227,7 @@ where
         + m[8].clone() * m[2].clone() * m[7].clone()
         - m[8].clone() * m[3].clone() * m[6].clone();
 
-    out[(0, 2)] = m[4].clone() * m[9].clone() * m[15].clone()
-        - m[4].clone() * m[11].clone() * m[13].clone()
-        - m[8].clone() * m[5].clone() * m[15].clone()
-        + m[8].clone() * m[7].clone() * m[13].clone()
-        + m[12].clone() * m[5].clone() * m[11].clone()
-        - m[12].clone() * m[7].clone() * m[9].clone();
+    out[(0, 2)] = cofactor02;
 
     out[(1, 2)] = -m[0].clone() * m[9].clone() * m[15].clone()
         + m[0].clone() * m[11].clone() * m[13].clone()
@@ -222,12 +243,7 @@ where
         + m[12].clone() * m[1].clone() * m[7].clone()
         - m[12].clone() * m[3].clone() * m[5].clone();
 
-    out[(0, 3)] = -m[4].clone() * m[9].clone() * m[14].clone()
-        + m[4].clone() * m[10].clone() * m[13].clone()
-        + m[8].clone() * m[5].clone() * m[14].clone()
-        - m[8].clone() * m[6].clone() * m[13].clone()
-        - m[12].clone() * m[5].clone() * m[10].clone()
-        + m[12].clone() * m[6].clone() * m[9].clone();
+    out[(0, 3)] = cofactor03;
 
     out[(3, 2)] = -m[0].clone() * m[5].clone() * m[11].clone()
         + m[0].clone() * m[7].clone() * m[9].clone()
@@ -257,21 +273,12 @@ where
         + m[8].clone() * m[1].clone() * m[6].clone()
         - m[8].clone() * m[2].clone() * m[5].clone();
 
-    let det = m[0].clone() * out[(0, 0)].clone()
-        + m[1].clone() * out[(0, 1)].clone()
-        + m[2].clone() * out[(0, 2)].clone()
-        + m[3].clone() * out[(0, 3)].clone();
+    let inv_det = T::one() / det;
 
-    if !det.is_zero() {
-        let inv_det = T::one() / det;
-
-        for j in 0..4 {
-            for i in 0..4 {
-                out[(i, j)] *= inv_det.clone();
-            }
+    for j in 0..4 {
+        for i in 0..4 {
+            out[(i, j)] *= inv_det.clone();
         }
-        true
-    } else {
-        false
     }
+    true
 }
diff --git a/tests/core/matrix.rs b/tests/core/matrix.rs
@@ -1263,6 +1263,16 @@ fn column_iterator_double_ended_mut() {
     assert_eq!(col_iter_mut.next(), None);
 }
 
+#[test]
+fn test_inversion_failure_leaves_matrix4_unchanged() {
+    let mut mat = na::Matrix4::new(
+        1.0, 2.0, 3.0, 4.0, 2.0, 4.0, 6.0, 8.0, 3.0, 6.0, 9.0, 12.0, 4.0, 8.0, 12.0, 16.0,
+    );
+    let expected = mat.clone();
+    assert!(!mat.try_inverse_mut());
+    assert_eq!(mat, expected);
+}
+
 #[test]
 #[cfg(feature = "rayon")]
 fn parallel_column_iteration() {