Back to home page

EIC code displayed by LXR
 
 

    

File indexing completed on 2025-04-19 09:06:21 

0001 // This file is part of Eigen, a lightweight C++ template library
0002 // for linear algebra.
0003 //
0004 // Copyright (C) 2008-2010 Benoit Jacob <jacob.benoit.1@gmail.com>
0005 // Copyright (C) 2014 Gael Guennebaud <gael.guennebaud@inria.fr>
0006 //
0007 // This Source Code Form is subject to the terms of the Mozilla
0008 // Public License v. 2.0. If a copy of the MPL was not distributed
0009 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
0010 
0011 #ifndef EIGEN_INVERSE_IMPL_H
0012 #define EIGEN_INVERSE_IMPL_H
0013 
0014 namespace RivetEigen { 
0015 
0016 namespace internal {
0017 
0018 /**********************************
0019 *** General case implementation ***
0020 **********************************/
0021 
0022 template<typename MatrixType, typename ResultType, int Size = MatrixType::RowsAtCompileTime>
0023 struct compute_inverse
0024 {
0025   EIGEN_DEVICE_FUNC
0026   static inline void run(const MatrixType& matrix, ResultType& result)
0027   {
0028     result = matrix.partialPivLu().inverse();
0029   }
0030 };
0031 
0032 template<typename MatrixType, typename ResultType, int Size = MatrixType::RowsAtCompileTime>
0033 struct compute_inverse_and_det_with_check { /* nothing! general case not supported. */ };
0034 
0035 /****************************
0036 *** Size 1 implementation ***
0037 ****************************/
0038 
0039 template<typename MatrixType, typename ResultType>
0040 struct compute_inverse<MatrixType, ResultType, 1>
0041 {
0042   EIGEN_DEVICE_FUNC
0043   static inline void run(const MatrixType& matrix, ResultType& result)
0044   {
0045     typedef typename MatrixType::Scalar Scalar;
0046     internal::evaluator<MatrixType> matrixEval(matrix);
0047     result.coeffRef(0,0) = Scalar(1) / matrixEval.coeff(0,0);
0048   }
0049 };
0050 
0051 template<typename MatrixType, typename ResultType>
0052 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 1>
0053 {
0054   EIGEN_DEVICE_FUNC
0055   static inline void run(
0056     const MatrixType& matrix,
0057     const typename MatrixType::RealScalar& absDeterminantThreshold,
0058     ResultType& result,
0059     typename ResultType::Scalar& determinant,
0060     bool& invertible
0061   )
0062   {
0063     using std::abs;
0064     determinant = matrix.coeff(0,0);
0065     invertible = abs(determinant) > absDeterminantThreshold;
0066     if(invertible) result.coeffRef(0,0) = typename ResultType::Scalar(1) / determinant;
0067   }
0068 };
0069 
0070 /****************************
0071 *** Size 2 implementation ***
0072 ****************************/
0073 
0074 template<typename MatrixType, typename ResultType>
0075 EIGEN_DEVICE_FUNC 
0076 inline void compute_inverse_size2_helper(
0077     const MatrixType& matrix, const typename ResultType::Scalar& invdet,
0078     ResultType& result)
0079 {
0080   typename ResultType::Scalar temp = matrix.coeff(0,0);
0081   result.coeffRef(0,0) =  matrix.coeff(1,1) * invdet;
0082   result.coeffRef(1,0) = -matrix.coeff(1,0) * invdet;
0083   result.coeffRef(0,1) = -matrix.coeff(0,1) * invdet;
0084   result.coeffRef(1,1) =  temp * invdet;
0085 }
0086 
0087 template<typename MatrixType, typename ResultType>
0088 struct compute_inverse<MatrixType, ResultType, 2>
0089 {
0090   EIGEN_DEVICE_FUNC
0091   static inline void run(const MatrixType& matrix, ResultType& result)
0092   {
0093     typedef typename ResultType::Scalar Scalar;
0094     const Scalar invdet = typename MatrixType::Scalar(1) / matrix.determinant();
0095     compute_inverse_size2_helper(matrix, invdet, result);
0096   }
0097 };
0098 
0099 template<typename MatrixType, typename ResultType>
0100 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 2>
0101 {
0102   EIGEN_DEVICE_FUNC
0103   static inline void run(
0104     const MatrixType& matrix,
0105     const typename MatrixType::RealScalar& absDeterminantThreshold,
0106     ResultType& inverse,
0107     typename ResultType::Scalar& determinant,
0108     bool& invertible
0109   )
0110   {
0111     using std::abs;
0112     typedef typename ResultType::Scalar Scalar;
0113     determinant = matrix.determinant();
0114     invertible = abs(determinant) > absDeterminantThreshold;
0115     if(!invertible) return;
0116     const Scalar invdet = Scalar(1) / determinant;
0117     compute_inverse_size2_helper(matrix, invdet, inverse);
0118   }
0119 };
0120 
0121 /****************************
0122 *** Size 3 implementation ***
0123 ****************************/
0124 
0125 template<typename MatrixType, int i, int j>
0126 EIGEN_DEVICE_FUNC 
0127 inline typename MatrixType::Scalar cofactor_3x3(const MatrixType& m)
0128 {
0129   enum {
0130     i1 = (i+1) % 3,
0131     i2 = (i+2) % 3,
0132     j1 = (j+1) % 3,
0133     j2 = (j+2) % 3
0134   };
0135   return m.coeff(i1, j1) * m.coeff(i2, j2)
0136        - m.coeff(i1, j2) * m.coeff(i2, j1);
0137 }
0138 
0139 template<typename MatrixType, typename ResultType>
0140 EIGEN_DEVICE_FUNC
0141 inline void compute_inverse_size3_helper(
0142     const MatrixType& matrix,
0143     const typename ResultType::Scalar& invdet,
0144     const Matrix<typename ResultType::Scalar,3,1>& cofactors_col0,
0145     ResultType& result)
0146 {
0147   // Compute cofactors in a way that avoids aliasing issues.
0148   typedef typename ResultType::Scalar Scalar;
0149   const Scalar c01 = cofactor_3x3<MatrixType,0,1>(matrix) * invdet;
0150   const Scalar c11 = cofactor_3x3<MatrixType,1,1>(matrix) * invdet;
0151   const Scalar c02 = cofactor_3x3<MatrixType,0,2>(matrix) * invdet;
0152   result.coeffRef(1,2) =  cofactor_3x3<MatrixType,2,1>(matrix) * invdet;
0153   result.coeffRef(2,1) =  cofactor_3x3<MatrixType,1,2>(matrix) * invdet;
0154   result.coeffRef(2,2) =  cofactor_3x3<MatrixType,2,2>(matrix) * invdet;
0155   result.coeffRef(1,0) =  c01;
0156   result.coeffRef(1,1) =  c11;
0157   result.coeffRef(2,0) =  c02;  
0158   result.row(0) = cofactors_col0 * invdet;
0159 }
0160 
0161 template<typename MatrixType, typename ResultType>
0162 struct compute_inverse<MatrixType, ResultType, 3>
0163 {
0164   EIGEN_DEVICE_FUNC
0165   static inline void run(const MatrixType& matrix, ResultType& result)
0166   {
0167     typedef typename ResultType::Scalar Scalar;
0168     Matrix<typename MatrixType::Scalar,3,1> cofactors_col0;
0169     cofactors_col0.coeffRef(0) =  cofactor_3x3<MatrixType,0,0>(matrix);
0170     cofactors_col0.coeffRef(1) =  cofactor_3x3<MatrixType,1,0>(matrix);
0171     cofactors_col0.coeffRef(2) =  cofactor_3x3<MatrixType,2,0>(matrix);
0172     const Scalar det = (cofactors_col0.cwiseProduct(matrix.col(0))).sum();
0173     const Scalar invdet = Scalar(1) / det;
0174     compute_inverse_size3_helper(matrix, invdet, cofactors_col0, result);
0175   }
0176 };
0177 
0178 template<typename MatrixType, typename ResultType>
0179 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 3>
0180 {
0181   EIGEN_DEVICE_FUNC
0182   static inline void run(
0183     const MatrixType& matrix,
0184     const typename MatrixType::RealScalar& absDeterminantThreshold,
0185     ResultType& inverse,
0186     typename ResultType::Scalar& determinant,
0187     bool& invertible
0188   )
0189   {
0190     typedef typename ResultType::Scalar Scalar;
0191     Matrix<Scalar,3,1> cofactors_col0;
0192     cofactors_col0.coeffRef(0) =  cofactor_3x3<MatrixType,0,0>(matrix);
0193     cofactors_col0.coeffRef(1) =  cofactor_3x3<MatrixType,1,0>(matrix);
0194     cofactors_col0.coeffRef(2) =  cofactor_3x3<MatrixType,2,0>(matrix);
0195     determinant = (cofactors_col0.cwiseProduct(matrix.col(0))).sum();
0196     invertible = RivetEigen::numext::abs(determinant) > absDeterminantThreshold;
0197     if(!invertible) return;
0198     const Scalar invdet = Scalar(1) / determinant;
0199     compute_inverse_size3_helper(matrix, invdet, cofactors_col0, inverse);
0200   }
0201 };
0202 
0203 /****************************
0204 *** Size 4 implementation ***
0205 ****************************/
0206 
0207 template<typename Derived>
0208 EIGEN_DEVICE_FUNC 
0209 inline const typename Derived::Scalar general_det3_helper
0210 (const MatrixBase<Derived>& matrix, int i1, int i2, int i3, int j1, int j2, int j3)
0211 {
0212   return matrix.coeff(i1,j1)
0213          * (matrix.coeff(i2,j2) * matrix.coeff(i3,j3) - matrix.coeff(i2,j3) * matrix.coeff(i3,j2));
0214 }
0215 
0216 template<typename MatrixType, int i, int j>
0217 EIGEN_DEVICE_FUNC 
0218 inline typename MatrixType::Scalar cofactor_4x4(const MatrixType& matrix)
0219 {
0220   enum {
0221     i1 = (i+1) % 4,
0222     i2 = (i+2) % 4,
0223     i3 = (i+3) % 4,
0224     j1 = (j+1) % 4,
0225     j2 = (j+2) % 4,
0226     j3 = (j+3) % 4
0227   };
0228   return general_det3_helper(matrix, i1, i2, i3, j1, j2, j3)
0229        + general_det3_helper(matrix, i2, i3, i1, j1, j2, j3)
0230        + general_det3_helper(matrix, i3, i1, i2, j1, j2, j3);
0231 }
0232 
0233 template<int Arch, typename Scalar, typename MatrixType, typename ResultType>
0234 struct compute_inverse_size4
0235 {
0236   EIGEN_DEVICE_FUNC
0237   static void run(const MatrixType& matrix, ResultType& result)
0238   {
0239     result.coeffRef(0,0) =  cofactor_4x4<MatrixType,0,0>(matrix);
0240     result.coeffRef(1,0) = -cofactor_4x4<MatrixType,0,1>(matrix);
0241     result.coeffRef(2,0) =  cofactor_4x4<MatrixType,0,2>(matrix);
0242     result.coeffRef(3,0) = -cofactor_4x4<MatrixType,0,3>(matrix);
0243     result.coeffRef(0,2) =  cofactor_4x4<MatrixType,2,0>(matrix);
0244     result.coeffRef(1,2) = -cofactor_4x4<MatrixType,2,1>(matrix);
0245     result.coeffRef(2,2) =  cofactor_4x4<MatrixType,2,2>(matrix);
0246     result.coeffRef(3,2) = -cofactor_4x4<MatrixType,2,3>(matrix);
0247     result.coeffRef(0,1) = -cofactor_4x4<MatrixType,1,0>(matrix);
0248     result.coeffRef(1,1) =  cofactor_4x4<MatrixType,1,1>(matrix);
0249     result.coeffRef(2,1) = -cofactor_4x4<MatrixType,1,2>(matrix);
0250     result.coeffRef(3,1) =  cofactor_4x4<MatrixType,1,3>(matrix);
0251     result.coeffRef(0,3) = -cofactor_4x4<MatrixType,3,0>(matrix);
0252     result.coeffRef(1,3) =  cofactor_4x4<MatrixType,3,1>(matrix);
0253     result.coeffRef(2,3) = -cofactor_4x4<MatrixType,3,2>(matrix);
0254     result.coeffRef(3,3) =  cofactor_4x4<MatrixType,3,3>(matrix);
0255     result /= (matrix.col(0).cwiseProduct(result.row(0).transpose())).sum();
0256   }
0257 };
0258 
0259 template<typename MatrixType, typename ResultType>
0260 struct compute_inverse<MatrixType, ResultType, 4>
0261  : compute_inverse_size4<Architecture::Target, typename MatrixType::Scalar,
0262                             MatrixType, ResultType>
0263 {
0264 };
0265 
0266 template<typename MatrixType, typename ResultType>
0267 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 4>
0268 {
0269   EIGEN_DEVICE_FUNC
0270   static inline void run(
0271     const MatrixType& matrix,
0272     const typename MatrixType::RealScalar& absDeterminantThreshold,
0273     ResultType& inverse,
0274     typename ResultType::Scalar& determinant,
0275     bool& invertible
0276   )
0277   {
0278     using std::abs;
0279     determinant = matrix.determinant();
0280     invertible = abs(determinant) > absDeterminantThreshold;
0281     if(invertible && extract_data(matrix) != extract_data(inverse)) {
0282       compute_inverse<MatrixType, ResultType>::run(matrix, inverse);
0283     }
0284     else if(invertible) {
0285       MatrixType matrix_t = matrix;
0286       compute_inverse<MatrixType, ResultType>::run(matrix_t, inverse);
0287     }
0288   }
0289 };
0290 
0291 /*************************
0292 *** MatrixBase methods ***
0293 *************************/
0294 
0295 } // end namespace internal
0296 
0297 namespace internal {
0298 
0299 // Specialization for "dense = dense_xpr.inverse()"
0300 template<typename DstXprType, typename XprType>
0301 struct Assignment<DstXprType, Inverse<XprType>, internal::assign_op<typename DstXprType::Scalar,typename XprType::Scalar>, Dense2Dense>
0302 {
0303   typedef Inverse<XprType> SrcXprType;
0304   EIGEN_DEVICE_FUNC
0305   static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<typename DstXprType::Scalar,typename XprType::Scalar> &)
0306   {
0307     Index dstRows = src.rows();
0308     Index dstCols = src.cols();
0309     if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
0310       dst.resize(dstRows, dstCols);
0311     
0312     const int Size = EIGEN_PLAIN_ENUM_MIN(XprType::ColsAtCompileTime,DstXprType::ColsAtCompileTime);
0313     EIGEN_ONLY_USED_FOR_DEBUG(Size);
0314     eigen_assert(( (Size<=1) || (Size>4) || (extract_data(src.nestedExpression())!=extract_data(dst)))
0315               && "Aliasing problem detected in inverse(), you need to do inverse().eval() here.");
0316 
0317     typedef typename internal::nested_eval<XprType,XprType::ColsAtCompileTime>::type  ActualXprType;
0318     typedef typename internal::remove_all<ActualXprType>::type                        ActualXprTypeCleanded;
0319     
0320     ActualXprType actual_xpr(src.nestedExpression());
0321     
0322     compute_inverse<ActualXprTypeCleanded, DstXprType>::run(actual_xpr, dst);
0323   }
0324 };
0325 
0326   
0327 } // end namespace internal
0328 
0329 /** \lu_module
0330   *
0331   * \returns the matrix inverse of this matrix.
0332   *
0333   * For small fixed sizes up to 4x4, this method uses cofactors.
0334   * In the general case, this method uses class PartialPivLU.
0335   *
0336   * \note This matrix must be invertible, otherwise the result is undefined. If you need an
0337   * invertibility check, do the following:
0338   * \li for fixed sizes up to 4x4, use computeInverseAndDetWithCheck().
0339   * \li for the general case, use class FullPivLU.
0340   *
0341   * Example: \include MatrixBase_inverse.cpp
0342   * Output: \verbinclude MatrixBase_inverse.out
0343   *
0344   * \sa computeInverseAndDetWithCheck()
0345   */
0346 template<typename Derived>
0347 EIGEN_DEVICE_FUNC
0348 inline const Inverse<Derived> MatrixBase<Derived>::inverse() const
0349 {
0350   EIGEN_STATIC_ASSERT(!NumTraits<Scalar>::IsInteger,THIS_FUNCTION_IS_NOT_FOR_INTEGER_NUMERIC_TYPES)
0351   eigen_assert(rows() == cols());
0352   return Inverse<Derived>(derived());
0353 }
0354 
0355 /** \lu_module
0356   *
0357   * Computation of matrix inverse and determinant, with invertibility check.
0358   *
0359   * This is only for fixed-size square matrices of size up to 4x4.
0360   *
0361   * Notice that it will trigger a copy of input matrix when trying to do the inverse in place.
0362   *
0363   * \param inverse Reference to the matrix in which to store the inverse.
0364   * \param determinant Reference to the variable in which to store the determinant.
0365   * \param invertible Reference to the bool variable in which to store whether the matrix is invertible.
0366   * \param absDeterminantThreshold Optional parameter controlling the invertibility check.
0367   *                                The matrix will be declared invertible if the absolute value of its
0368   *                                determinant is greater than this threshold.
0369   *
0370   * Example: \include MatrixBase_computeInverseAndDetWithCheck.cpp
0371   * Output: \verbinclude MatrixBase_computeInverseAndDetWithCheck.out
0372   *
0373   * \sa inverse(), computeInverseWithCheck()
0374   */
0375 template<typename Derived>
0376 template<typename ResultType>
0377 inline void MatrixBase<Derived>::computeInverseAndDetWithCheck(
0378     ResultType& inverse,
0379     typename ResultType::Scalar& determinant,
0380     bool& invertible,
0381     const RealScalar& absDeterminantThreshold
0382   ) const
0383 {
0384   // i'd love to put some static assertions there, but SFINAE means that they have no effect...
0385   eigen_assert(rows() == cols());
0386   // for 2x2, it's worth giving a chance to avoid evaluating.
0387   // for larger sizes, evaluating has negligible cost and limits code size.
0388   typedef typename internal::conditional<
0389     RowsAtCompileTime == 2,
0390     typename internal::remove_all<typename internal::nested_eval<Derived, 2>::type>::type,
0391     PlainObject
0392   >::type MatrixType;
0393   internal::compute_inverse_and_det_with_check<MatrixType, ResultType>::run
0394     (derived(), absDeterminantThreshold, inverse, determinant, invertible);
0395 }
0396 
0397 /** \lu_module
0398   *
0399   * Computation of matrix inverse, with invertibility check.
0400   *
0401   * This is only for fixed-size square matrices of size up to 4x4.
0402   *
0403   * Notice that it will trigger a copy of input matrix when trying to do the inverse in place.
0404   *
0405   * \param inverse Reference to the matrix in which to store the inverse.
0406   * \param invertible Reference to the bool variable in which to store whether the matrix is invertible.
0407   * \param absDeterminantThreshold Optional parameter controlling the invertibility check.
0408   *                                The matrix will be declared invertible if the absolute value of its
0409   *                                determinant is greater than this threshold.
0410   *
0411   * Example: \include MatrixBase_computeInverseWithCheck.cpp
0412   * Output: \verbinclude MatrixBase_computeInverseWithCheck.out
0413   *
0414   * \sa inverse(), computeInverseAndDetWithCheck()
0415   */
0416 template<typename Derived>
0417 template<typename ResultType>
0418 inline void MatrixBase<Derived>::computeInverseWithCheck(
0419     ResultType& inverse,
0420     bool& invertible,
0421     const RealScalar& absDeterminantThreshold
0422   ) const
0423 {
0424   Scalar determinant;
0425   // i'd love to put some static assertions there, but SFINAE means that they have no effect...
0426   eigen_assert(rows() == cols());
0427   computeInverseAndDetWithCheck(inverse,determinant,invertible,absDeterminantThreshold);
0428 }
0429 
0430 } // end namespace RivetEigen
0431 
0432 #endif // EIGEN_INVERSE_IMPL_H
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.3.7 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!