You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

396 lines
14 KiB

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