Inverse.h
Go to the documentation of this file.
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 // Eigen is free software; you can redistribute it and/or
7 // modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation; either
9 // version 3 of the License, or (at your option) any later version.
10 //
11 // Alternatively, you can redistribute it and/or
12 // modify it under the terms of the GNU General Public License as
13 // published by the Free Software Foundation; either version 2 of
14 // the License, or (at your option) any later version.
15 //
16 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
17 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
18 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
19 // GNU General Public License for more details.
20 //
21 // You should have received a copy of the GNU Lesser General Public
22 // License and a copy of the GNU General Public License along with
23 // Eigen. If not, see <http://www.gnu.org/licenses/>.
24 
25 #ifndef EIGEN_INVERSE_H
26 #define EIGEN_INVERSE_H
27 
28 namespace Eigen {
29 
30 namespace internal {
31 
32 /**********************************
33 *** General case implementation ***
34 **********************************/
35 
36 template<typename MatrixType, typename ResultType, int Size = MatrixType::RowsAtCompileTime>
37 struct compute_inverse
38 {
39  static inline void run(const MatrixType& matrix, ResultType& result)
40  {
41  result = matrix.partialPivLu().inverse();
42  }
43 };
44 
45 template<typename MatrixType, typename ResultType, int Size = MatrixType::RowsAtCompileTime>
46 struct compute_inverse_and_det_with_check { /* nothing! general case not supported. */ };
47 
48 /****************************
49 *** Size 1 implementation ***
50 ****************************/
51 
52 template<typename MatrixType, typename ResultType>
53 struct compute_inverse<MatrixType, ResultType, 1>
54 {
55  static inline void run(const MatrixType& matrix, ResultType& result)
56  {
57  typedef typename MatrixType::Scalar Scalar;
58  result.coeffRef(0,0) = Scalar(1) / matrix.coeff(0,0);
59  }
60 };
61 
62 template<typename MatrixType, typename ResultType>
63 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 1>
64 {
65  static inline void run(
66  const MatrixType& matrix,
67  const typename MatrixType::RealScalar& absDeterminantThreshold,
68  ResultType& result,
69  typename ResultType::Scalar& determinant,
70  bool& invertible
71  )
72  {
73  determinant = matrix.coeff(0,0);
74  invertible = abs(determinant) > absDeterminantThreshold;
75  if(invertible) result.coeffRef(0,0) = typename ResultType::Scalar(1) / determinant;
76  }
77 };
78 
79 /****************************
80 *** Size 2 implementation ***
81 ****************************/
82 
83 template<typename MatrixType, typename ResultType>
85  const MatrixType& matrix, const typename ResultType::Scalar& invdet,
86  ResultType& result)
87 {
88  result.coeffRef(0,0) = matrix.coeff(1,1) * invdet;
89  result.coeffRef(1,0) = -matrix.coeff(1,0) * invdet;
90  result.coeffRef(0,1) = -matrix.coeff(0,1) * invdet;
91  result.coeffRef(1,1) = matrix.coeff(0,0) * invdet;
92 }
93 
94 template<typename MatrixType, typename ResultType>
95 struct compute_inverse<MatrixType, ResultType, 2>
96 {
97  static inline void run(const MatrixType& matrix, ResultType& result)
98  {
99  typedef typename ResultType::Scalar Scalar;
100  const Scalar invdet = typename MatrixType::Scalar(1) / matrix.determinant();
101  compute_inverse_size2_helper(matrix, invdet, result);
102  }
103 };
104 
105 template<typename MatrixType, typename ResultType>
106 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 2>
107 {
108  static inline void run(
109  const MatrixType& matrix,
110  const typename MatrixType::RealScalar& absDeterminantThreshold,
111  ResultType& inverse,
112  typename ResultType::Scalar& determinant,
113  bool& invertible
114  )
115  {
116  typedef typename ResultType::Scalar Scalar;
117  determinant = matrix.determinant();
118  invertible = abs(determinant) > absDeterminantThreshold;
119  if(!invertible) return;
120  const Scalar invdet = Scalar(1) / determinant;
121  compute_inverse_size2_helper(matrix, invdet, inverse);
122  }
123 };
124 
125 /****************************
126 *** Size 3 implementation ***
127 ****************************/
128 
129 template<typename MatrixType, int i, int j>
130 inline typename MatrixType::Scalar cofactor_3x3(const MatrixType& m)
131 {
132  enum {
133  i1 = (i+1) % 3,
134  i2 = (i+2) % 3,
135  j1 = (j+1) % 3,
136  j2 = (j+2) % 3
137  };
138  return m.coeff(i1, j1) * m.coeff(i2, j2)
139  - m.coeff(i1, j2) * m.coeff(i2, j1);
140 }
141 
142 template<typename MatrixType, typename ResultType>
144  const MatrixType& matrix,
145  const typename ResultType::Scalar& invdet,
146  const Matrix<typename ResultType::Scalar,3,1>& cofactors_col0,
147  ResultType& result)
148 {
149  result.row(0) = cofactors_col0 * invdet;
150  result.coeffRef(1,0) = cofactor_3x3<MatrixType,0,1>(matrix) * invdet;
151  result.coeffRef(1,1) = cofactor_3x3<MatrixType,1,1>(matrix) * invdet;
152  result.coeffRef(1,2) = cofactor_3x3<MatrixType,2,1>(matrix) * invdet;
153  result.coeffRef(2,0) = cofactor_3x3<MatrixType,0,2>(matrix) * invdet;
154  result.coeffRef(2,1) = cofactor_3x3<MatrixType,1,2>(matrix) * invdet;
155  result.coeffRef(2,2) = cofactor_3x3<MatrixType,2,2>(matrix) * invdet;
156 }
157 
158 template<typename MatrixType, typename ResultType>
159 struct compute_inverse<MatrixType, ResultType, 3>
160 {
161  static inline void run(const MatrixType& matrix, ResultType& result)
162  {
163  typedef typename ResultType::Scalar Scalar;
165  cofactors_col0.coeffRef(0) = cofactor_3x3<MatrixType,0,0>(matrix);
166  cofactors_col0.coeffRef(1) = cofactor_3x3<MatrixType,1,0>(matrix);
167  cofactors_col0.coeffRef(2) = cofactor_3x3<MatrixType,2,0>(matrix);
168  const Scalar det = (cofactors_col0.cwiseProduct(matrix.col(0))).sum();
169  const Scalar invdet = Scalar(1) / det;
170  compute_inverse_size3_helper(matrix, invdet, cofactors_col0, result);
171  }
172 };
173 
174 template<typename MatrixType, typename ResultType>
175 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 3>
176 {
177  static inline void run(
178  const MatrixType& matrix,
179  const typename MatrixType::RealScalar& absDeterminantThreshold,
180  ResultType& inverse,
181  typename ResultType::Scalar& determinant,
182  bool& invertible
183  )
184  {
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 inline const typename Derived::Scalar general_det3_helper
204 (const MatrixBase<Derived>& matrix, int i1, int i2, int i3, int j1, int j2, int j3)
205 {
206  return matrix.coeff(i1,j1)
207  * (matrix.coeff(i2,j2) * matrix.coeff(i3,j3) - matrix.coeff(i2,j3) * matrix.coeff(i3,j2));
208 }
209 
210 template<typename MatrixType, int i, int j>
211 inline typename MatrixType::Scalar cofactor_4x4(const MatrixType& matrix)
212 {
213  enum {
214  i1 = (i+1) % 4,
215  i2 = (i+2) % 4,
216  i3 = (i+3) % 4,
217  j1 = (j+1) % 4,
218  j2 = (j+2) % 4,
219  j3 = (j+3) % 4
220  };
221  return general_det3_helper(matrix, i1, i2, i3, j1, j2, j3)
222  + general_det3_helper(matrix, i2, i3, i1, j1, j2, j3)
223  + general_det3_helper(matrix, i3, i1, i2, j1, j2, j3);
224 }
225 
226 template<int Arch, typename Scalar, typename MatrixType, typename ResultType>
227 struct compute_inverse_size4
228 {
229  static void run(const MatrixType& matrix, ResultType& result)
230  {
231  result.coeffRef(0,0) = cofactor_4x4<MatrixType,0,0>(matrix);
232  result.coeffRef(1,0) = -cofactor_4x4<MatrixType,0,1>(matrix);
233  result.coeffRef(2,0) = cofactor_4x4<MatrixType,0,2>(matrix);
234  result.coeffRef(3,0) = -cofactor_4x4<MatrixType,0,3>(matrix);
235  result.coeffRef(0,2) = cofactor_4x4<MatrixType,2,0>(matrix);
236  result.coeffRef(1,2) = -cofactor_4x4<MatrixType,2,1>(matrix);
237  result.coeffRef(2,2) = cofactor_4x4<MatrixType,2,2>(matrix);
238  result.coeffRef(3,2) = -cofactor_4x4<MatrixType,2,3>(matrix);
239  result.coeffRef(0,1) = -cofactor_4x4<MatrixType,1,0>(matrix);
240  result.coeffRef(1,1) = cofactor_4x4<MatrixType,1,1>(matrix);
241  result.coeffRef(2,1) = -cofactor_4x4<MatrixType,1,2>(matrix);
242  result.coeffRef(3,1) = cofactor_4x4<MatrixType,1,3>(matrix);
243  result.coeffRef(0,3) = -cofactor_4x4<MatrixType,3,0>(matrix);
244  result.coeffRef(1,3) = cofactor_4x4<MatrixType,3,1>(matrix);
245  result.coeffRef(2,3) = -cofactor_4x4<MatrixType,3,2>(matrix);
246  result.coeffRef(3,3) = cofactor_4x4<MatrixType,3,3>(matrix);
247  result /= (matrix.col(0).cwiseProduct(result.row(0).transpose())).sum();
248  }
249 };
250 
251 template<typename MatrixType, typename ResultType>
252 struct compute_inverse<MatrixType, ResultType, 4>
253  : compute_inverse_size4<Architecture::Target, typename MatrixType::Scalar,
254  MatrixType, ResultType>
255 {
256 };
257 
258 template<typename MatrixType, typename ResultType>
259 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 4>
260 {
261  static inline void run(
262  const MatrixType& matrix,
263  const typename MatrixType::RealScalar& absDeterminantThreshold,
264  ResultType& inverse,
265  typename ResultType::Scalar& determinant,
266  bool& invertible
267  )
268  {
269  determinant = matrix.determinant();
270  invertible = abs(determinant) > absDeterminantThreshold;
271  if(invertible) compute_inverse<MatrixType, ResultType>::run(matrix, inverse);
272  }
273 };
274 
275 /*************************
276 *** MatrixBase methods ***
277 *************************/
278 
279 template<typename MatrixType>
280 struct traits<inverse_impl<MatrixType> >
281 {
282  typedef typename MatrixType::PlainObject ReturnType;
283 };
284 
285 template<typename MatrixType>
286 struct inverse_impl : public ReturnByValue<inverse_impl<MatrixType> >
287 {
288  typedef typename MatrixType::Index Index;
289  typedef typename internal::eval<MatrixType>::type MatrixTypeNested;
290  typedef typename remove_all<MatrixTypeNested>::type MatrixTypeNestedCleaned;
291  MatrixTypeNested m_matrix;
292 
293  inverse_impl(const MatrixType& matrix)
294  : m_matrix(matrix)
295  {}
296 
297  inline Index rows() const { return m_matrix.rows(); }
298  inline Index cols() const { return m_matrix.cols(); }
299 
300  template<typename Dest> inline void evalTo(Dest& dst) const
301  {
302  const int Size = EIGEN_PLAIN_ENUM_MIN(MatrixType::ColsAtCompileTime,Dest::ColsAtCompileTime);
304  eigen_assert(( (Size<=1) || (Size>4) || (extract_data(m_matrix)!=extract_data(dst)))
305  && "Aliasing problem detected in inverse(), you need to do inverse().eval() here.");
306 
307  compute_inverse<MatrixTypeNestedCleaned, Dest>::run(m_matrix, dst);
308  }
309 };
310 
311 } // end namespace internal
312 
330 template<typename Derived>
331 inline const internal::inverse_impl<Derived> MatrixBase<Derived>::inverse() const
332 {
333  EIGEN_STATIC_ASSERT(!NumTraits<Scalar>::IsInteger,THIS_FUNCTION_IS_NOT_FOR_INTEGER_NUMERIC_TYPES)
334  eigen_assert(rows() == cols());
335  return internal::inverse_impl<Derived>(derived());
336 }
337 
356 template<typename Derived>
357 template<typename ResultType>
359  ResultType& inverse,
360  typename ResultType::Scalar& determinant,
361  bool& invertible,
362  const RealScalar& absDeterminantThreshold
363  ) const
364 {
365  // i'd love to put some static assertions there, but SFINAE means that they have no effect...
366  eigen_assert(rows() == cols());
367  // for 2x2, it's worth giving a chance to avoid evaluating.
368  // for larger sizes, evaluating has negligible cost and limits code size.
369  typedef typename internal::conditional<
370  RowsAtCompileTime == 2,
371  typename internal::remove_all<typename internal::nested<Derived, 2>::type>::type,
373  >::type MatrixType;
374  internal::compute_inverse_and_det_with_check<MatrixType, ResultType>::run
375  (derived(), absDeterminantThreshold, inverse, determinant, invertible);
376 }
377 
395 template<typename Derived>
396 template<typename ResultType>
398  ResultType& inverse,
399  bool& invertible,
400  const RealScalar& absDeterminantThreshold
401  ) const
402 {
403  RealScalar determinant;
404  // i'd love to put some static assertions there, but SFINAE means that they have no effect...
405  eigen_assert(rows() == cols());
406  computeInverseAndDetWithCheck(inverse,determinant,invertible,absDeterminantThreshold);
407 }
408 
409 } // end namespace Eigen
410 
411 #endif // EIGEN_INVERSE_H