Inverse.h
Go to the documentation of this file.
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 // Eigen is free software; you can redistribute it and/or
00007 // modify it under the terms of the GNU Lesser General Public
00008 // License as published by the Free Software Foundation; either
00009 // version 3 of the License, or (at your option) any later version.
00010 //
00011 // Alternatively, you can redistribute it and/or
00012 // modify it under the terms of the GNU General Public License as
00013 // published by the Free Software Foundation; either version 2 of
00014 // the License, or (at your option) any later version.
00015 //
00016 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
00017 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
00018 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
00019 // GNU General Public License for more details.
00020 //
00021 // You should have received a copy of the GNU Lesser General Public
00022 // License and a copy of the GNU General Public License along with
00023 // Eigen. If not, see <http://www.gnu.org/licenses/>.
00024 
00025 #ifndef EIGEN_INVERSE_H
00026 #define EIGEN_INVERSE_H
00027 
00028 namespace Eigen { 
00029 
00030 namespace internal {
00031 
00032 /**********************************
00033 *** General case implementation ***
00034 **********************************/
00035 
00036 template<typename MatrixType, typename ResultType, int Size = MatrixType::RowsAtCompileTime>
00037 struct compute_inverse
00038 {
00039   static inline void run(const MatrixType& matrix, ResultType& result)
00040   {
00041     result = matrix.partialPivLu().inverse();
00042   }
00043 };
00044 
00045 template<typename MatrixType, typename ResultType, int Size = MatrixType::RowsAtCompileTime>
00046 struct compute_inverse_and_det_with_check { /* nothing! general case not supported. */ };
00047 
00048 /****************************
00049 *** Size 1 implementation ***
00050 ****************************/
00051 
00052 template<typename MatrixType, typename ResultType>
00053 struct compute_inverse<MatrixType, ResultType, 1>
00054 {
00055   static inline void run(const MatrixType& matrix, ResultType& result)
00056   {
00057     typedef typename MatrixType::Scalar Scalar;
00058     result.coeffRef(0,0) = Scalar(1) / matrix.coeff(0,0);
00059   }
00060 };
00061 
00062 template<typename MatrixType, typename ResultType>
00063 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 1>
00064 {
00065   static inline void run(
00066     const MatrixType& matrix,
00067     const typename MatrixType::RealScalar& absDeterminantThreshold,
00068     ResultType& result,
00069     typename ResultType::Scalar& determinant,
00070     bool& invertible
00071   )
00072   {
00073     determinant = matrix.coeff(0,0);
00074     invertible = abs(determinant) > absDeterminantThreshold;
00075     if(invertible) result.coeffRef(0,0) = typename ResultType::Scalar(1) / determinant;
00076   }
00077 };
00078 
00079 /****************************
00080 *** Size 2 implementation ***
00081 ****************************/
00082 
00083 template<typename MatrixType, typename ResultType>
00084 inline void compute_inverse_size2_helper(
00085     const MatrixType& matrix, const typename ResultType::Scalar& invdet,
00086     ResultType& result)
00087 {
00088   result.coeffRef(0,0) = matrix.coeff(1,1) * invdet;
00089   result.coeffRef(1,0) = -matrix.coeff(1,0) * invdet;
00090   result.coeffRef(0,1) = -matrix.coeff(0,1) * invdet;
00091   result.coeffRef(1,1) = matrix.coeff(0,0) * invdet;
00092 }
00093 
00094 template<typename MatrixType, typename ResultType>
00095 struct compute_inverse<MatrixType, ResultType, 2>
00096 {
00097   static inline void run(const MatrixType& matrix, ResultType& result)
00098   {
00099     typedef typename ResultType::Scalar Scalar;
00100     const Scalar invdet = typename MatrixType::Scalar(1) / matrix.determinant();
00101     compute_inverse_size2_helper(matrix, invdet, result);
00102   }
00103 };
00104 
00105 template<typename MatrixType, typename ResultType>
00106 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 2>
00107 {
00108   static inline void run(
00109     const MatrixType& matrix,
00110     const typename MatrixType::RealScalar& absDeterminantThreshold,
00111     ResultType& inverse,
00112     typename ResultType::Scalar& determinant,
00113     bool& invertible
00114   )
00115   {
00116     typedef typename ResultType::Scalar Scalar;
00117     determinant = matrix.determinant();
00118     invertible = abs(determinant) > absDeterminantThreshold;
00119     if(!invertible) return;
00120     const Scalar invdet = Scalar(1) / determinant;
00121     compute_inverse_size2_helper(matrix, invdet, inverse);
00122   }
00123 };
00124 
00125 /****************************
00126 *** Size 3 implementation ***
00127 ****************************/
00128 
00129 template<typename MatrixType, int i, int j>
00130 inline typename MatrixType::Scalar cofactor_3x3(const MatrixType& m)
00131 {
00132   enum {
00133     i1 = (i+1) % 3,
00134     i2 = (i+2) % 3,
00135     j1 = (j+1) % 3,
00136     j2 = (j+2) % 3
00137   };
00138   return m.coeff(i1, j1) * m.coeff(i2, j2)
00139        - m.coeff(i1, j2) * m.coeff(i2, j1);
00140 }
00141 
00142 template<typename MatrixType, typename ResultType>
00143 inline void compute_inverse_size3_helper(
00144     const MatrixType& matrix,
00145     const typename ResultType::Scalar& invdet,
00146     const Matrix<typename ResultType::Scalar,3,1>& cofactors_col0,
00147     ResultType& result)
00148 {
00149   result.row(0) = cofactors_col0 * invdet;
00150   result.coeffRef(1,0) =  cofactor_3x3<MatrixType,0,1>(matrix) * invdet;
00151   result.coeffRef(1,1) =  cofactor_3x3<MatrixType,1,1>(matrix) * invdet;
00152   result.coeffRef(1,2) =  cofactor_3x3<MatrixType,2,1>(matrix) * invdet;
00153   result.coeffRef(2,0) =  cofactor_3x3<MatrixType,0,2>(matrix) * invdet;
00154   result.coeffRef(2,1) =  cofactor_3x3<MatrixType,1,2>(matrix) * invdet;
00155   result.coeffRef(2,2) =  cofactor_3x3<MatrixType,2,2>(matrix) * invdet;
00156 }
00157 
00158 template<typename MatrixType, typename ResultType>
00159 struct compute_inverse<MatrixType, ResultType, 3>
00160 {
00161   static inline void run(const MatrixType& matrix, ResultType& result)
00162   {
00163     typedef typename ResultType::Scalar Scalar;
00164     Matrix<typename MatrixType::Scalar,3,1> cofactors_col0;
00165     cofactors_col0.coeffRef(0) =  cofactor_3x3<MatrixType,0,0>(matrix);
00166     cofactors_col0.coeffRef(1) =  cofactor_3x3<MatrixType,1,0>(matrix);
00167     cofactors_col0.coeffRef(2) =  cofactor_3x3<MatrixType,2,0>(matrix);
00168     const Scalar det = (cofactors_col0.cwiseProduct(matrix.col(0))).sum();
00169     const Scalar invdet = Scalar(1) / det;
00170     compute_inverse_size3_helper(matrix, invdet, cofactors_col0, result);
00171   }
00172 };
00173 
00174 template<typename MatrixType, typename ResultType>
00175 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 3>
00176 {
00177   static inline void run(
00178     const MatrixType& matrix,
00179     const typename MatrixType::RealScalar& absDeterminantThreshold,
00180     ResultType& inverse,
00181     typename ResultType::Scalar& determinant,
00182     bool& invertible
00183   )
00184   {
00185     typedef typename ResultType::Scalar Scalar;
00186     Matrix<Scalar,3,1> cofactors_col0;
00187     cofactors_col0.coeffRef(0) =  cofactor_3x3<MatrixType,0,0>(matrix);
00188     cofactors_col0.coeffRef(1) =  cofactor_3x3<MatrixType,1,0>(matrix);
00189     cofactors_col0.coeffRef(2) =  cofactor_3x3<MatrixType,2,0>(matrix);
00190     determinant = (cofactors_col0.cwiseProduct(matrix.col(0))).sum();
00191     invertible = abs(determinant) > absDeterminantThreshold;
00192     if(!invertible) return;
00193     const Scalar invdet = Scalar(1) / determinant;
00194     compute_inverse_size3_helper(matrix, invdet, cofactors_col0, inverse);
00195   }
00196 };
00197 
00198 /****************************
00199 *** Size 4 implementation ***
00200 ****************************/
00201 
00202 template<typename Derived>
00203 inline const typename Derived::Scalar general_det3_helper
00204 (const MatrixBase<Derived>& matrix, int i1, int i2, int i3, int j1, int j2, int j3)
00205 {
00206   return matrix.coeff(i1,j1)
00207          * (matrix.coeff(i2,j2) * matrix.coeff(i3,j3) - matrix.coeff(i2,j3) * matrix.coeff(i3,j2));
00208 }
00209 
00210 template<typename MatrixType, int i, int j>
00211 inline typename MatrixType::Scalar cofactor_4x4(const MatrixType& matrix)
00212 {
00213   enum {
00214     i1 = (i+1) % 4,
00215     i2 = (i+2) % 4,
00216     i3 = (i+3) % 4,
00217     j1 = (j+1) % 4,
00218     j2 = (j+2) % 4,
00219     j3 = (j+3) % 4
00220   };
00221   return general_det3_helper(matrix, i1, i2, i3, j1, j2, j3)
00222        + general_det3_helper(matrix, i2, i3, i1, j1, j2, j3)
00223        + general_det3_helper(matrix, i3, i1, i2, j1, j2, j3);
00224 }
00225 
00226 template<int Arch, typename Scalar, typename MatrixType, typename ResultType>
00227 struct compute_inverse_size4
00228 {
00229   static void run(const MatrixType& matrix, ResultType& result)
00230   {
00231     result.coeffRef(0,0) =  cofactor_4x4<MatrixType,0,0>(matrix);
00232     result.coeffRef(1,0) = -cofactor_4x4<MatrixType,0,1>(matrix);
00233     result.coeffRef(2,0) =  cofactor_4x4<MatrixType,0,2>(matrix);
00234     result.coeffRef(3,0) = -cofactor_4x4<MatrixType,0,3>(matrix);
00235     result.coeffRef(0,2) =  cofactor_4x4<MatrixType,2,0>(matrix);
00236     result.coeffRef(1,2) = -cofactor_4x4<MatrixType,2,1>(matrix);
00237     result.coeffRef(2,2) =  cofactor_4x4<MatrixType,2,2>(matrix);
00238     result.coeffRef(3,2) = -cofactor_4x4<MatrixType,2,3>(matrix);
00239     result.coeffRef(0,1) = -cofactor_4x4<MatrixType,1,0>(matrix);
00240     result.coeffRef(1,1) =  cofactor_4x4<MatrixType,1,1>(matrix);
00241     result.coeffRef(2,1) = -cofactor_4x4<MatrixType,1,2>(matrix);
00242     result.coeffRef(3,1) =  cofactor_4x4<MatrixType,1,3>(matrix);
00243     result.coeffRef(0,3) = -cofactor_4x4<MatrixType,3,0>(matrix);
00244     result.coeffRef(1,3) =  cofactor_4x4<MatrixType,3,1>(matrix);
00245     result.coeffRef(2,3) = -cofactor_4x4<MatrixType,3,2>(matrix);
00246     result.coeffRef(3,3) =  cofactor_4x4<MatrixType,3,3>(matrix);
00247     result /= (matrix.col(0).cwiseProduct(result.row(0).transpose())).sum();
00248   }
00249 };
00250 
00251 template<typename MatrixType, typename ResultType>
00252 struct compute_inverse<MatrixType, ResultType, 4>
00253  : compute_inverse_size4<Architecture::Target, typename MatrixType::Scalar,
00254                             MatrixType, ResultType>
00255 {
00256 };
00257 
00258 template<typename MatrixType, typename ResultType>
00259 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 4>
00260 {
00261   static inline void run(
00262     const MatrixType& matrix,
00263     const typename MatrixType::RealScalar& absDeterminantThreshold,
00264     ResultType& inverse,
00265     typename ResultType::Scalar& determinant,
00266     bool& invertible
00267   )
00268   {
00269     determinant = matrix.determinant();
00270     invertible = abs(determinant) > absDeterminantThreshold;
00271     if(invertible) compute_inverse<MatrixType, ResultType>::run(matrix, inverse);
00272   }
00273 };
00274 
00275 /*************************
00276 *** MatrixBase methods ***
00277 *************************/
00278 
00279 template<typename MatrixType>
00280 struct traits<inverse_impl<MatrixType> >
00281 {
00282   typedef typename MatrixType::PlainObject ReturnType;
00283 };
00284 
00285 template<typename MatrixType>
00286 struct inverse_impl : public ReturnByValue<inverse_impl<MatrixType> >
00287 {
00288   typedef typename MatrixType::Index Index;
00289   typedef typename internal::eval<MatrixType>::type MatrixTypeNested;
00290   typedef typename remove_all<MatrixTypeNested>::type MatrixTypeNestedCleaned;
00291   MatrixTypeNested m_matrix;
00292 
00293   inverse_impl(const MatrixType& matrix)
00294     : m_matrix(matrix)
00295   {}
00296 
00297   inline Index rows() const { return m_matrix.rows(); }
00298   inline Index cols() const { return m_matrix.cols(); }
00299 
00300   template<typename Dest> inline void evalTo(Dest& dst) const
00301   {
00302     const int Size = EIGEN_PLAIN_ENUM_MIN(MatrixType::ColsAtCompileTime,Dest::ColsAtCompileTime);
00303     EIGEN_ONLY_USED_FOR_DEBUG(Size);
00304     eigen_assert(( (Size<=1) || (Size>4) || (extract_data(m_matrix)!=extract_data(dst)))
00305               && "Aliasing problem detected in inverse(), you need to do inverse().eval() here.");
00306 
00307     compute_inverse<MatrixTypeNestedCleaned, Dest>::run(m_matrix, dst);
00308   }
00309 };
00310 
00311 } // end namespace internal
00312 
00330 template<typename Derived>
00331 inline const internal::inverse_impl<Derived> MatrixBase<Derived>::inverse() const
00332 {
00333   EIGEN_STATIC_ASSERT(!NumTraits<Scalar>::IsInteger,THIS_FUNCTION_IS_NOT_FOR_INTEGER_NUMERIC_TYPES)
00334   eigen_assert(rows() == cols());
00335   return internal::inverse_impl<Derived>(derived());
00336 }
00337 
00356 template<typename Derived>
00357 template<typename ResultType>
00358 inline void MatrixBase<Derived>::computeInverseAndDetWithCheck(
00359     ResultType& inverse,
00360     typename ResultType::Scalar& determinant,
00361     bool& invertible,
00362     const RealScalar& absDeterminantThreshold
00363   ) const
00364 {
00365   // i'd love to put some static assertions there, but SFINAE means that they have no effect...
00366   eigen_assert(rows() == cols());
00367   // for 2x2, it's worth giving a chance to avoid evaluating.
00368   // for larger sizes, evaluating has negligible cost and limits code size.
00369   typedef typename internal::conditional<
00370     RowsAtCompileTime == 2,
00371     typename internal::remove_all<typename internal::nested<Derived, 2>::type>::type,
00372     PlainObject
00373   >::type MatrixType;
00374   internal::compute_inverse_and_det_with_check<MatrixType, ResultType>::run
00375     (derived(), absDeterminantThreshold, inverse, determinant, invertible);
00376 }
00377 
00395 template<typename Derived>
00396 template<typename ResultType>
00397 inline void MatrixBase<Derived>::computeInverseWithCheck(
00398     ResultType& inverse,
00399     bool& invertible,
00400     const RealScalar& absDeterminantThreshold
00401   ) const
00402 {
00403   RealScalar determinant;
00404   // i'd love to put some static assertions there, but SFINAE means that they have no effect...
00405   eigen_assert(rows() == cols());
00406   computeInverseAndDetWithCheck(inverse,determinant,invertible,absDeterminantThreshold);
00407 }
00408 
00409 } // end namespace Eigen
00410 
00411 #endif // EIGEN_INVERSE_H