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