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 | |
---|
298 | /** \lu_module |
---|
299 | * |
---|
300 | * \returns the matrix inverse of this matrix. |
---|
301 | * |
---|
302 | * For small fixed sizes up to 4x4, this method uses cofactors. |
---|
303 | * In the general case, this method uses class PartialPivLU. |
---|
304 | * |
---|
305 | * \note This matrix must be invertible, otherwise the result is undefined. If you need an |
---|
306 | * invertibility check, do the following: |
---|
307 | * \li for fixed sizes up to 4x4, use computeInverseAndDetWithCheck(). |
---|
308 | * \li for the general case, use class FullPivLU. |
---|
309 | * |
---|
310 | * Example: \include MatrixBase_inverse.cpp |
---|
311 | * Output: \verbinclude MatrixBase_inverse.out |
---|
312 | * |
---|
313 | * \sa computeInverseAndDetWithCheck() |
---|
314 | */ |
---|
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 | |
---|
323 | /** \lu_module |
---|
324 | * |
---|
325 | * Computation of matrix inverse and determinant, with invertibility check. |
---|
326 | * |
---|
327 | * This is only for fixed-size square matrices of size up to 4x4. |
---|
328 | * |
---|
329 | * \param inverse Reference to the matrix in which to store the inverse. |
---|
330 | * \param determinant Reference to the variable in which to store the inverse. |
---|
331 | * \param invertible Reference to the bool variable in which to store whether the matrix is invertible. |
---|
332 | * \param absDeterminantThreshold Optional parameter controlling the invertibility check. |
---|
333 | * The matrix will be declared invertible if the absolute value of its |
---|
334 | * determinant is greater than this threshold. |
---|
335 | * |
---|
336 | * Example: \include MatrixBase_computeInverseAndDetWithCheck.cpp |
---|
337 | * Output: \verbinclude MatrixBase_computeInverseAndDetWithCheck.out |
---|
338 | * |
---|
339 | * \sa inverse(), computeInverseWithCheck() |
---|
340 | */ |
---|
341 | template<typename Derived> |
---|
342 | template<typename ResultType> |
---|
343 | inline void MatrixBase<Derived>::computeInverseAndDetWithCheck( |
---|
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, |
---|
357 | PlainObject |
---|
358 | >::type MatrixType; |
---|
359 | internal::compute_inverse_and_det_with_check<MatrixType, ResultType>::run |
---|
360 | (derived(), absDeterminantThreshold, inverse, determinant, invertible); |
---|
361 | } |
---|
362 | |
---|
363 | /** \lu_module |
---|
364 | * |
---|
365 | * Computation of matrix inverse, with invertibility check. |
---|
366 | * |
---|
367 | * This is only for fixed-size square matrices of size up to 4x4. |
---|
368 | * |
---|
369 | * \param inverse Reference to the matrix in which to store the inverse. |
---|
370 | * \param invertible Reference to the bool variable in which to store whether the matrix is invertible. |
---|
371 | * \param absDeterminantThreshold Optional parameter controlling the invertibility check. |
---|
372 | * The matrix will be declared invertible if the absolute value of its |
---|
373 | * determinant is greater than this threshold. |
---|
374 | * |
---|
375 | * Example: \include MatrixBase_computeInverseWithCheck.cpp |
---|
376 | * Output: \verbinclude MatrixBase_computeInverseWithCheck.out |
---|
377 | * |
---|
378 | * \sa inverse(), computeInverseAndDetWithCheck() |
---|
379 | */ |
---|
380 | template<typename Derived> |
---|
381 | template<typename ResultType> |
---|
382 | inline void MatrixBase<Derived>::computeInverseWithCheck( |
---|
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 |
---|