1 // This file is part of Eigen, a lightweight C++ template library 2 // for linear algebra. 3 // 4 // Copyright (C) 2008-2010 Benoit Jacob <jacob.benoit.1 (at) gmail.com> 5 // 6 // This Source Code Form is subject to the terms of the Mozilla 7 // Public License v. 2.0. If a copy of the MPL was not distributed 8 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 10 #ifndef EIGEN_INVERSE_H 11 #define EIGEN_INVERSE_H 12 13 namespace Eigen { 14 15 namespace internal { 16 17 /********************************** 18 *** General case implementation *** 19 **********************************/ 20 21 template<typename MatrixType, typename ResultType, int Size = MatrixType::RowsAtCompileTime> 22 struct compute_inverse 23 { 24 static inline void run(const MatrixType& matrix, ResultType& result) 25 { 26 result = matrix.partialPivLu().inverse(); 27 } 28 }; 29 30 template<typename MatrixType, typename ResultType, int Size = MatrixType::RowsAtCompileTime> 31 struct compute_inverse_and_det_with_check { /* nothing! general case not supported. */ }; 32 33 /**************************** 34 *** Size 1 implementation *** 35 ****************************/ 36 37 template<typename MatrixType, typename ResultType> 38 struct compute_inverse<MatrixType, ResultType, 1> 39 { 40 static inline void run(const MatrixType& matrix, ResultType& result) 41 { 42 typedef typename MatrixType::Scalar Scalar; 43 result.coeffRef(0,0) = Scalar(1) / matrix.coeff(0,0); 44 } 45 }; 46 47 template<typename MatrixType, typename ResultType> 48 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 1> 49 { 50 static inline void run( 51 const MatrixType& matrix, 52 const typename MatrixType::RealScalar& absDeterminantThreshold, 53 ResultType& result, 54 typename ResultType::Scalar& determinant, 55 bool& invertible 56 ) 57 { 58 determinant = matrix.coeff(0,0); 59 invertible = abs(determinant) > absDeterminantThreshold; 60 if(invertible) result.coeffRef(0,0) = typename ResultType::Scalar(1) / determinant; 61 } 62 }; 63 64 /**************************** 65 *** Size 2 implementation *** 66 ****************************/ 67 68 template<typename MatrixType, typename ResultType> 69 inline void compute_inverse_size2_helper( 70 const MatrixType& matrix, const typename ResultType::Scalar& invdet, 71 ResultType& result) 72 { 73 result.coeffRef(0,0) = matrix.coeff(1,1) * invdet; 74 result.coeffRef(1,0) = -matrix.coeff(1,0) * invdet; 75 result.coeffRef(0,1) = -matrix.coeff(0,1) * invdet; 76 result.coeffRef(1,1) = matrix.coeff(0,0) * invdet; 77 } 78 79 template<typename MatrixType, typename ResultType> 80 struct compute_inverse<MatrixType, ResultType, 2> 81 { 82 static inline void run(const MatrixType& matrix, ResultType& result) 83 { 84 typedef typename ResultType::Scalar Scalar; 85 const Scalar invdet = typename MatrixType::Scalar(1) / matrix.determinant(); 86 compute_inverse_size2_helper(matrix, invdet, result); 87 } 88 }; 89 90 template<typename MatrixType, typename ResultType> 91 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 2> 92 { 93 static inline void run( 94 const MatrixType& matrix, 95 const typename MatrixType::RealScalar& absDeterminantThreshold, 96 ResultType& inverse, 97 typename ResultType::Scalar& determinant, 98 bool& invertible 99 ) 100 { 101 typedef typename ResultType::Scalar Scalar; 102 determinant = matrix.determinant(); 103 invertible = abs(determinant) > absDeterminantThreshold; 104 if(!invertible) return; 105 const Scalar invdet = Scalar(1) / determinant; 106 compute_inverse_size2_helper(matrix, invdet, inverse); 107 } 108 }; 109 110 /**************************** 111 *** Size 3 implementation *** 112 ****************************/ 113 114 template<typename MatrixType, int i, int j> 115 inline typename MatrixType::Scalar cofactor_3x3(const MatrixType& m) 116 { 117 enum { 118 i1 = (i+1) % 3, 119 i2 = (i+2) % 3, 120 j1 = (j+1) % 3, 121 j2 = (j+2) % 3 122 }; 123 return m.coeff(i1, j1) * m.coeff(i2, j2) 124 - m.coeff(i1, j2) * m.coeff(i2, j1); 125 } 126 127 template<typename MatrixType, typename ResultType> 128 inline void compute_inverse_size3_helper( 129 const MatrixType& matrix, 130 const typename ResultType::Scalar& invdet, 131 const Matrix<typename ResultType::Scalar,3,1>& cofactors_col0, 132 ResultType& result) 133 { 134 result.row(0) = cofactors_col0 * invdet; 135 result.coeffRef(1,0) = cofactor_3x3<MatrixType,0,1>(matrix) * invdet; 136 result.coeffRef(1,1) = cofactor_3x3<MatrixType,1,1>(matrix) * invdet; 137 result.coeffRef(1,2) = cofactor_3x3<MatrixType,2,1>(matrix) * invdet; 138 result.coeffRef(2,0) = cofactor_3x3<MatrixType,0,2>(matrix) * invdet; 139 result.coeffRef(2,1) = cofactor_3x3<MatrixType,1,2>(matrix) * invdet; 140 result.coeffRef(2,2) = cofactor_3x3<MatrixType,2,2>(matrix) * invdet; 141 } 142 143 template<typename MatrixType, typename ResultType> 144 struct compute_inverse<MatrixType, ResultType, 3> 145 { 146 static inline void run(const MatrixType& matrix, ResultType& result) 147 { 148 typedef typename ResultType::Scalar Scalar; 149 Matrix<typename MatrixType::Scalar,3,1> cofactors_col0; 150 cofactors_col0.coeffRef(0) = cofactor_3x3<MatrixType,0,0>(matrix); 151 cofactors_col0.coeffRef(1) = cofactor_3x3<MatrixType,1,0>(matrix); 152 cofactors_col0.coeffRef(2) = cofactor_3x3<MatrixType,2,0>(matrix); 153 const Scalar det = (cofactors_col0.cwiseProduct(matrix.col(0))).sum(); 154 const Scalar invdet = Scalar(1) / det; 155 compute_inverse_size3_helper(matrix, invdet, cofactors_col0, result); 156 } 157 }; 158 159 template<typename MatrixType, typename ResultType> 160 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 3> 161 { 162 static inline void run( 163 const MatrixType& matrix, 164 const typename MatrixType::RealScalar& absDeterminantThreshold, 165 ResultType& inverse, 166 typename ResultType::Scalar& determinant, 167 bool& invertible 168 ) 169 { 170 typedef typename ResultType::Scalar Scalar; 171 Matrix<Scalar,3,1> cofactors_col0; 172 cofactors_col0.coeffRef(0) = cofactor_3x3<MatrixType,0,0>(matrix); 173 cofactors_col0.coeffRef(1) = cofactor_3x3<MatrixType,1,0>(matrix); 174 cofactors_col0.coeffRef(2) = cofactor_3x3<MatrixType,2,0>(matrix); 175 determinant = (cofactors_col0.cwiseProduct(matrix.col(0))).sum(); 176 invertible = abs(determinant) > absDeterminantThreshold; 177 if(!invertible) return; 178 const Scalar invdet = Scalar(1) / determinant; 179 compute_inverse_size3_helper(matrix, invdet, cofactors_col0, inverse); 180 } 181 }; 182 183 /**************************** 184 *** Size 4 implementation *** 185 ****************************/ 186 187 template<typename Derived> 188 inline const typename Derived::Scalar general_det3_helper 189 (const MatrixBase<Derived>& matrix, int i1, int i2, int i3, int j1, int j2, int j3) 190 { 191 return matrix.coeff(i1,j1) 192 * (matrix.coeff(i2,j2) * matrix.coeff(i3,j3) - matrix.coeff(i2,j3) * matrix.coeff(i3,j2)); 193 } 194 195 template<typename MatrixType, int i, int j> 196 inline typename MatrixType::Scalar cofactor_4x4(const MatrixType& matrix) 197 { 198 enum { 199 i1 = (i+1) % 4, 200 i2 = (i+2) % 4, 201 i3 = (i+3) % 4, 202 j1 = (j+1) % 4, 203 j2 = (j+2) % 4, 204 j3 = (j+3) % 4 205 }; 206 return general_det3_helper(matrix, i1, i2, i3, j1, j2, j3) 207 + general_det3_helper(matrix, i2, i3, i1, j1, j2, j3) 208 + general_det3_helper(matrix, i3, i1, i2, j1, j2, j3); 209 } 210 211 template<int Arch, typename Scalar, typename MatrixType, typename ResultType> 212 struct compute_inverse_size4 213 { 214 static void run(const MatrixType& matrix, ResultType& result) 215 { 216 result.coeffRef(0,0) = cofactor_4x4<MatrixType,0,0>(matrix); 217 result.coeffRef(1,0) = -cofactor_4x4<MatrixType,0,1>(matrix); 218 result.coeffRef(2,0) = cofactor_4x4<MatrixType,0,2>(matrix); 219 result.coeffRef(3,0) = -cofactor_4x4<MatrixType,0,3>(matrix); 220 result.coeffRef(0,2) = cofactor_4x4<MatrixType,2,0>(matrix); 221 result.coeffRef(1,2) = -cofactor_4x4<MatrixType,2,1>(matrix); 222 result.coeffRef(2,2) = cofactor_4x4<MatrixType,2,2>(matrix); 223 result.coeffRef(3,2) = -cofactor_4x4<MatrixType,2,3>(matrix); 224 result.coeffRef(0,1) = -cofactor_4x4<MatrixType,1,0>(matrix); 225 result.coeffRef(1,1) = cofactor_4x4<MatrixType,1,1>(matrix); 226 result.coeffRef(2,1) = -cofactor_4x4<MatrixType,1,2>(matrix); 227 result.coeffRef(3,1) = cofactor_4x4<MatrixType,1,3>(matrix); 228 result.coeffRef(0,3) = -cofactor_4x4<MatrixType,3,0>(matrix); 229 result.coeffRef(1,3) = cofactor_4x4<MatrixType,3,1>(matrix); 230 result.coeffRef(2,3) = -cofactor_4x4<MatrixType,3,2>(matrix); 231 result.coeffRef(3,3) = cofactor_4x4<MatrixType,3,3>(matrix); 232 result /= (matrix.col(0).cwiseProduct(result.row(0).transpose())).sum(); 233 } 234 }; 235 236 template<typename MatrixType, typename ResultType> 237 struct compute_inverse<MatrixType, ResultType, 4> 238 : compute_inverse_size4<Architecture::Target, typename MatrixType::Scalar, 239 MatrixType, ResultType> 240 { 241 }; 242 243 template<typename MatrixType, typename ResultType> 244 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 4> 245 { 246 static inline void run( 247 const MatrixType& matrix, 248 const typename MatrixType::RealScalar& absDeterminantThreshold, 249 ResultType& inverse, 250 typename ResultType::Scalar& determinant, 251 bool& invertible 252 ) 253 { 254 determinant = matrix.determinant(); 255 invertible = abs(determinant) > absDeterminantThreshold; 256 if(invertible) compute_inverse<MatrixType, ResultType>::run(matrix, inverse); 257 } 258 }; 259 260 /************************* 261 *** MatrixBase methods *** 262 *************************/ 263 264 template<typename MatrixType> 265 struct traits<inverse_impl<MatrixType> > 266 { 267 typedef typename MatrixType::PlainObject ReturnType; 268 }; 269 270 template<typename MatrixType> 271 struct inverse_impl : public ReturnByValue<inverse_impl<MatrixType> > 272 { 273 typedef typename MatrixType::Index Index; 274 typedef typename internal::eval<MatrixType>::type MatrixTypeNested; 275 typedef typename remove_all<MatrixTypeNested>::type MatrixTypeNestedCleaned; 276 MatrixTypeNested m_matrix; 277 278 inverse_impl(const MatrixType& matrix) 279 : m_matrix(matrix) 280 {} 281 282 inline Index rows() const { return m_matrix.rows(); } 283 inline Index cols() const { return m_matrix.cols(); } 284 285 template<typename Dest> inline void evalTo(Dest& dst) const 286 { 287 const int Size = EIGEN_PLAIN_ENUM_MIN(MatrixType::ColsAtCompileTime,Dest::ColsAtCompileTime); 288 EIGEN_ONLY_USED_FOR_DEBUG(Size); 289 eigen_assert(( (Size<=1) || (Size>4) || (extract_data(m_matrix)!=extract_data(dst))) 290 && "Aliasing problem detected in inverse(), you need to do inverse().eval() here."); 291 292 compute_inverse<MatrixTypeNestedCleaned, Dest>::run(m_matrix, dst); 293 } 294 }; 295 296 } // end namespace internal 297 298 /** \lu_module 299 * 300 * \returns the matrix inverse of this matrix. 301 * 302 * For small fixed sizes up to 4x4, this method uses cofactors. 303 * In the general case, this method uses class PartialPivLU. 304 * 305 * \note This matrix must be invertible, otherwise the result is undefined. If you need an 306 * invertibility check, do the following: 307 * \li for fixed sizes up to 4x4, use computeInverseAndDetWithCheck(). 308 * \li for the general case, use class FullPivLU. 309 * 310 * Example: \include MatrixBase_inverse.cpp 311 * Output: \verbinclude MatrixBase_inverse.out 312 * 313 * \sa computeInverseAndDetWithCheck() 314 */ 315 template<typename Derived> 316 inline const internal::inverse_impl<Derived> MatrixBase<Derived>::inverse() const 317 { 318 EIGEN_STATIC_ASSERT(!NumTraits<Scalar>::IsInteger,THIS_FUNCTION_IS_NOT_FOR_INTEGER_NUMERIC_TYPES) 319 eigen_assert(rows() == cols()); 320 return internal::inverse_impl<Derived>(derived()); 321 } 322 323 /** \lu_module 324 * 325 * Computation of matrix inverse and determinant, with invertibility check. 326 * 327 * This is only for fixed-size square matrices of size up to 4x4. 328 * 329 * \param inverse Reference to the matrix in which to store the inverse. 330 * \param determinant Reference to the variable in which to store the inverse. 331 * \param invertible Reference to the bool variable in which to store whether the matrix is invertible. 332 * \param absDeterminantThreshold Optional parameter controlling the invertibility check. 333 * The matrix will be declared invertible if the absolute value of its 334 * determinant is greater than this threshold. 335 * 336 * Example: \include MatrixBase_computeInverseAndDetWithCheck.cpp 337 * Output: \verbinclude MatrixBase_computeInverseAndDetWithCheck.out 338 * 339 * \sa inverse(), computeInverseWithCheck() 340 */ 341 template<typename Derived> 342 template<typename ResultType> 343 inline void MatrixBase<Derived>::computeInverseAndDetWithCheck( 344 ResultType& inverse, 345 typename ResultType::Scalar& determinant, 346 bool& invertible, 347 const RealScalar& absDeterminantThreshold 348 ) const 349 { 350 // i'd love to put some static assertions there, but SFINAE means that they have no effect... 351 eigen_assert(rows() == cols()); 352 // for 2x2, it's worth giving a chance to avoid evaluating. 353 // for larger sizes, evaluating has negligible cost and limits code size. 354 typedef typename internal::conditional< 355 RowsAtCompileTime == 2, 356 typename internal::remove_all<typename internal::nested<Derived, 2>::type>::type, 357 PlainObject 358 >::type MatrixType; 359 internal::compute_inverse_and_det_with_check<MatrixType, ResultType>::run 360 (derived(), absDeterminantThreshold, inverse, determinant, invertible); 361 } 362 363 /** \lu_module 364 * 365 * Computation of matrix inverse, with invertibility check. 366 * 367 * This is only for fixed-size square matrices of size up to 4x4. 368 * 369 * \param inverse Reference to the matrix in which to store the inverse. 370 * \param invertible Reference to the bool variable in which to store whether the matrix is invertible. 371 * \param absDeterminantThreshold Optional parameter controlling the invertibility check. 372 * The matrix will be declared invertible if the absolute value of its 373 * determinant is greater than this threshold. 374 * 375 * Example: \include MatrixBase_computeInverseWithCheck.cpp 376 * Output: \verbinclude MatrixBase_computeInverseWithCheck.out 377 * 378 * \sa inverse(), computeInverseAndDetWithCheck() 379 */ 380 template<typename Derived> 381 template<typename ResultType> 382 inline void MatrixBase<Derived>::computeInverseWithCheck( 383 ResultType& inverse, 384 bool& invertible, 385 const RealScalar& absDeterminantThreshold 386 ) const 387 { 388 RealScalar determinant; 389 // i'd love to put some static assertions there, but SFINAE means that they have no effect... 390 eigen_assert(rows() == cols()); 391 computeInverseAndDetWithCheck(inverse,determinant,invertible,absDeterminantThreshold); 392 } 393 394 } // end namespace Eigen 395 396 #endif // EIGEN_INVERSE_H 397
