Inverse.h
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 {
24  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  determinant = matrix.coeff(0,0);
59  invertible = abs(determinant) > absDeterminantThreshold;
60  if(invertible) result.coeffRef(0,0) = typename ResultType::Scalar(1) / determinant;
61  }
62 };
63 
64 /****************************
65 *** Size 2 implementation ***
66 ****************************/
67 
68 template<typename MatrixType, typename ResultType>
69 inline void compute_inverse_size2_helper(
70  const MatrixType& matrix, const typename ResultType::Scalar& invdet,
71  ResultType& result)
72 {
73  result.coeffRef(0,0) = matrix.coeff(1,1) * invdet;
74  result.coeffRef(1,0) = -matrix.coeff(1,0) * invdet;
75  result.coeffRef(0,1) = -matrix.coeff(0,1) * invdet;
76  result.coeffRef(1,1) = matrix.coeff(0,0) * invdet;
77 }
78 
79 template<typename MatrixType, typename ResultType>
80 struct compute_inverse<MatrixType, ResultType, 2>
81 {
82  static inline void run(const MatrixType& matrix, ResultType& result)
83  {
84  typedef typename ResultType::Scalar Scalar;
85  const Scalar invdet = typename MatrixType::Scalar(1) / matrix.determinant();
86  compute_inverse_size2_helper(matrix, invdet, result);
87  }
88 };
89 
90 template<typename MatrixType, typename ResultType>
91 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 2>
92 {
93  static inline void run(
94  const MatrixType& matrix,
95  const typename MatrixType::RealScalar& absDeterminantThreshold,
96  ResultType& inverse,
97  typename ResultType::Scalar& determinant,
98  bool& invertible
99  )
100  {
101  typedef typename ResultType::Scalar Scalar;
102  determinant = matrix.determinant();
103  invertible = abs(determinant) > absDeterminantThreshold;
104  if(!invertible) return;
105  const Scalar invdet = Scalar(1) / determinant;
106  compute_inverse_size2_helper(matrix, invdet, inverse);
107  }
108 };
109 
110 /****************************
111 *** Size 3 implementation ***
112 ****************************/
113 
114 template<typename MatrixType, int i, int j>
115 inline typename MatrixType::Scalar cofactor_3x3(const MatrixType& m)
116 {
117  enum {
118  i1 = (i+1) % 3,
119  i2 = (i+2) % 3,
120  j1 = (j+1) % 3,
121  j2 = (j+2) % 3
122  };
123  return m.coeff(i1, j1) * m.coeff(i2, j2)
124  - m.coeff(i1, j2) * m.coeff(i2, j1);
125 }
126 
127 template<typename MatrixType, typename ResultType>
128 inline void compute_inverse_size3_helper(
129  const MatrixType& matrix,
130  const typename ResultType::Scalar& invdet,
131  const Matrix<typename ResultType::Scalar,3,1>& cofactors_col0,
132  ResultType& result)
133 {
134  result.row(0) = cofactors_col0 * invdet;
135  result.coeffRef(1,0) = cofactor_3x3<MatrixType,0,1>(matrix) * invdet;
136  result.coeffRef(1,1) = cofactor_3x3<MatrixType,1,1>(matrix) * invdet;
137  result.coeffRef(1,2) = cofactor_3x3<MatrixType,2,1>(matrix) * invdet;
138  result.coeffRef(2,0) = cofactor_3x3<MatrixType,0,2>(matrix) * invdet;
139  result.coeffRef(2,1) = cofactor_3x3<MatrixType,1,2>(matrix) * invdet;
140  result.coeffRef(2,2) = cofactor_3x3<MatrixType,2,2>(matrix) * invdet;
141 }
142 
143 template<typename MatrixType, typename ResultType>
144 struct compute_inverse<MatrixType, ResultType, 3>
145 {
146  static inline void run(const MatrixType& matrix, ResultType& result)
147  {
148  typedef typename ResultType::Scalar Scalar;
149  Matrix<typename MatrixType::Scalar,3,1> cofactors_col0;
150  cofactors_col0.coeffRef(0) = cofactor_3x3<MatrixType,0,0>(matrix);
151  cofactors_col0.coeffRef(1) = cofactor_3x3<MatrixType,1,0>(matrix);
152  cofactors_col0.coeffRef(2) = cofactor_3x3<MatrixType,2,0>(matrix);
153  const Scalar det = (cofactors_col0.cwiseProduct(matrix.col(0))).sum();
154  const Scalar invdet = Scalar(1) / det;
155  compute_inverse_size3_helper(matrix, invdet, cofactors_col0, result);
156  }
157 };
158 
159 template<typename MatrixType, typename ResultType>
160 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 3>
161 {
162  static inline void run(
163  const MatrixType& matrix,
164  const typename MatrixType::RealScalar& absDeterminantThreshold,
165  ResultType& inverse,
166  typename ResultType::Scalar& determinant,
167  bool& invertible
168  )
169  {
170  typedef typename ResultType::Scalar Scalar;
171  Matrix<Scalar,3,1> cofactors_col0;
172  cofactors_col0.coeffRef(0) = cofactor_3x3<MatrixType,0,0>(matrix);
173  cofactors_col0.coeffRef(1) = cofactor_3x3<MatrixType,1,0>(matrix);
174  cofactors_col0.coeffRef(2) = cofactor_3x3<MatrixType,2,0>(matrix);
175  determinant = (cofactors_col0.cwiseProduct(matrix.col(0))).sum();
176  invertible = abs(determinant) > absDeterminantThreshold;
177  if(!invertible) return;
178  const Scalar invdet = Scalar(1) / determinant;
179  compute_inverse_size3_helper(matrix, invdet, cofactors_col0, inverse);
180  }
181 };
182 
183 /****************************
184 *** Size 4 implementation ***
185 ****************************/
186 
187 template<typename Derived>
188 inline const typename Derived::Scalar general_det3_helper
189 (const MatrixBase<Derived>& matrix, int i1, int i2, int i3, int j1, int j2, int j3)
190 {
191  return matrix.coeff(i1,j1)
192  * (matrix.coeff(i2,j2) * matrix.coeff(i3,j3) - matrix.coeff(i2,j3) * matrix.coeff(i3,j2));
193 }
194 
195 template<typename MatrixType, int i, int j>
196 inline typename MatrixType::Scalar cofactor_4x4(const MatrixType& matrix)
197 {
198  enum {
199  i1 = (i+1) % 4,
200  i2 = (i+2) % 4,
201  i3 = (i+3) % 4,
202  j1 = (j+1) % 4,
203  j2 = (j+2) % 4,
204  j3 = (j+3) % 4
205  };
206  return general_det3_helper(matrix, i1, i2, i3, j1, j2, j3)
207  + general_det3_helper(matrix, i2, i3, i1, j1, j2, j3)
208  + general_det3_helper(matrix, i3, i1, i2, j1, j2, j3);
209 }
210 
211 template<int Arch, typename Scalar, typename MatrixType, typename ResultType>
212 struct compute_inverse_size4
213 {
214  static void run(const MatrixType& matrix, ResultType& result)
215  {
216  result.coeffRef(0,0) = cofactor_4x4<MatrixType,0,0>(matrix);
217  result.coeffRef(1,0) = -cofactor_4x4<MatrixType,0,1>(matrix);
218  result.coeffRef(2,0) = cofactor_4x4<MatrixType,0,2>(matrix);
219  result.coeffRef(3,0) = -cofactor_4x4<MatrixType,0,3>(matrix);
220  result.coeffRef(0,2) = cofactor_4x4<MatrixType,2,0>(matrix);
221  result.coeffRef(1,2) = -cofactor_4x4<MatrixType,2,1>(matrix);
222  result.coeffRef(2,2) = cofactor_4x4<MatrixType,2,2>(matrix);
223  result.coeffRef(3,2) = -cofactor_4x4<MatrixType,2,3>(matrix);
224  result.coeffRef(0,1) = -cofactor_4x4<MatrixType,1,0>(matrix);
225  result.coeffRef(1,1) = cofactor_4x4<MatrixType,1,1>(matrix);
226  result.coeffRef(2,1) = -cofactor_4x4<MatrixType,1,2>(matrix);
227  result.coeffRef(3,1) = cofactor_4x4<MatrixType,1,3>(matrix);
228  result.coeffRef(0,3) = -cofactor_4x4<MatrixType,3,0>(matrix);
229  result.coeffRef(1,3) = cofactor_4x4<MatrixType,3,1>(matrix);
230  result.coeffRef(2,3) = -cofactor_4x4<MatrixType,3,2>(matrix);
231  result.coeffRef(3,3) = cofactor_4x4<MatrixType,3,3>(matrix);
232  result /= (matrix.col(0).cwiseProduct(result.row(0).transpose())).sum();
233  }
234 };
235 
236 template<typename MatrixType, typename ResultType>
237 struct compute_inverse<MatrixType, ResultType, 4>
238  : compute_inverse_size4<Architecture::Target, typename MatrixType::Scalar,
239  MatrixType, ResultType>
240 {
241 };
242 
243 template<typename MatrixType, typename ResultType>
244 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 4>
245 {
246  static inline void run(
247  const MatrixType& matrix,
248  const typename MatrixType::RealScalar& absDeterminantThreshold,
249  ResultType& inverse,
250  typename ResultType::Scalar& determinant,
251  bool& invertible
252  )
253  {
254  determinant = matrix.determinant();
255  invertible = abs(determinant) > absDeterminantThreshold;
256  if(invertible) compute_inverse<MatrixType, ResultType>::run(matrix, inverse);
257  }
258 };
259 
260 /*************************
261 *** MatrixBase methods ***
262 *************************/
263 
264 template<typename MatrixType>
265 struct traits<inverse_impl<MatrixType> >
266 {
267  typedef typename MatrixType::PlainObject ReturnType;
268 };
269 
270 template<typename MatrixType>
271 struct inverse_impl : public ReturnByValue<inverse_impl<MatrixType> >
272 {
273  typedef typename MatrixType::Index Index;
274  typedef typename internal::eval<MatrixType>::type MatrixTypeNested;
275  typedef typename remove_all<MatrixTypeNested>::type MatrixTypeNestedCleaned;
276  MatrixTypeNested m_matrix;
277 
278  inverse_impl(const MatrixType& matrix)
279  : m_matrix(matrix)
280  {}
281 
282  inline Index rows() const { return m_matrix.rows(); }
283  inline Index cols() const { return m_matrix.cols(); }
284 
285  template<typename Dest> inline void evalTo(Dest& dst) const
286  {
287  const int Size = EIGEN_PLAIN_ENUM_MIN(MatrixType::ColsAtCompileTime,Dest::ColsAtCompileTime);
288  EIGEN_ONLY_USED_FOR_DEBUG(Size);
289  eigen_assert(( (Size<=1) || (Size>4) || (extract_data(m_matrix)!=extract_data(dst)))
290  && "Aliasing problem detected in inverse(), you need to do inverse().eval() here.");
291 
292  compute_inverse<MatrixTypeNestedCleaned, Dest>::run(m_matrix, dst);
293  }
294 };
295 
296 } // end namespace internal
297 
315 template<typename Derived>
316 inline const internal::inverse_impl<Derived> MatrixBase<Derived>::inverse() const
317 {
318  EIGEN_STATIC_ASSERT(!NumTraits<Scalar>::IsInteger,THIS_FUNCTION_IS_NOT_FOR_INTEGER_NUMERIC_TYPES)
319  eigen_assert(rows() == cols());
320  return internal::inverse_impl<Derived>(derived());
321 }
322 
341 template<typename Derived>
342 template<typename ResultType>
344  ResultType& inverse,
345  typename ResultType::Scalar& determinant,
346  bool& invertible,
347  const RealScalar& absDeterminantThreshold
348  ) const
349 {
350  // i'd love to put some static assertions there, but SFINAE means that they have no effect...
351  eigen_assert(rows() == cols());
352  // for 2x2, it's worth giving a chance to avoid evaluating.
353  // for larger sizes, evaluating has negligible cost and limits code size.
354  typedef typename internal::conditional<
355  RowsAtCompileTime == 2,
356  typename internal::remove_all<typename internal::nested<Derived, 2>::type>::type,
358  >::type MatrixType;
359  internal::compute_inverse_and_det_with_check<MatrixType, ResultType>::run
360  (derived(), absDeterminantThreshold, inverse, determinant, invertible);
361 }
362 
380 template<typename Derived>
381 template<typename ResultType>
383  ResultType& inverse,
384  bool& invertible,
385  const RealScalar& absDeterminantThreshold
386  ) const
387 {
388  RealScalar determinant;
389  // i'd love to put some static assertions there, but SFINAE means that they have no effect...
390  eigen_assert(rows() == cols());
391  computeInverseAndDetWithCheck(inverse,determinant,invertible,absDeterminantThreshold);
392 }
393 
394 } // end namespace Eigen
395 
396 #endif // EIGEN_INVERSE_H