$treeview $search $mathjax
Eigen
3.2.5
$projectbrief
|
$projectbrief
|
$searchbox |
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 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 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 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