| // This file is part of Eigen, a lightweight C++ template library |
| // for linear algebra. |
| // |
| // Copyright (C) 2008-2010 Benoit Jacob <jacob.benoit.1@gmail.com> |
| // Copyright (C) 2014 Gael Guennebaud <gael.guennebaud@inria.fr> |
| // |
| // This Source Code Form is subject to the terms of the Mozilla |
| // Public License v. 2.0. If a copy of the MPL was not distributed |
| // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. |
| |
| #ifndef EIGEN_INVERSE_IMPL_H |
| #define EIGEN_INVERSE_IMPL_H |
| |
| #include "./InternalHeaderCheck.h" |
| |
| namespace Eigen { |
| |
| namespace internal { |
| |
| /********************************** |
| *** General case implementation *** |
| **********************************/ |
| |
| template<typename MatrixType, typename ResultType, int Size = MatrixType::RowsAtCompileTime> |
| struct compute_inverse |
| { |
| EIGEN_DEVICE_FUNC |
| static inline void run(const MatrixType& matrix, ResultType& result) |
| { |
| result = matrix.partialPivLu().inverse(); |
| } |
| }; |
| |
| template<typename MatrixType, typename ResultType, int Size = MatrixType::RowsAtCompileTime> |
| struct compute_inverse_and_det_with_check { /* nothing! general case not supported. */ }; |
| |
| /**************************** |
| *** Size 1 implementation *** |
| ****************************/ |
| |
| template<typename MatrixType, typename ResultType> |
| struct compute_inverse<MatrixType, ResultType, 1> |
| { |
| EIGEN_DEVICE_FUNC |
| static inline void run(const MatrixType& matrix, ResultType& result) |
| { |
| typedef typename MatrixType::Scalar Scalar; |
| internal::evaluator<MatrixType> matrixEval(matrix); |
| result.coeffRef(0,0) = Scalar(1) / matrixEval.coeff(0,0); |
| } |
| }; |
| |
| template<typename MatrixType, typename ResultType> |
| struct compute_inverse_and_det_with_check<MatrixType, ResultType, 1> |
| { |
| EIGEN_DEVICE_FUNC |
| static inline void run( |
| const MatrixType& matrix, |
| const typename MatrixType::RealScalar& absDeterminantThreshold, |
| ResultType& result, |
| typename ResultType::Scalar& determinant, |
| bool& invertible |
| ) |
| { |
| using std::abs; |
| determinant = matrix.coeff(0,0); |
| invertible = abs(determinant) > absDeterminantThreshold; |
| if(invertible) result.coeffRef(0,0) = typename ResultType::Scalar(1) / determinant; |
| } |
| }; |
| |
| /**************************** |
| *** Size 2 implementation *** |
| ****************************/ |
| |
| template<typename MatrixType, typename ResultType> |
| EIGEN_DEVICE_FUNC |
| inline void compute_inverse_size2_helper( |
| const MatrixType& matrix, const typename ResultType::Scalar& invdet, |
| ResultType& result) |
| { |
| typename ResultType::Scalar temp = matrix.coeff(0,0); |
| result.coeffRef(0,0) = matrix.coeff(1,1) * invdet; |
| result.coeffRef(1,0) = -matrix.coeff(1,0) * invdet; |
| result.coeffRef(0,1) = -matrix.coeff(0,1) * invdet; |
| result.coeffRef(1,1) = temp * invdet; |
| } |
| |
| template<typename MatrixType, typename ResultType> |
| struct compute_inverse<MatrixType, ResultType, 2> |
| { |
| EIGEN_DEVICE_FUNC |
| static inline void run(const MatrixType& matrix, ResultType& result) |
| { |
| typedef typename ResultType::Scalar Scalar; |
| const Scalar invdet = typename MatrixType::Scalar(1) / matrix.determinant(); |
| compute_inverse_size2_helper(matrix, invdet, result); |
| } |
| }; |
| |
| template<typename MatrixType, typename ResultType> |
| struct compute_inverse_and_det_with_check<MatrixType, ResultType, 2> |
| { |
| EIGEN_DEVICE_FUNC |
| static inline void run( |
| const MatrixType& matrix, |
| const typename MatrixType::RealScalar& absDeterminantThreshold, |
| ResultType& inverse, |
| typename ResultType::Scalar& determinant, |
| bool& invertible |
| ) |
| { |
| using std::abs; |
| typedef typename ResultType::Scalar Scalar; |
| determinant = matrix.determinant(); |
| invertible = abs(determinant) > absDeterminantThreshold; |
| if(!invertible) return; |
| const Scalar invdet = Scalar(1) / determinant; |
| compute_inverse_size2_helper(matrix, invdet, inverse); |
| } |
| }; |
| |
| /**************************** |
| *** Size 3 implementation *** |
| ****************************/ |
| |
| template<typename MatrixType, int i, int j> |
| EIGEN_DEVICE_FUNC |
| inline typename MatrixType::Scalar cofactor_3x3(const MatrixType& m) |
| { |
| enum { |
| i1 = (i+1) % 3, |
| i2 = (i+2) % 3, |
| j1 = (j+1) % 3, |
| j2 = (j+2) % 3 |
| }; |
| return m.coeff(i1, j1) * m.coeff(i2, j2) |
| - m.coeff(i1, j2) * m.coeff(i2, j1); |
| } |
| |
| template<typename MatrixType, typename ResultType> |
| EIGEN_DEVICE_FUNC |
| inline void compute_inverse_size3_helper( |
| const MatrixType& matrix, |
| const typename ResultType::Scalar& invdet, |
| const Matrix<typename ResultType::Scalar,3,1>& cofactors_col0, |
| ResultType& result) |
| { |
| // Compute cofactors in a way that avoids aliasing issues. |
| typedef typename ResultType::Scalar Scalar; |
| const Scalar c01 = cofactor_3x3<MatrixType,0,1>(matrix) * invdet; |
| const Scalar c11 = cofactor_3x3<MatrixType,1,1>(matrix) * invdet; |
| const Scalar c02 = cofactor_3x3<MatrixType,0,2>(matrix) * invdet; |
| result.coeffRef(1,2) = cofactor_3x3<MatrixType,2,1>(matrix) * invdet; |
| result.coeffRef(2,1) = cofactor_3x3<MatrixType,1,2>(matrix) * invdet; |
| result.coeffRef(2,2) = cofactor_3x3<MatrixType,2,2>(matrix) * invdet; |
| result.coeffRef(1,0) = c01; |
| result.coeffRef(1,1) = c11; |
| result.coeffRef(2,0) = c02; |
| result.row(0) = cofactors_col0 * invdet; |
| } |
| |
| template<typename MatrixType, typename ResultType> |
| struct compute_inverse<MatrixType, ResultType, 3> |
| { |
| EIGEN_DEVICE_FUNC |
| static inline void run(const MatrixType& matrix, ResultType& result) |
| { |
| typedef typename ResultType::Scalar Scalar; |
| Matrix<typename MatrixType::Scalar,3,1> cofactors_col0; |
| cofactors_col0.coeffRef(0) = cofactor_3x3<MatrixType,0,0>(matrix); |
| cofactors_col0.coeffRef(1) = cofactor_3x3<MatrixType,1,0>(matrix); |
| cofactors_col0.coeffRef(2) = cofactor_3x3<MatrixType,2,0>(matrix); |
| const Scalar det = (cofactors_col0.cwiseProduct(matrix.col(0))).sum(); |
| const Scalar invdet = Scalar(1) / det; |
| compute_inverse_size3_helper(matrix, invdet, cofactors_col0, result); |
| } |
| }; |
| |
| template<typename MatrixType, typename ResultType> |
| struct compute_inverse_and_det_with_check<MatrixType, ResultType, 3> |
| { |
| EIGEN_DEVICE_FUNC |
| static inline void run( |
| const MatrixType& matrix, |
| const typename MatrixType::RealScalar& absDeterminantThreshold, |
| ResultType& inverse, |
| typename ResultType::Scalar& determinant, |
| bool& invertible |
| ) |
| { |
| typedef typename ResultType::Scalar Scalar; |
| Matrix<Scalar,3,1> cofactors_col0; |
| cofactors_col0.coeffRef(0) = cofactor_3x3<MatrixType,0,0>(matrix); |
| cofactors_col0.coeffRef(1) = cofactor_3x3<MatrixType,1,0>(matrix); |
| cofactors_col0.coeffRef(2) = cofactor_3x3<MatrixType,2,0>(matrix); |
| determinant = (cofactors_col0.cwiseProduct(matrix.col(0))).sum(); |
| invertible = Eigen::numext::abs(determinant) > absDeterminantThreshold; |
| if(!invertible) return; |
| const Scalar invdet = Scalar(1) / determinant; |
| compute_inverse_size3_helper(matrix, invdet, cofactors_col0, inverse); |
| } |
| }; |
| |
| /**************************** |
| *** Size 4 implementation *** |
| ****************************/ |
| |
| template<typename Derived> |
| EIGEN_DEVICE_FUNC |
| inline const typename Derived::Scalar general_det3_helper |
| (const MatrixBase<Derived>& matrix, int i1, int i2, int i3, int j1, int j2, int j3) |
| { |
| return matrix.coeff(i1,j1) |
| * (matrix.coeff(i2,j2) * matrix.coeff(i3,j3) - matrix.coeff(i2,j3) * matrix.coeff(i3,j2)); |
| } |
| |
| template<typename MatrixType, int i, int j> |
| EIGEN_DEVICE_FUNC |
| inline typename MatrixType::Scalar cofactor_4x4(const MatrixType& matrix) |
| { |
| enum { |
| i1 = (i+1) % 4, |
| i2 = (i+2) % 4, |
| i3 = (i+3) % 4, |
| j1 = (j+1) % 4, |
| j2 = (j+2) % 4, |
| j3 = (j+3) % 4 |
| }; |
| return general_det3_helper(matrix, i1, i2, i3, j1, j2, j3) |
| + general_det3_helper(matrix, i2, i3, i1, j1, j2, j3) |
| + general_det3_helper(matrix, i3, i1, i2, j1, j2, j3); |
| } |
| |
| template<int Arch, typename Scalar, typename MatrixType, typename ResultType> |
| struct compute_inverse_size4 |
| { |
| EIGEN_DEVICE_FUNC |
| static void run(const MatrixType& matrix, ResultType& result) |
| { |
| result.coeffRef(0,0) = cofactor_4x4<MatrixType,0,0>(matrix); |
| result.coeffRef(1,0) = -cofactor_4x4<MatrixType,0,1>(matrix); |
| result.coeffRef(2,0) = cofactor_4x4<MatrixType,0,2>(matrix); |
| result.coeffRef(3,0) = -cofactor_4x4<MatrixType,0,3>(matrix); |
| result.coeffRef(0,2) = cofactor_4x4<MatrixType,2,0>(matrix); |
| result.coeffRef(1,2) = -cofactor_4x4<MatrixType,2,1>(matrix); |
| result.coeffRef(2,2) = cofactor_4x4<MatrixType,2,2>(matrix); |
| result.coeffRef(3,2) = -cofactor_4x4<MatrixType,2,3>(matrix); |
| result.coeffRef(0,1) = -cofactor_4x4<MatrixType,1,0>(matrix); |
| result.coeffRef(1,1) = cofactor_4x4<MatrixType,1,1>(matrix); |
| result.coeffRef(2,1) = -cofactor_4x4<MatrixType,1,2>(matrix); |
| result.coeffRef(3,1) = cofactor_4x4<MatrixType,1,3>(matrix); |
| result.coeffRef(0,3) = -cofactor_4x4<MatrixType,3,0>(matrix); |
| result.coeffRef(1,3) = cofactor_4x4<MatrixType,3,1>(matrix); |
| result.coeffRef(2,3) = -cofactor_4x4<MatrixType,3,2>(matrix); |
| result.coeffRef(3,3) = cofactor_4x4<MatrixType,3,3>(matrix); |
| result /= (matrix.col(0).cwiseProduct(result.row(0).transpose())).sum(); |
| } |
| }; |
| |
| template<typename MatrixType, typename ResultType> |
| struct compute_inverse<MatrixType, ResultType, 4> |
| : compute_inverse_size4<Architecture::Target, typename MatrixType::Scalar, |
| MatrixType, ResultType> |
| { |
| }; |
| |
| template<typename MatrixType, typename ResultType> |
| struct compute_inverse_and_det_with_check<MatrixType, ResultType, 4> |
| { |
| EIGEN_DEVICE_FUNC |
| static inline void run( |
| const MatrixType& matrix, |
| const typename MatrixType::RealScalar& absDeterminantThreshold, |
| ResultType& inverse, |
| typename ResultType::Scalar& determinant, |
| bool& invertible |
| ) |
| { |
| using std::abs; |
| determinant = matrix.determinant(); |
| invertible = abs(determinant) > absDeterminantThreshold; |
| if(invertible && extract_data(matrix) != extract_data(inverse)) { |
| compute_inverse<MatrixType, ResultType>::run(matrix, inverse); |
| } |
| else if(invertible) { |
| MatrixType matrix_t = matrix; |
| compute_inverse<MatrixType, ResultType>::run(matrix_t, inverse); |
| } |
| } |
| }; |
| |
| /************************* |
| *** MatrixBase methods *** |
| *************************/ |
| |
| } // end namespace internal |
| |
| namespace internal { |
| |
| // Specialization for "dense = dense_xpr.inverse()" |
| template<typename DstXprType, typename XprType> |
| struct Assignment<DstXprType, Inverse<XprType>, internal::assign_op<typename DstXprType::Scalar,typename XprType::Scalar>, Dense2Dense> |
| { |
| typedef Inverse<XprType> SrcXprType; |
| EIGEN_DEVICE_FUNC |
| static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<typename DstXprType::Scalar,typename XprType::Scalar> &) |
| { |
| Index dstRows = src.rows(); |
| Index dstCols = src.cols(); |
| if((dst.rows()!=dstRows) || (dst.cols()!=dstCols)) |
| dst.resize(dstRows, dstCols); |
| |
| const int Size = plain_enum_min(XprType::ColsAtCompileTime, DstXprType::ColsAtCompileTime); |
| EIGEN_ONLY_USED_FOR_DEBUG(Size); |
| eigen_assert(( (Size<=1) || (Size>4) || (extract_data(src.nestedExpression())!=extract_data(dst))) |
| && "Aliasing problem detected in inverse(), you need to do inverse().eval() here."); |
| |
| typedef typename internal::nested_eval<XprType,XprType::ColsAtCompileTime>::type ActualXprType; |
| typedef internal::remove_all_t<ActualXprType> ActualXprTypeCleanded; |
| |
| ActualXprType actual_xpr(src.nestedExpression()); |
| |
| compute_inverse<ActualXprTypeCleanded, DstXprType>::run(actual_xpr, dst); |
| } |
| }; |
| |
| |
| } // end namespace internal |
| |
| /** \lu_module |
| * |
| * \returns the matrix inverse of this matrix. |
| * |
| * For small fixed sizes up to 4x4, this method uses cofactors. |
| * In the general case, this method uses class PartialPivLU. |
| * |
| * \note This matrix must be invertible, otherwise the result is undefined. If you need an |
| * invertibility check, do the following: |
| * \li for fixed sizes up to 4x4, use computeInverseAndDetWithCheck(). |
| * \li for the general case, use class FullPivLU. |
| * |
| * Example: \include MatrixBase_inverse.cpp |
| * Output: \verbinclude MatrixBase_inverse.out |
| * |
| * \sa computeInverseAndDetWithCheck() |
| */ |
| template<typename Derived> |
| EIGEN_DEVICE_FUNC |
| inline const Inverse<Derived> MatrixBase<Derived>::inverse() const |
| { |
| EIGEN_STATIC_ASSERT(!NumTraits<Scalar>::IsInteger,THIS_FUNCTION_IS_NOT_FOR_INTEGER_NUMERIC_TYPES) |
| eigen_assert(rows() == cols()); |
| return Inverse<Derived>(derived()); |
| } |
| |
| /** \lu_module |
| * |
| * Computation of matrix inverse and determinant, with invertibility check. |
| * |
| * This is only for fixed-size square matrices of size up to 4x4. |
| * |
| * Notice that it will trigger a copy of input matrix when trying to do the inverse in place. |
| * |
| * \param inverse Reference to the matrix in which to store the inverse. |
| * \param determinant Reference to the variable in which to store the determinant. |
| * \param invertible Reference to the bool variable in which to store whether the matrix is invertible. |
| * \param absDeterminantThreshold Optional parameter controlling the invertibility check. |
| * The matrix will be declared invertible if the absolute value of its |
| * determinant is greater than this threshold. |
| * |
| * Example: \include MatrixBase_computeInverseAndDetWithCheck.cpp |
| * Output: \verbinclude MatrixBase_computeInverseAndDetWithCheck.out |
| * |
| * \sa inverse(), computeInverseWithCheck() |
| */ |
| template<typename Derived> |
| template<typename ResultType> |
| inline void MatrixBase<Derived>::computeInverseAndDetWithCheck( |
| ResultType& inverse, |
| typename ResultType::Scalar& determinant, |
| bool& invertible, |
| const RealScalar& absDeterminantThreshold |
| ) const |
| { |
| // i'd love to put some static assertions there, but SFINAE means that they have no effect... |
| eigen_assert(rows() == cols()); |
| // for 2x2, it's worth giving a chance to avoid evaluating. |
| // for larger sizes, evaluating has negligible cost and limits code size. |
| typedef std::conditional_t< |
| RowsAtCompileTime == 2, |
| internal::remove_all_t<typename internal::nested_eval<Derived, 2>::type>, |
| PlainObject |
| > MatrixType; |
| internal::compute_inverse_and_det_with_check<MatrixType, ResultType>::run |
| (derived(), absDeterminantThreshold, inverse, determinant, invertible); |
| } |
| |
| /** \lu_module |
| * |
| * Computation of matrix inverse, with invertibility check. |
| * |
| * This is only for fixed-size square matrices of size up to 4x4. |
| * |
| * Notice that it will trigger a copy of input matrix when trying to do the inverse in place. |
| * |
| * \param inverse Reference to the matrix in which to store the inverse. |
| * \param invertible Reference to the bool variable in which to store whether the matrix is invertible. |
| * \param absDeterminantThreshold Optional parameter controlling the invertibility check. |
| * The matrix will be declared invertible if the absolute value of its |
| * determinant is greater than this threshold. |
| * |
| * Example: \include MatrixBase_computeInverseWithCheck.cpp |
| * Output: \verbinclude MatrixBase_computeInverseWithCheck.out |
| * |
| * \sa inverse(), computeInverseAndDetWithCheck() |
| */ |
| template<typename Derived> |
| template<typename ResultType> |
| inline void MatrixBase<Derived>::computeInverseWithCheck( |
| ResultType& inverse, |
| bool& invertible, |
| const RealScalar& absDeterminantThreshold |
| ) const |
| { |
| Scalar determinant; |
| // i'd love to put some static assertions there, but SFINAE means that they have no effect... |
| eigen_assert(rows() == cols()); |
| computeInverseAndDetWithCheck(inverse,determinant,invertible,absDeterminantThreshold); |
| } |
| |
| } // end namespace Eigen |
| |
| #endif // EIGEN_INVERSE_IMPL_H |