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