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