![]() |
Eigen
3.3.3
|
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