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