computeInverseWithCheck method added to matrix base (specialization for 1D to 4D)

2 files changed, 185 insertions, 17 deletions

diff --git a/Eigen/src/Core/MatrixBase.h b/Eigen/src/Core/MatrixBase.h
index addf59570..df411da31 100644
--- a/Eigen/src/Core/MatrixBase.h
+++ b/Eigen/src/Core/MatrixBase.h
@@ -643,6 +643,7 @@ template<typename Derived> class MatrixBase
     const PartialLU<PlainMatrixType> partialLu() const;
     const PlainMatrixType inverse() const;
     void computeInverse(PlainMatrixType *result) const;
+	bool computeInverseWithCheck( PlainMatrixType *result ) const;
     Scalar determinant() const;
 
 /////////// Cholesky module ///////////
diff --git a/Eigen/src/LU/Inverse.h b/Eigen/src/LU/Inverse.h
index b29efb60c..02e23b407 100644
--- a/Eigen/src/LU/Inverse.h
+++ b/Eigen/src/LU/Inverse.h
@@ -29,15 +29,23 @@
 *** Part 1 : optimized implementations for fixed-size 2,3,4 cases ***
 ********************************************************************/
 
+template<typename XprType, typename MatrixType>
+inline void ei_compute_inverse_in_size2_aux(
+		const XprType& matrix, const typename MatrixType::Scalar invdet,
+		MatrixType* result)
+{
+	result->coeffRef(0,0) = matrix.coeff(1,1) * invdet;
+	result->coeffRef(1,0) = -matrix.coeff(1,0) * invdet;
+	result->coeffRef(0,1) = -matrix.coeff(0,1) * invdet;
+	result->coeffRef(1,1) = matrix.coeff(0,0) * invdet;
+}
+
 template<typename MatrixType>
 void ei_compute_inverse_in_size2_case(const MatrixType& matrix, MatrixType* result)
 {
   typedef typename MatrixType::Scalar Scalar;
   const Scalar invdet = Scalar(1) / matrix.determinant();
-  result->coeffRef(0,0) = matrix.coeff(1,1) * invdet;
-  result->coeffRef(1,0) = -matrix.coeff(1,0) * invdet;
-  result->coeffRef(0,1) = -matrix.coeff(0,1) * invdet;
-  result->coeffRef(1,1) = matrix.coeff(0,0) * invdet;
+  ei_compute_inverse_in_size2_aux( matrix, invdet, result );
 }
 
 template<typename XprType, typename MatrixType>
@@ -47,23 +55,19 @@ bool ei_compute_inverse_in_size2_case_with_check(const XprType& matrix, MatrixTy
   const Scalar det = matrix.determinant();
   if(ei_isMuchSmallerThan(det, matrix.cwise().abs().maxCoeff())) return false;
   const Scalar invdet = Scalar(1) / det;
-  result->coeffRef(0,0) = matrix.coeff(1,1) * invdet;
-  result->coeffRef(1,0) = -matrix.coeff(1,0) * invdet;
-  result->coeffRef(0,1) = -matrix.coeff(0,1) * invdet;
-  result->coeffRef(1,1) = matrix.coeff(0,0) * invdet;
+  ei_compute_inverse_in_size2_aux( matrix, invdet, result );
   return true;
 }
 
-template<typename MatrixType>
-void ei_compute_inverse_in_size3_case(const MatrixType& matrix, MatrixType* result)
+template<typename XprType, typename MatrixType>
+inline void ei_compute_inverse_in_size3_aux(
+		const XprType& matrix,
+		const typename MatrixType::Scalar invdet,
+		const typename MatrixType::Scalar det_minor00,
+		const typename MatrixType::Scalar det_minor10,
+		const typename MatrixType::Scalar det_minor20,
+		MatrixType* result)
 {
-  typedef typename MatrixType::Scalar Scalar;
-  const Scalar det_minor00 = matrix.minor(0,0).determinant();
-  const Scalar det_minor10 = matrix.minor(1,0).determinant();
-  const Scalar det_minor20 = matrix.minor(2,0).determinant();
-  const Scalar invdet = Scalar(1) / ( det_minor00 * matrix.coeff(0,0)
-                                    - det_minor10 * matrix.coeff(1,0)
-                                    + det_minor20 * matrix.coeff(2,0) );
   result->coeffRef(0, 0) = det_minor00 * invdet;
   result->coeffRef(0, 1) = -det_minor10 * invdet;
   result->coeffRef(0, 2) = det_minor20 * invdet;
@@ -75,6 +79,36 @@ void ei_compute_inverse_in_size3_case(const MatrixType& matrix, MatrixType* resu
   result->coeffRef(2, 2) = matrix.minor(2,2).determinant() * invdet;
 }
 
+
+template<typename MatrixType>
+void ei_compute_inverse_in_size3_case(const MatrixType& matrix, MatrixType* result)
+{
+  typedef typename MatrixType::Scalar Scalar;
+  const Scalar det_minor00 = matrix.minor(0,0).determinant();
+  const Scalar det_minor10 = matrix.minor(1,0).determinant();
+  const Scalar det_minor20 = matrix.minor(2,0).determinant();
+  const Scalar invdet = Scalar(1) / ( det_minor00 * matrix.coeff(0,0)
+                                    - det_minor10 * matrix.coeff(1,0)
+                                    + det_minor20 * matrix.coeff(2,0) );
+  ei_compute_inverse_in_size3_aux( matrix, invdet, det_minor00, det_minor10, det_minor20, result );
+}
+
+template<typename XprType, typename MatrixType>
+bool ei_compute_inverse_in_size3_case_with_check(const XprType& matrix, MatrixType* result)
+{
+  typedef typename MatrixType::Scalar Scalar;
+  const Scalar det_minor00 = matrix.minor(0,0).determinant();
+  const Scalar det_minor10 = matrix.minor(1,0).determinant();
+  const Scalar det_minor20 = matrix.minor(2,0).determinant();
+  const Scalar det = ( det_minor00 * matrix.coeff(0,0)
+		  - det_minor10 * matrix.coeff(1,0)
+		  + det_minor20 * matrix.coeff(2,0) );
+  if(ei_isMuchSmallerThan(det, matrix.cwise().abs().maxCoeff())) return false;
+  const Scalar invdet = Scalar(1) / det;
+  ei_compute_inverse_in_size3_aux( matrix, invdet, det_minor00, det_minor10, det_minor20, result );
+  return true;
+}
+
 template<typename MatrixType>
 bool ei_compute_inverse_in_size4_case_helper(const MatrixType& matrix, MatrixType* result)
 {
@@ -161,6 +195,59 @@ void ei_compute_inverse_in_size4_case(const MatrixType& matrix, MatrixType* resu
   }
 }
 
+
+template<typename XprType, typename MatrixType>
+bool ei_compute_inverse_in_size4_case_with_check(const XprType& matrix, MatrixType* result)
+{
+  if(ei_compute_inverse_in_size4_case_helper(matrix, result))
+  {
+    // good ! The topleft 2x2 block was invertible, so the 2x2 blocks approach is successful.
+    return true;
+  }
+  else
+  {
+    // rare case: the topleft 2x2 block is not invertible (but the matrix itself is assumed to be).
+    // 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(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 some
+      // rows were permuted, the actual inverse of matrix is derived from the inverse of m by permuting
+      // the corresponding columns.
+      result->col(0).swap(result->col(2));
+      result->col(1).swap(result->col(3));
+	  return true;
+    }
+    else
+    {
+      // last possible case. Since matrix is assumed to be invertible, this last case has to work.
+      // 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));
+	  if( ei_compute_inverse_in_size4_case_helper(m, result) )
+	  {
+		  result->col(1).swap(result->col(swap1with));
+		  result->col(0).swap(result->col(swap0with));
+		  return true;
+	  }
+	  else{
+		  return false; }
+    }
+  }
+
+}
+
+
+
 /***********************************************
 *** Part 2 : selector and MatrixBase methods ***
 ***********************************************/
@@ -254,4 +341,84 @@ inline const typename MatrixBase<Derived>::PlainMatrixType MatrixBase<Derived>::
   return result;
 }
 
+
+/*****************************************
+ * Compute inverse with invertibility check
+*********************************************/
+
+template<typename MatrixType, int Size = MatrixType::RowsAtCompileTime>
+struct ei_compute_inverse_with_check
+{
+  static inline bool run(const MatrixType& matrix, MatrixType* result)
+  {
+	  typedef typename MatrixType::Scalar Scalar;
+	  LU<MatrixType> lu( matrix );
+	  if( !lu.isInvertible() ) return false;
+	  lu.computeInverse(result);
+	  return true;
+  }
+};
+
+template<typename MatrixType>
+struct ei_compute_inverse_with_check<MatrixType, 1>
+{
+  static inline bool run(const MatrixType& matrix, MatrixType* result)
+  {
+	  if( 0 == result->coeffRef(0,0) ) return false;
+	
+	  typedef typename MatrixType::Scalar Scalar;
+	  result->coeffRef(0,0) = Scalar(1) / matrix.coeff(0,0);
+	  return true;
+  }
+};
+
+template<typename MatrixType>
+struct ei_compute_inverse_with_check<MatrixType, 2>
+{
+  static inline bool run(const MatrixType& matrix, MatrixType* result)
+  {
+    return ei_compute_inverse_in_size2_case_with_check(matrix, result);
+  }
+};
+
+template<typename MatrixType>
+struct ei_compute_inverse_with_check<MatrixType, 3>
+{
+  static inline bool run(const MatrixType& matrix, MatrixType* result)
+  {
+    return ei_compute_inverse_in_size3_case_with_check(matrix, result);
+  }
+};
+
+template<typename MatrixType>
+struct ei_compute_inverse_with_check<MatrixType, 4>
+{
+  static inline bool run(const MatrixType& matrix, MatrixType* result)
+  {
+    return ei_compute_inverse_in_size4_case_with_check(matrix, result);
+  }
+};
+
+/** \lu_module
+  *
+  * If the matrix is invertible, computes the matrix inverse of this matrix
+  * and returns true otherwise return false.
+  *
+  * \note This matrix must be invertible, otherwise the result is undefined.
+  *
+  * \param result Pointer to the matrix in which to store the result. Undefined
+  *  if the matrix is not invertible.
+  * \return true if the matrix is invertible false otherwise.
+  *
+  * \sa inverse()
+  */
+template<typename Derived>
+inline bool MatrixBase<Derived>::computeInverseWithCheck(PlainMatrixType *result) const
+{
+  ei_assert(rows() == cols());
+  EIGEN_STATIC_ASSERT(NumTraits<Scalar>::HasFloatingPoint,NUMERIC_TYPE_MUST_BE_FLOATING_POINT)
+  return ei_compute_inverse_with_check<PlainMatrixType>::run(eval(), result);
+}
+
+
 #endif // EIGEN_INVERSE_H