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


libicr
Author(s): Robert Krug
autogenerated on Mon Jan 6 2014 11:32:51