fix the 4x4 inverse -- unit test passes again

1 files changed, 15 insertions, 5 deletions

diff --git a/Eigen/src/LU/Inverse.h b/Eigen/src/LU/Inverse.h
index eff0047d0..3d4d63489 100644
--- a/Eigen/src/LU/Inverse.h
+++ b/Eigen/src/LU/Inverse.h
@@ -132,21 +132,31 @@ void ei_compute_inverse_in_size4_case(const MatrixType& matrix, MatrixType* resu
     // since this is a rare case, we don't need to optimize it. We just want to handle it with little
     // additional code.
     MatrixType m(matrix);
-    m.row(1).swap(m.row(2));
+    m.row(0).swap(m.row(2));
+    m.row(1).swap(m.row(3));
     if(ei_compute_inverse_in_size4_case_helper(m, result))
     {
-      // good, the topleft 2x2 block of m is invertible. Since m is different from matrix in that two
+      // good, the topleft 2x2 block of m is invertible. Since m is different from matrix in that some
       // rows were permuted, the actual inverse of matrix is derived from the inverse of m by permuting
       // the corresponding columns.
-      result->col(1).swap(result->col(2));
+      result->col(0).swap(result->col(2));
+      result->col(1).swap(result->col(3));
     }
     else
     {
       // last possible case. Since matrix is assumed to be invertible, this last case has to work.
-      m.row(1).swap(m.row(2));
+      // first, undo the swaps previously made
+      m.row(0).swap(m.row(2));
       m.row(1).swap(m.row(3));
+      // swap row 0 with the the row among 0 and 1 that has the biggest 2 first coeffs
+      int swap0with = ei_abs(m.coeff(0,0))+ei_abs(m.coeff(0,1))>ei_abs(m.coeff(1,0))+ei_abs(m.coeff(1,1)) ? 0 : 1;
+      m.row(0).swap(m.row(swap0with));
+      // swap row 1 with the the row among 2 and 3 that has the biggest 2 first coeffs
+      int swap1with = ei_abs(m.coeff(2,0))+ei_abs(m.coeff(2,1))>ei_abs(m.coeff(3,0))+ei_abs(m.coeff(3,1)) ? 2 : 3;
+      m.row(1).swap(m.row(swap1with));
       ei_compute_inverse_in_size4_case_helper(m, result);
-      result->col(1).swap(result->col(3));
+      result->col(1).swap(result->col(swap1with));
+      result->col(0).swap(result->col(swap0with));
     }
   }
 }