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
13namespace Eigen {
14
15namespace internal {
16
17/**********************************
18*** General case implementation ***
19**********************************/
20
21template<typename MatrixType, typename ResultType, int Size = MatrixType::RowsAtCompileTime>
22struct compute_inverse
23{
24  static inline void run(const MatrixType& matrix, ResultType& result)
25  {
26    result = matrix.partialPivLu().inverse();
27  }
28};
29
30template<typename MatrixType, typename ResultType, int Size = MatrixType::RowsAtCompileTime>
31struct compute_inverse_and_det_with_check { /* nothing! general case not supported. */ };
32
33/****************************
34*** Size 1 implementation ***
35****************************/
36
37template<typename MatrixType, typename ResultType>
38struct 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
47template<typename MatrixType, typename ResultType>
48struct 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
69template<typename MatrixType, typename ResultType>
70inline 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
80template<typename MatrixType, typename ResultType>
81struct 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
91template<typename MatrixType, typename ResultType>
92struct 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
116template<typename MatrixType, int i, int j>
117inline 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
129template<typename MatrixType, typename ResultType>
130inline 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
145template<typename MatrixType, typename ResultType>
146struct 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
161template<typename MatrixType, typename ResultType>
162struct 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
190template<typename Derived>
191inline 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
198template<typename MatrixType, int i, int j>
199inline 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
214template<int Arch, typename Scalar, typename MatrixType, typename ResultType>
215struct 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
239template<typename MatrixType, typename ResultType>
240struct compute_inverse<MatrixType, ResultType, 4>
241 : compute_inverse_size4<Architecture::Target, typename MatrixType::Scalar,
242                            MatrixType, ResultType>
243{
244};
245
246template<typename MatrixType, typename ResultType>
247struct 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
268template<typename MatrixType>
269struct traits<inverse_impl<MatrixType> >
270{
271  typedef typename MatrixType::PlainObject ReturnType;
272};
273
274template<typename MatrixType>
275struct 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  */
319template<typename Derived>
320inline 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  */
345template<typename Derived>
346template<typename ResultType>
347inline 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  */
384template<typename Derived>
385template<typename ResultType>
386inline 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