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