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
14namespace Eigen {
15
16namespace internal {
17
18/**********************************
19*** General case implementation ***
20**********************************/
21
22template<typename MatrixType, typename ResultType, int Size = MatrixType::RowsAtCompileTime>
23struct compute_inverse
24{
25  EIGEN_DEVICE_FUNC
26  static inline void run(const MatrixType& matrix, ResultType& result)
27  {
28    result = matrix.partialPivLu().inverse();
29  }
30};
31
32template<typename MatrixType, typename ResultType, int Size = MatrixType::RowsAtCompileTime>
33struct compute_inverse_and_det_with_check { /* nothing! general case not supported. */ };
34
35/****************************
36*** Size 1 implementation ***
37****************************/
38
39template<typename MatrixType, typename ResultType>
40struct 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
51template<typename MatrixType, typename ResultType>
52struct 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
74template<typename MatrixType, typename ResultType>
75EIGEN_DEVICE_FUNC
76inline 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
86template<typename MatrixType, typename ResultType>
87struct 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
98template<typename MatrixType, typename ResultType>
99struct 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
124template<typename MatrixType, int i, int j>
125EIGEN_DEVICE_FUNC
126inline 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
138template<typename MatrixType, typename ResultType>
139EIGEN_DEVICE_FUNC
140inline 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
155template<typename MatrixType, typename ResultType>
156struct 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
172template<typename MatrixType, typename ResultType>
173struct 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
202template<typename Derived>
203EIGEN_DEVICE_FUNC
204inline 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
211template<typename MatrixType, int i, int j>
212EIGEN_DEVICE_FUNC
213inline 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
228template<int Arch, typename Scalar, typename MatrixType, typename ResultType>
229struct 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
254template<typename MatrixType, typename ResultType>
255struct compute_inverse<MatrixType, ResultType, 4>
256 : compute_inverse_size4<Architecture::Target, typename MatrixType::Scalar,
257                            MatrixType, ResultType>
258{
259};
260
261template<typename MatrixType, typename ResultType>
262struct 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
286namespace internal {
287
288// Specialization for "dense = dense_xpr.inverse()"
289template<typename DstXprType, typename XprType>
290struct 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  */
334template<typename Derived>
335inline 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  */
360template<typename Derived>
361template<typename ResultType>
362inline 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  */
399template<typename Derived>
400template<typename ResultType>
401inline 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