Inverse.h

00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra.
00003 //
00004 // Copyright (C) 2008-2010 Benoit Jacob <jacob.benoit.1@gmail.com>
00005 //
00006 // This Source Code Form is subject to the terms of the Mozilla
00007 // Public License v. 2.0. If a copy of the MPL was not distributed
00008 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
00009 
00010 #ifndef EIGEN_INVERSE_H
00011 #define EIGEN_INVERSE_H
00012 
00013 namespace Eigen { 
00014 
00015 namespace internal {
00016 
00017 /**********************************
00018 *** General case implementation ***
00019 **********************************/
00020 
00021 template<typename MatrixType, typename ResultType, int Size = MatrixType::RowsAtCompileTime>
00022 struct compute_inverse
00023 {
00024   static inline void run(const MatrixType& matrix, ResultType& result)
00025   {
00026     result = matrix.partialPivLu().inverse();
00027   }
00028 };
00029 
00030 template<typename MatrixType, typename ResultType, int Size = MatrixType::RowsAtCompileTime>
00031 struct compute_inverse_and_det_with_check { /* nothing! general case not supported. */ };
00032 
00033 /****************************
00034 *** Size 1 implementation ***
00035 ****************************/
00036 
00037 template<typename MatrixType, typename ResultType>
00038 struct compute_inverse<MatrixType, ResultType, 1>
00039 {
00040   static inline void run(const MatrixType& matrix, ResultType& result)
00041   {
00042     typedef typename MatrixType::Scalar Scalar;
00043     result.coeffRef(0,0) = Scalar(1) / matrix.coeff(0,0);
00044   }
00045 };
00046 
00047 template<typename MatrixType, typename ResultType>
00048 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 1>
00049 {
00050   static inline void run(
00051     const MatrixType& matrix,
00052     const typename MatrixType::RealScalar& absDeterminantThreshold,
00053     ResultType& result,
00054     typename ResultType::Scalar& determinant,
00055     bool& invertible
00056   )
00057   {
00058     using std::abs;
00059     determinant = matrix.coeff(0,0);
00060     invertible = abs(determinant) > absDeterminantThreshold;
00061     if(invertible) result.coeffRef(0,0) = typename ResultType::Scalar(1) / determinant;
00062   }
00063 };
00064 
00065 /****************************
00066 *** Size 2 implementation ***
00067 ****************************/
00068 
00069 template<typename MatrixType, typename ResultType>
00070 inline void compute_inverse_size2_helper(
00071     const MatrixType& matrix, const typename ResultType::Scalar& invdet,
00072     ResultType& result)
00073 {
00074   result.coeffRef(0,0) = matrix.coeff(1,1) * invdet;
00075   result.coeffRef(1,0) = -matrix.coeff(1,0) * invdet;
00076   result.coeffRef(0,1) = -matrix.coeff(0,1) * invdet;
00077   result.coeffRef(1,1) = matrix.coeff(0,0) * invdet;
00078 }
00079 
00080 template<typename MatrixType, typename ResultType>
00081 struct compute_inverse<MatrixType, ResultType, 2>
00082 {
00083   static inline void run(const MatrixType& matrix, ResultType& result)
00084   {
00085     typedef typename ResultType::Scalar Scalar;
00086     const Scalar invdet = typename MatrixType::Scalar(1) / matrix.determinant();
00087     compute_inverse_size2_helper(matrix, invdet, result);
00088   }
00089 };
00090 
00091 template<typename MatrixType, typename ResultType>
00092 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 2>
00093 {
00094   static inline void run(
00095     const MatrixType& matrix,
00096     const typename MatrixType::RealScalar& absDeterminantThreshold,
00097     ResultType& inverse,
00098     typename ResultType::Scalar& determinant,
00099     bool& invertible
00100   )
00101   {
00102     using std::abs;
00103     typedef typename ResultType::Scalar Scalar;
00104     determinant = matrix.determinant();
00105     invertible = abs(determinant) > absDeterminantThreshold;
00106     if(!invertible) return;
00107     const Scalar invdet = Scalar(1) / determinant;
00108     compute_inverse_size2_helper(matrix, invdet, inverse);
00109   }
00110 };
00111 
00112 /****************************
00113 *** Size 3 implementation ***
00114 ****************************/
00115 
00116 template<typename MatrixType, int i, int j>
00117 inline typename MatrixType::Scalar cofactor_3x3(const MatrixType& m)
00118 {
00119   enum {
00120     i1 = (i+1) % 3,
00121     i2 = (i+2) % 3,
00122     j1 = (j+1) % 3,
00123     j2 = (j+2) % 3
00124   };
00125   return m.coeff(i1, j1) * m.coeff(i2, j2)
00126        - m.coeff(i1, j2) * m.coeff(i2, j1);
00127 }
00128 
00129 template<typename MatrixType, typename ResultType>
00130 inline void compute_inverse_size3_helper(
00131     const MatrixType& matrix,
00132     const typename ResultType::Scalar& invdet,
00133     const Matrix<typename ResultType::Scalar,3,1>& cofactors_col0,
00134     ResultType& result)
00135 {
00136   result.row(0) = cofactors_col0 * invdet;
00137   result.coeffRef(1,0) =  cofactor_3x3<MatrixType,0,1>(matrix) * invdet;
00138   result.coeffRef(1,1) =  cofactor_3x3<MatrixType,1,1>(matrix) * invdet;
00139   result.coeffRef(1,2) =  cofactor_3x3<MatrixType,2,1>(matrix) * invdet;
00140   result.coeffRef(2,0) =  cofactor_3x3<MatrixType,0,2>(matrix) * invdet;
00141   result.coeffRef(2,1) =  cofactor_3x3<MatrixType,1,2>(matrix) * invdet;
00142   result.coeffRef(2,2) =  cofactor_3x3<MatrixType,2,2>(matrix) * invdet;
00143 }
00144 
00145 template<typename MatrixType, typename ResultType>
00146 struct compute_inverse<MatrixType, ResultType, 3>
00147 {
00148   static inline void run(const MatrixType& matrix, ResultType& result)
00149   {
00150     typedef typename ResultType::Scalar Scalar;
00151     Matrix<typename MatrixType::Scalar,3,1> cofactors_col0;
00152     cofactors_col0.coeffRef(0) =  cofactor_3x3<MatrixType,0,0>(matrix);
00153     cofactors_col0.coeffRef(1) =  cofactor_3x3<MatrixType,1,0>(matrix);
00154     cofactors_col0.coeffRef(2) =  cofactor_3x3<MatrixType,2,0>(matrix);
00155     const Scalar det = (cofactors_col0.cwiseProduct(matrix.col(0))).sum();
00156     const Scalar invdet = Scalar(1) / det;
00157     compute_inverse_size3_helper(matrix, invdet, cofactors_col0, result);
00158   }
00159 };
00160 
00161 template<typename MatrixType, typename ResultType>
00162 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 3>
00163 {
00164   static inline void run(
00165     const MatrixType& matrix,
00166     const typename MatrixType::RealScalar& absDeterminantThreshold,
00167     ResultType& inverse,
00168     typename ResultType::Scalar& determinant,
00169     bool& invertible
00170   )
00171   {
00172     using std::abs;
00173     typedef typename ResultType::Scalar Scalar;
00174     Matrix<Scalar,3,1> cofactors_col0;
00175     cofactors_col0.coeffRef(0) =  cofactor_3x3<MatrixType,0,0>(matrix);
00176     cofactors_col0.coeffRef(1) =  cofactor_3x3<MatrixType,1,0>(matrix);
00177     cofactors_col0.coeffRef(2) =  cofactor_3x3<MatrixType,2,0>(matrix);
00178     determinant = (cofactors_col0.cwiseProduct(matrix.col(0))).sum();
00179     invertible = abs(determinant) > absDeterminantThreshold;
00180     if(!invertible) return;
00181     const Scalar invdet = Scalar(1) / determinant;
00182     compute_inverse_size3_helper(matrix, invdet, cofactors_col0, inverse);
00183   }
00184 };
00185 
00186 /****************************
00187 *** Size 4 implementation ***
00188 ****************************/
00189 
00190 template<typename Derived>
00191 inline const typename Derived::Scalar general_det3_helper
00192 (const MatrixBase<Derived>& matrix, int i1, int i2, int i3, int j1, int j2, int j3)
00193 {
00194   return matrix.coeff(i1,j1)
00195          * (matrix.coeff(i2,j2) * matrix.coeff(i3,j3) - matrix.coeff(i2,j3) * matrix.coeff(i3,j2));
00196 }
00197 
00198 template<typename MatrixType, int i, int j>
00199 inline typename MatrixType::Scalar cofactor_4x4(const MatrixType& matrix)
00200 {
00201   enum {
00202     i1 = (i+1) % 4,
00203     i2 = (i+2) % 4,
00204     i3 = (i+3) % 4,
00205     j1 = (j+1) % 4,
00206     j2 = (j+2) % 4,
00207     j3 = (j+3) % 4
00208   };
00209   return general_det3_helper(matrix, i1, i2, i3, j1, j2, j3)
00210        + general_det3_helper(matrix, i2, i3, i1, j1, j2, j3)
00211        + general_det3_helper(matrix, i3, i1, i2, j1, j2, j3);
00212 }
00213 
00214 template<int Arch, typename Scalar, typename MatrixType, typename ResultType>
00215 struct compute_inverse_size4
00216 {
00217   static void run(const MatrixType& matrix, ResultType& result)
00218   {
00219     result.coeffRef(0,0) =  cofactor_4x4<MatrixType,0,0>(matrix);
00220     result.coeffRef(1,0) = -cofactor_4x4<MatrixType,0,1>(matrix);
00221     result.coeffRef(2,0) =  cofactor_4x4<MatrixType,0,2>(matrix);
00222     result.coeffRef(3,0) = -cofactor_4x4<MatrixType,0,3>(matrix);
00223     result.coeffRef(0,2) =  cofactor_4x4<MatrixType,2,0>(matrix);
00224     result.coeffRef(1,2) = -cofactor_4x4<MatrixType,2,1>(matrix);
00225     result.coeffRef(2,2) =  cofactor_4x4<MatrixType,2,2>(matrix);
00226     result.coeffRef(3,2) = -cofactor_4x4<MatrixType,2,3>(matrix);
00227     result.coeffRef(0,1) = -cofactor_4x4<MatrixType,1,0>(matrix);
00228     result.coeffRef(1,1) =  cofactor_4x4<MatrixType,1,1>(matrix);
00229     result.coeffRef(2,1) = -cofactor_4x4<MatrixType,1,2>(matrix);
00230     result.coeffRef(3,1) =  cofactor_4x4<MatrixType,1,3>(matrix);
00231     result.coeffRef(0,3) = -cofactor_4x4<MatrixType,3,0>(matrix);
00232     result.coeffRef(1,3) =  cofactor_4x4<MatrixType,3,1>(matrix);
00233     result.coeffRef(2,3) = -cofactor_4x4<MatrixType,3,2>(matrix);
00234     result.coeffRef(3,3) =  cofactor_4x4<MatrixType,3,3>(matrix);
00235     result /= (matrix.col(0).cwiseProduct(result.row(0).transpose())).sum();
00236   }
00237 };
00238 
00239 template<typename MatrixType, typename ResultType>
00240 struct compute_inverse<MatrixType, ResultType, 4>
00241  : compute_inverse_size4<Architecture::Target, typename MatrixType::Scalar,
00242                             MatrixType, ResultType>
00243 {
00244 };
00245 
00246 template<typename MatrixType, typename ResultType>
00247 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 4>
00248 {
00249   static inline void run(
00250     const MatrixType& matrix,
00251     const typename MatrixType::RealScalar& absDeterminantThreshold,
00252     ResultType& inverse,
00253     typename ResultType::Scalar& determinant,
00254     bool& invertible
00255   )
00256   {
00257     using std::abs;
00258     determinant = matrix.determinant();
00259     invertible = abs(determinant) > absDeterminantThreshold;
00260     if(invertible) compute_inverse<MatrixType, ResultType>::run(matrix, inverse);
00261   }
00262 };
00263 
00264 /*************************
00265 *** MatrixBase methods ***
00266 *************************/
00267 
00268 template<typename MatrixType>
00269 struct traits<inverse_impl<MatrixType> >
00270 {
00271   typedef typename MatrixType::PlainObject ReturnType;
00272 };
00273 
00274 template<typename MatrixType>
00275 struct inverse_impl : public ReturnByValue<inverse_impl<MatrixType> >
00276 {
00277   typedef typename MatrixType::Index Index;
00278   typedef typename internal::eval<MatrixType>::type MatrixTypeNested;
00279   typedef typename remove_all<MatrixTypeNested>::type MatrixTypeNestedCleaned;
00280   MatrixTypeNested m_matrix;
00281 
00282   inverse_impl(const MatrixType& matrix)
00283     : m_matrix(matrix)
00284   {}
00285 
00286   inline Index rows() const { return m_matrix.rows(); }
00287   inline Index cols() const { return m_matrix.cols(); }
00288 
00289   template<typename Dest> inline void evalTo(Dest& dst) const
00290   {
00291     const int Size = EIGEN_PLAIN_ENUM_MIN(MatrixType::ColsAtCompileTime,Dest::ColsAtCompileTime);
00292     EIGEN_ONLY_USED_FOR_DEBUG(Size);
00293     eigen_assert(( (Size<=1) || (Size>4) || (extract_data(m_matrix)!=extract_data(dst)))
00294               && "Aliasing problem detected in inverse(), you need to do inverse().eval() here.");
00295 
00296     compute_inverse<MatrixTypeNestedCleaned, Dest>::run(m_matrix, dst);
00297   }
00298 };
00299 
00300 } // end namespace internal
00301 
00302 /** \lu_module
00303   *
00304   * \returns the matrix inverse of this matrix.
00305   *
00306   * For small fixed sizes up to 4x4, this method uses cofactors.
00307   * In the general case, this method uses class PartialPivLU.
00308   *
00309   * \note This matrix must be invertible, otherwise the result is undefined. If you need an
00310   * invertibility check, do the following:
00311   * \li for fixed sizes up to 4x4, use computeInverseAndDetWithCheck().
00312   * \li for the general case, use class FullPivLU.
00313   *
00314   * Example: \include MatrixBase_inverse.cpp
00315   * Output: \verbinclude MatrixBase_inverse.out
00316   *
00317   * \sa computeInverseAndDetWithCheck()
00318   */
00319 template<typename Derived>
00320 inline const internal::inverse_impl<Derived> MatrixBase<Derived>::inverse () const
00321 {
00322   EIGEN_STATIC_ASSERT(!NumTraits<Scalar>::IsInteger,THIS_FUNCTION_IS_NOT_FOR_INTEGER_NUMERIC_TYPES)
00323   eigen_assert(rows() == cols());
00324   return internal::inverse_impl<Derived>(derived());
00325 }
00326 
00327 /** \lu_module
00328   *
00329   * Computation of matrix inverse and determinant, with invertibility check.
00330   *
00331   * This is only for fixed-size square matrices of size up to 4x4.
00332   *
00333   * \param inverse Reference to the matrix in which to store the inverse.
00334   * \param determinant Reference to the variable in which to store the determinant.
00335   * \param invertible Reference to the bool variable in which to store whether the matrix is invertible.
00336   * \param absDeterminantThreshold Optional parameter controlling the invertibility check.
00337   *                                The matrix will be declared invertible if the absolute value of its
00338   *                                determinant is greater than this threshold.
00339   *
00340   * Example: \include MatrixBase_computeInverseAndDetWithCheck.cpp
00341   * Output: \verbinclude MatrixBase_computeInverseAndDetWithCheck.out
00342   *
00343   * \sa inverse(), computeInverseWithCheck()
00344   */
00345 template<typename Derived>
00346 template<typename ResultType>
00347 inline void MatrixBase<Derived>::computeInverseAndDetWithCheck (
00348     ResultType& inverse,
00349     typename ResultType::Scalar& determinant,
00350     bool& invertible,
00351     const RealScalar& absDeterminantThreshold
00352   ) const
00353 {
00354   // i'd love to put some static assertions there, but SFINAE means that they have no effect...
00355   eigen_assert(rows() == cols());
00356   // for 2x2, it's worth giving a chance to avoid evaluating.
00357   // for larger sizes, evaluating has negligible cost and limits code size.
00358   typedef typename internal::conditional<
00359     RowsAtCompileTime == 2,
00360     typename internal::remove_all<typename internal::nested<Derived, 2>::type>::type,
00361     PlainObject
00362   >::type MatrixType;
00363   internal::compute_inverse_and_det_with_check<MatrixType, ResultType>::run
00364     (derived(), absDeterminantThreshold, inverse, determinant, invertible);
00365 }
00366 
00367 /** \lu_module
00368   *
00369   * Computation of matrix inverse, with invertibility check.
00370   *
00371   * This is only for fixed-size square matrices of size up to 4x4.
00372   *
00373   * \param inverse Reference to the matrix in which to store the inverse.
00374   * \param invertible Reference to the bool variable in which to store whether the matrix is invertible.
00375   * \param absDeterminantThreshold Optional parameter controlling the invertibility check.
00376   *                                The matrix will be declared invertible if the absolute value of its
00377   *                                determinant is greater than this threshold.
00378   *
00379   * Example: \include MatrixBase_computeInverseWithCheck.cpp
00380   * Output: \verbinclude MatrixBase_computeInverseWithCheck.out
00381   *
00382   * \sa inverse(), computeInverseAndDetWithCheck()
00383   */
00384 template<typename Derived>
00385 template<typename ResultType>
00386 inline void MatrixBase<Derived>::computeInverseWithCheck (
00387     ResultType& inverse,
00388     bool& invertible,
00389     const RealScalar& absDeterminantThreshold
00390   ) const
00391 {
00392   RealScalar determinant;
00393   // i'd love to put some static assertions there, but SFINAE means that they have no effect...
00394   eigen_assert(rows() == cols());
00395   computeInverseAndDetWithCheck(inverse,determinant,invertible,absDeterminantThreshold);
00396 }
00397 
00398 } // end namespace Eigen
00399 
00400 #endif // EIGEN_INVERSE_H