Eigen  3.3.3
InverseImpl.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 // Copyright (C) 2014 Gael Guennebaud <gael.guennebaud@inria.fr>
00006 //
00007 // This Source Code Form is subject to the terms of the Mozilla
00008 // Public License v. 2.0. If a copy of the MPL was not distributed
00009 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
00010 
00011 #ifndef EIGEN_INVERSE_IMPL_H
00012 #define EIGEN_INVERSE_IMPL_H
00013 
00014 namespace Eigen { 
00015 
00016 namespace internal {
00017 
00018 /**********************************
00019 *** General case implementation ***
00020 **********************************/
00021 
00022 template<typename MatrixType, typename ResultType, int Size = MatrixType::RowsAtCompileTime>
00023 struct compute_inverse
00024 {
00025   EIGEN_DEVICE_FUNC
00026   static inline void run(const MatrixType& matrix, ResultType& result)
00027   {
00028     result = matrix.partialPivLu().inverse();
00029   }
00030 };
00031 
00032 template<typename MatrixType, typename ResultType, int Size = MatrixType::RowsAtCompileTime>
00033 struct compute_inverse_and_det_with_check { /* nothing! general case not supported. */ };
00034 
00035 /****************************
00036 *** Size 1 implementation ***
00037 ****************************/
00038 
00039 template<typename MatrixType, typename ResultType>
00040 struct compute_inverse<MatrixType, ResultType, 1>
00041 {
00042   EIGEN_DEVICE_FUNC
00043   static inline void run(const MatrixType& matrix, ResultType& result)
00044   {
00045     typedef typename MatrixType::Scalar Scalar;
00046     internal::evaluator<MatrixType> matrixEval(matrix);
00047     result.coeffRef(0,0) = Scalar(1) / matrixEval.coeff(0,0);
00048   }
00049 };
00050 
00051 template<typename MatrixType, typename ResultType>
00052 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 1>
00053 {
00054   EIGEN_DEVICE_FUNC
00055   static inline void run(
00056     const MatrixType& matrix,
00057     const typename MatrixType::RealScalar& absDeterminantThreshold,
00058     ResultType& result,
00059     typename ResultType::Scalar& determinant,
00060     bool& invertible
00061   )
00062   {
00063     using std::abs;
00064     determinant = matrix.coeff(0,0);
00065     invertible = abs(determinant) > absDeterminantThreshold;
00066     if(invertible) result.coeffRef(0,0) = typename ResultType::Scalar(1) / determinant;
00067   }
00068 };
00069 
00070 /****************************
00071 *** Size 2 implementation ***
00072 ****************************/
00073 
00074 template<typename MatrixType, typename ResultType>
00075 EIGEN_DEVICE_FUNC 
00076 inline void compute_inverse_size2_helper(
00077     const MatrixType& matrix, const typename ResultType::Scalar& invdet,
00078     ResultType& result)
00079 {
00080   result.coeffRef(0,0) =  matrix.coeff(1,1) * invdet;
00081   result.coeffRef(1,0) = -matrix.coeff(1,0) * invdet;
00082   result.coeffRef(0,1) = -matrix.coeff(0,1) * invdet;
00083   result.coeffRef(1,1) =  matrix.coeff(0,0) * invdet;
00084 }
00085 
00086 template<typename MatrixType, typename ResultType>
00087 struct compute_inverse<MatrixType, ResultType, 2>
00088 {
00089   EIGEN_DEVICE_FUNC
00090   static inline void run(const MatrixType& matrix, ResultType& result)
00091   {
00092     typedef typename ResultType::Scalar Scalar;
00093     const Scalar invdet = typename MatrixType::Scalar(1) / matrix.determinant();
00094     compute_inverse_size2_helper(matrix, invdet, result);
00095   }
00096 };
00097 
00098 template<typename MatrixType, typename ResultType>
00099 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 2>
00100 {
00101   EIGEN_DEVICE_FUNC
00102   static inline void run(
00103     const MatrixType& matrix,
00104     const typename MatrixType::RealScalar& absDeterminantThreshold,
00105     ResultType& inverse,
00106     typename ResultType::Scalar& determinant,
00107     bool& invertible
00108   )
00109   {
00110     using std::abs;
00111     typedef typename ResultType::Scalar Scalar;
00112     determinant = matrix.determinant();
00113     invertible = abs(determinant) > absDeterminantThreshold;
00114     if(!invertible) return;
00115     const Scalar invdet = Scalar(1) / determinant;
00116     compute_inverse_size2_helper(matrix, invdet, inverse);
00117   }
00118 };
00119 
00120 /****************************
00121 *** Size 3 implementation ***
00122 ****************************/
00123 
00124 template<typename MatrixType, int i, int j>
00125 EIGEN_DEVICE_FUNC 
00126 inline typename MatrixType::Scalar cofactor_3x3(const MatrixType& m)
00127 {
00128   enum {
00129     i1 = (i+1) % 3,
00130     i2 = (i+2) % 3,
00131     j1 = (j+1) % 3,
00132     j2 = (j+2) % 3
00133   };
00134   return m.coeff(i1, j1) * m.coeff(i2, j2)
00135        - m.coeff(i1, j2) * m.coeff(i2, j1);
00136 }
00137 
00138 template<typename MatrixType, typename ResultType>
00139 EIGEN_DEVICE_FUNC
00140 inline void compute_inverse_size3_helper(
00141     const MatrixType& matrix,
00142     const typename ResultType::Scalar& invdet,
00143     const Matrix<typename ResultType::Scalar,3,1>& cofactors_col0,
00144     ResultType& result)
00145 {
00146   result.row(0) = cofactors_col0 * invdet;
00147   result.coeffRef(1,0) =  cofactor_3x3<MatrixType,0,1>(matrix) * invdet;
00148   result.coeffRef(1,1) =  cofactor_3x3<MatrixType,1,1>(matrix) * invdet;
00149   result.coeffRef(1,2) =  cofactor_3x3<MatrixType,2,1>(matrix) * invdet;
00150   result.coeffRef(2,0) =  cofactor_3x3<MatrixType,0,2>(matrix) * invdet;
00151   result.coeffRef(2,1) =  cofactor_3x3<MatrixType,1,2>(matrix) * invdet;
00152   result.coeffRef(2,2) =  cofactor_3x3<MatrixType,2,2>(matrix) * invdet;
00153 }
00154 
00155 template<typename MatrixType, typename ResultType>
00156 struct compute_inverse<MatrixType, ResultType, 3>
00157 {
00158   EIGEN_DEVICE_FUNC
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   EIGEN_DEVICE_FUNC
00176   static inline void run(
00177     const MatrixType& matrix,
00178     const typename MatrixType::RealScalar& absDeterminantThreshold,
00179     ResultType& inverse,
00180     typename ResultType::Scalar& determinant,
00181     bool& invertible
00182   )
00183   {
00184     using std::abs;
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 EIGEN_DEVICE_FUNC 
00204 inline const typename Derived::Scalar general_det3_helper
00205 (const MatrixBase<Derived>& matrix, int i1, int i2, int i3, int j1, int j2, int j3)
00206 {
00207   return matrix.coeff(i1,j1)
00208          * (matrix.coeff(i2,j2) * matrix.coeff(i3,j3) - matrix.coeff(i2,j3) * matrix.coeff(i3,j2));
00209 }
00210 
00211 template<typename MatrixType, int i, int j>
00212 EIGEN_DEVICE_FUNC 
00213 inline typename MatrixType::Scalar cofactor_4x4(const MatrixType& matrix)
00214 {
00215   enum {
00216     i1 = (i+1) % 4,
00217     i2 = (i+2) % 4,
00218     i3 = (i+3) % 4,
00219     j1 = (j+1) % 4,
00220     j2 = (j+2) % 4,
00221     j3 = (j+3) % 4
00222   };
00223   return general_det3_helper(matrix, i1, i2, i3, j1, j2, j3)
00224        + general_det3_helper(matrix, i2, i3, i1, j1, j2, j3)
00225        + general_det3_helper(matrix, i3, i1, i2, j1, j2, j3);
00226 }
00227 
00228 template<int Arch, typename Scalar, typename MatrixType, typename ResultType>
00229 struct compute_inverse_size4
00230 {
00231   EIGEN_DEVICE_FUNC
00232   static void run(const MatrixType& matrix, ResultType& result)
00233   {
00234     result.coeffRef(0,0) =  cofactor_4x4<MatrixType,0,0>(matrix);
00235     result.coeffRef(1,0) = -cofactor_4x4<MatrixType,0,1>(matrix);
00236     result.coeffRef(2,0) =  cofactor_4x4<MatrixType,0,2>(matrix);
00237     result.coeffRef(3,0) = -cofactor_4x4<MatrixType,0,3>(matrix);
00238     result.coeffRef(0,2) =  cofactor_4x4<MatrixType,2,0>(matrix);
00239     result.coeffRef(1,2) = -cofactor_4x4<MatrixType,2,1>(matrix);
00240     result.coeffRef(2,2) =  cofactor_4x4<MatrixType,2,2>(matrix);
00241     result.coeffRef(3,2) = -cofactor_4x4<MatrixType,2,3>(matrix);
00242     result.coeffRef(0,1) = -cofactor_4x4<MatrixType,1,0>(matrix);
00243     result.coeffRef(1,1) =  cofactor_4x4<MatrixType,1,1>(matrix);
00244     result.coeffRef(2,1) = -cofactor_4x4<MatrixType,1,2>(matrix);
00245     result.coeffRef(3,1) =  cofactor_4x4<MatrixType,1,3>(matrix);
00246     result.coeffRef(0,3) = -cofactor_4x4<MatrixType,3,0>(matrix);
00247     result.coeffRef(1,3) =  cofactor_4x4<MatrixType,3,1>(matrix);
00248     result.coeffRef(2,3) = -cofactor_4x4<MatrixType,3,2>(matrix);
00249     result.coeffRef(3,3) =  cofactor_4x4<MatrixType,3,3>(matrix);
00250     result /= (matrix.col(0).cwiseProduct(result.row(0).transpose())).sum();
00251   }
00252 };
00253 
00254 template<typename MatrixType, typename ResultType>
00255 struct compute_inverse<MatrixType, ResultType, 4>
00256  : compute_inverse_size4<Architecture::Target, typename MatrixType::Scalar,
00257                             MatrixType, ResultType>
00258 {
00259 };
00260 
00261 template<typename MatrixType, typename ResultType>
00262 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 4>
00263 {
00264   EIGEN_DEVICE_FUNC
00265   static inline void run(
00266     const MatrixType& matrix,
00267     const typename MatrixType::RealScalar& absDeterminantThreshold,
00268     ResultType& inverse,
00269     typename ResultType::Scalar& determinant,
00270     bool& invertible
00271   )
00272   {
00273     using std::abs;
00274     determinant = matrix.determinant();
00275     invertible = abs(determinant) > absDeterminantThreshold;
00276     if(invertible) compute_inverse<MatrixType, ResultType>::run(matrix, inverse);
00277   }
00278 };
00279 
00280 /*************************
00281 *** MatrixBase methods ***
00282 *************************/
00283 
00284 } // end namespace internal
00285 
00286 namespace internal {
00287 
00288 // Specialization for "dense = dense_xpr.inverse()"
00289 template<typename DstXprType, typename XprType>
00290 struct Assignment<DstXprType, Inverse<XprType>, internal::assign_op<typename DstXprType::Scalar,typename XprType::Scalar>, Dense2Dense>
00291 {
00292   typedef Inverse<XprType> SrcXprType;
00293   static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<typename DstXprType::Scalar,typename XprType::Scalar> &)
00294   {
00295     Index dstRows = src.rows();
00296     Index dstCols = src.cols();
00297     if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
00298       dst.resize(dstRows, dstCols);
00299     
00300     const int Size = EIGEN_PLAIN_ENUM_MIN(XprType::ColsAtCompileTime,DstXprType::ColsAtCompileTime);
00301     EIGEN_ONLY_USED_FOR_DEBUG(Size);
00302     eigen_assert(( (Size<=1) || (Size>4) || (extract_data(src.nestedExpression())!=extract_data(dst)))
00303               && "Aliasing problem detected in inverse(), you need to do inverse().eval() here.");
00304 
00305     typedef typename internal::nested_eval<XprType,XprType::ColsAtCompileTime>::type  ActualXprType;
00306     typedef typename internal::remove_all<ActualXprType>::type                        ActualXprTypeCleanded;
00307     
00308     ActualXprType actual_xpr(src.nestedExpression());
00309     
00310     compute_inverse<ActualXprTypeCleanded, DstXprType>::run(actual_xpr, dst);
00311   }
00312 };
00313 
00314   
00315 } // end namespace internal
00316 
00334 template<typename Derived>
00335 inline const Inverse<Derived> MatrixBase<Derived>::inverse() const
00336 {
00337   EIGEN_STATIC_ASSERT(!NumTraits<Scalar>::IsInteger,THIS_FUNCTION_IS_NOT_FOR_INTEGER_NUMERIC_TYPES)
00338   eigen_assert(rows() == cols());
00339   return Inverse<Derived>(derived());
00340 }
00341 
00360 template<typename Derived>
00361 template<typename ResultType>
00362 inline void MatrixBase<Derived>::computeInverseAndDetWithCheck(
00363     ResultType& inverse,
00364     typename ResultType::Scalar& determinant,
00365     bool& invertible,
00366     const RealScalar& absDeterminantThreshold
00367   ) const
00368 {
00369   // i'd love to put some static assertions there, but SFINAE means that they have no effect...
00370   eigen_assert(rows() == cols());
00371   // for 2x2, it's worth giving a chance to avoid evaluating.
00372   // for larger sizes, evaluating has negligible cost and limits code size.
00373   typedef typename internal::conditional<
00374     RowsAtCompileTime == 2,
00375     typename internal::remove_all<typename internal::nested_eval<Derived, 2>::type>::type,
00376     PlainObject
00377   >::type MatrixType;
00378   internal::compute_inverse_and_det_with_check<MatrixType, ResultType>::run
00379     (derived(), absDeterminantThreshold, inverse, determinant, invertible);
00380 }
00381 
00399 template<typename Derived>
00400 template<typename ResultType>
00401 inline void MatrixBase<Derived>::computeInverseWithCheck(
00402     ResultType& inverse,
00403     bool& invertible,
00404     const RealScalar& absDeterminantThreshold
00405   ) const
00406 {
00407   RealScalar determinant;
00408   // i'd love to put some static assertions there, but SFINAE means that they have no effect...
00409   eigen_assert(rows() == cols());
00410   computeInverseAndDetWithCheck(inverse,determinant,invertible,absDeterminantThreshold);
00411 }
00412 
00413 } // end namespace Eigen
00414 
00415 #endif // EIGEN_INVERSE_IMPL_H
 All Classes Functions Variables Typedefs Enumerations Enumerator Friends