RavEngine
Loading...
Searching...
No Matches
InverseImpl.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// 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 typename ResultType::Scalar temp = matrix.coeff(0,0);
81 result.coeffRef(0,0) = matrix.coeff(1,1) * invdet;
82 result.coeffRef(1,0) = -matrix.coeff(1,0) * invdet;
83 result.coeffRef(0,1) = -matrix.coeff(0,1) * invdet;
84 result.coeffRef(1,1) = temp * invdet;
85}
86
87template<typename MatrixType, typename ResultType>
88struct compute_inverse<MatrixType, ResultType, 2>
89{
90 EIGEN_DEVICE_FUNC
91 static inline void run(const MatrixType& matrix, ResultType& result)
92 {
93 typedef typename ResultType::Scalar Scalar;
94 const Scalar invdet = typename MatrixType::Scalar(1) / matrix.determinant();
95 compute_inverse_size2_helper(matrix, invdet, result);
96 }
97};
98
99template<typename MatrixType, typename ResultType>
100struct compute_inverse_and_det_with_check<MatrixType, ResultType, 2>
101{
102 EIGEN_DEVICE_FUNC
103 static inline void run(
104 const MatrixType& matrix,
105 const typename MatrixType::RealScalar& absDeterminantThreshold,
106 ResultType& inverse,
107 typename ResultType::Scalar& determinant,
108 bool& invertible
109 )
110 {
111 using std::abs;
112 typedef typename ResultType::Scalar Scalar;
113 determinant = matrix.determinant();
114 invertible = abs(determinant) > absDeterminantThreshold;
115 if(!invertible) return;
116 const Scalar invdet = Scalar(1) / determinant;
117 compute_inverse_size2_helper(matrix, invdet, inverse);
118 }
119};
120
121/****************************
122*** Size 3 implementation ***
123****************************/
124
125template<typename MatrixType, int i, int j>
126EIGEN_DEVICE_FUNC
127inline typename MatrixType::Scalar cofactor_3x3(const MatrixType& m)
128{
129 enum {
130 i1 = (i+1) % 3,
131 i2 = (i+2) % 3,
132 j1 = (j+1) % 3,
133 j2 = (j+2) % 3
134 };
135 return m.coeff(i1, j1) * m.coeff(i2, j2)
136 - m.coeff(i1, j2) * m.coeff(i2, j1);
137}
138
139template<typename MatrixType, typename ResultType>
140EIGEN_DEVICE_FUNC
141inline void compute_inverse_size3_helper(
142 const MatrixType& matrix,
143 const typename ResultType::Scalar& invdet,
144 const Matrix<typename ResultType::Scalar,3,1>& cofactors_col0,
145 ResultType& result)
146{
147 // Compute cofactors in a way that avoids aliasing issues.
148 typedef typename ResultType::Scalar Scalar;
149 const Scalar c01 = cofactor_3x3<MatrixType,0,1>(matrix) * invdet;
150 const Scalar c11 = cofactor_3x3<MatrixType,1,1>(matrix) * invdet;
151 const Scalar c02 = cofactor_3x3<MatrixType,0,2>(matrix) * invdet;
152 result.coeffRef(1,2) = cofactor_3x3<MatrixType,2,1>(matrix) * invdet;
153 result.coeffRef(2,1) = cofactor_3x3<MatrixType,1,2>(matrix) * invdet;
154 result.coeffRef(2,2) = cofactor_3x3<MatrixType,2,2>(matrix) * invdet;
155 result.coeffRef(1,0) = c01;
156 result.coeffRef(1,1) = c11;
157 result.coeffRef(2,0) = c02;
158 result.row(0) = cofactors_col0 * invdet;
159}
160
161template<typename MatrixType, typename ResultType>
162struct compute_inverse<MatrixType, ResultType, 3>
163{
164 EIGEN_DEVICE_FUNC
165 static inline void run(const MatrixType& matrix, ResultType& result)
166 {
167 typedef typename ResultType::Scalar Scalar;
168 Matrix<typename MatrixType::Scalar,3,1> cofactors_col0;
169 cofactors_col0.coeffRef(0) = cofactor_3x3<MatrixType,0,0>(matrix);
170 cofactors_col0.coeffRef(1) = cofactor_3x3<MatrixType,1,0>(matrix);
171 cofactors_col0.coeffRef(2) = cofactor_3x3<MatrixType,2,0>(matrix);
172 const Scalar det = (cofactors_col0.cwiseProduct(matrix.col(0))).sum();
173 const Scalar invdet = Scalar(1) / det;
174 compute_inverse_size3_helper(matrix, invdet, cofactors_col0, result);
175 }
176};
177
178template<typename MatrixType, typename ResultType>
179struct compute_inverse_and_det_with_check<MatrixType, ResultType, 3>
180{
181 EIGEN_DEVICE_FUNC
182 static inline void run(
183 const MatrixType& matrix,
184 const typename MatrixType::RealScalar& absDeterminantThreshold,
185 ResultType& inverse,
186 typename ResultType::Scalar& determinant,
187 bool& invertible
188 )
189 {
190 typedef typename ResultType::Scalar Scalar;
191 Matrix<Scalar,3,1> cofactors_col0;
192 cofactors_col0.coeffRef(0) = cofactor_3x3<MatrixType,0,0>(matrix);
193 cofactors_col0.coeffRef(1) = cofactor_3x3<MatrixType,1,0>(matrix);
194 cofactors_col0.coeffRef(2) = cofactor_3x3<MatrixType,2,0>(matrix);
195 determinant = (cofactors_col0.cwiseProduct(matrix.col(0))).sum();
196 invertible = Eigen::numext::abs(determinant) > absDeterminantThreshold;
197 if(!invertible) return;
198 const Scalar invdet = Scalar(1) / determinant;
199 compute_inverse_size3_helper(matrix, invdet, cofactors_col0, inverse);
200 }
201};
202
203/****************************
204*** Size 4 implementation ***
205****************************/
206
207template<typename Derived>
208EIGEN_DEVICE_FUNC
209inline const typename Derived::Scalar general_det3_helper
210(const MatrixBase<Derived>& matrix, int i1, int i2, int i3, int j1, int j2, int j3)
211{
212 return matrix.coeff(i1,j1)
213 * (matrix.coeff(i2,j2) * matrix.coeff(i3,j3) - matrix.coeff(i2,j3) * matrix.coeff(i3,j2));
214}
215
216template<typename MatrixType, int i, int j>
217EIGEN_DEVICE_FUNC
218inline typename MatrixType::Scalar cofactor_4x4(const MatrixType& matrix)
219{
220 enum {
221 i1 = (i+1) % 4,
222 i2 = (i+2) % 4,
223 i3 = (i+3) % 4,
224 j1 = (j+1) % 4,
225 j2 = (j+2) % 4,
226 j3 = (j+3) % 4
227 };
228 return general_det3_helper(matrix, i1, i2, i3, j1, j2, j3)
229 + general_det3_helper(matrix, i2, i3, i1, j1, j2, j3)
230 + general_det3_helper(matrix, i3, i1, i2, j1, j2, j3);
231}
232
233template<int Arch, typename Scalar, typename MatrixType, typename ResultType>
234struct compute_inverse_size4
235{
236 EIGEN_DEVICE_FUNC
237 static void run(const MatrixType& matrix, ResultType& result)
238 {
239 result.coeffRef(0,0) = cofactor_4x4<MatrixType,0,0>(matrix);
240 result.coeffRef(1,0) = -cofactor_4x4<MatrixType,0,1>(matrix);
241 result.coeffRef(2,0) = cofactor_4x4<MatrixType,0,2>(matrix);
242 result.coeffRef(3,0) = -cofactor_4x4<MatrixType,0,3>(matrix);
243 result.coeffRef(0,2) = cofactor_4x4<MatrixType,2,0>(matrix);
244 result.coeffRef(1,2) = -cofactor_4x4<MatrixType,2,1>(matrix);
245 result.coeffRef(2,2) = cofactor_4x4<MatrixType,2,2>(matrix);
246 result.coeffRef(3,2) = -cofactor_4x4<MatrixType,2,3>(matrix);
247 result.coeffRef(0,1) = -cofactor_4x4<MatrixType,1,0>(matrix);
248 result.coeffRef(1,1) = cofactor_4x4<MatrixType,1,1>(matrix);
249 result.coeffRef(2,1) = -cofactor_4x4<MatrixType,1,2>(matrix);
250 result.coeffRef(3,1) = cofactor_4x4<MatrixType,1,3>(matrix);
251 result.coeffRef(0,3) = -cofactor_4x4<MatrixType,3,0>(matrix);
252 result.coeffRef(1,3) = cofactor_4x4<MatrixType,3,1>(matrix);
253 result.coeffRef(2,3) = -cofactor_4x4<MatrixType,3,2>(matrix);
254 result.coeffRef(3,3) = cofactor_4x4<MatrixType,3,3>(matrix);
255 result /= (matrix.col(0).cwiseProduct(result.row(0).transpose())).sum();
256 }
257};
258
259template<typename MatrixType, typename ResultType>
260struct compute_inverse<MatrixType, ResultType, 4>
261 : compute_inverse_size4<Architecture::Target, typename MatrixType::Scalar,
262 MatrixType, ResultType>
263{
264};
265
266template<typename MatrixType, typename ResultType>
267struct compute_inverse_and_det_with_check<MatrixType, ResultType, 4>
268{
269 EIGEN_DEVICE_FUNC
270 static inline void run(
271 const MatrixType& matrix,
272 const typename MatrixType::RealScalar& absDeterminantThreshold,
273 ResultType& inverse,
274 typename ResultType::Scalar& determinant,
275 bool& invertible
276 )
277 {
278 using std::abs;
279 determinant = matrix.determinant();
280 invertible = abs(determinant) > absDeterminantThreshold;
281 if(invertible && extract_data(matrix) != extract_data(inverse)) {
282 compute_inverse<MatrixType, ResultType>::run(matrix, inverse);
283 }
284 else if(invertible) {
285 MatrixType matrix_t = matrix;
286 compute_inverse<MatrixType, ResultType>::run(matrix_t, inverse);
287 }
288 }
289};
290
291/*************************
292*** MatrixBase methods ***
293*************************/
294
295} // end namespace internal
296
297namespace internal {
298
299// Specialization for "dense = dense_xpr.inverse()"
300template<typename DstXprType, typename XprType>
301struct Assignment<DstXprType, Inverse<XprType>, internal::assign_op<typename DstXprType::Scalar,typename XprType::Scalar>, Dense2Dense>
302{
303 typedef Inverse<XprType> SrcXprType;
304 EIGEN_DEVICE_FUNC
305 static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<typename DstXprType::Scalar,typename XprType::Scalar> &)
306 {
307 Index dstRows = src.rows();
308 Index dstCols = src.cols();
309 if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
310 dst.resize(dstRows, dstCols);
311
312 const int Size = EIGEN_PLAIN_ENUM_MIN(XprType::ColsAtCompileTime,DstXprType::ColsAtCompileTime);
313 EIGEN_ONLY_USED_FOR_DEBUG(Size);
314 eigen_assert(( (Size<=1) || (Size>4) || (extract_data(src.nestedExpression())!=extract_data(dst)))
315 && "Aliasing problem detected in inverse(), you need to do inverse().eval() here.");
316
317 typedef typename internal::nested_eval<XprType,XprType::ColsAtCompileTime>::type ActualXprType;
318 typedef typename internal::remove_all<ActualXprType>::type ActualXprTypeCleanded;
319
320 ActualXprType actual_xpr(src.nestedExpression());
321
322 compute_inverse<ActualXprTypeCleanded, DstXprType>::run(actual_xpr, dst);
323 }
324};
325
326
327} // end namespace internal
328
346template<typename Derived>
347EIGEN_DEVICE_FUNC
348inline const Inverse<Derived> MatrixBase<Derived>::inverse() const
349{
350 EIGEN_STATIC_ASSERT(!NumTraits<Scalar>::IsInteger,THIS_FUNCTION_IS_NOT_FOR_INTEGER_NUMERIC_TYPES)
351 eigen_assert(rows() == cols());
352 return Inverse<Derived>(derived());
353}
354
375template<typename Derived>
376template<typename ResultType>
377inline void MatrixBase<Derived>::computeInverseAndDetWithCheck(
378 ResultType& inverse,
379 typename ResultType::Scalar& determinant,
380 bool& invertible,
381 const RealScalar& absDeterminantThreshold
382 ) const
383{
384 // i'd love to put some static assertions there, but SFINAE means that they have no effect...
385 eigen_assert(rows() == cols());
386 // for 2x2, it's worth giving a chance to avoid evaluating.
387 // for larger sizes, evaluating has negligible cost and limits code size.
388 typedef typename internal::conditional<
389 RowsAtCompileTime == 2,
390 typename internal::remove_all<typename internal::nested_eval<Derived, 2>::type>::type,
391 PlainObject
392 >::type MatrixType;
393 internal::compute_inverse_and_det_with_check<MatrixType, ResultType>::run
394 (derived(), absDeterminantThreshold, inverse, determinant, invertible);
395}
396
416template<typename Derived>
417template<typename ResultType>
418inline void MatrixBase<Derived>::computeInverseWithCheck(
419 ResultType& inverse,
420 bool& invertible,
421 const RealScalar& absDeterminantThreshold
422 ) const
423{
424 Scalar determinant;
425 // i'd love to put some static assertions there, but SFINAE means that they have no effect...
426 eigen_assert(rows() == cols());
427 computeInverseAndDetWithCheck(inverse,determinant,invertible,absDeterminantThreshold);
428}
429
430} // end namespace Eigen
431
432#endif // EIGEN_INVERSE_IMPL_H
Eigen::Inverse
Expression of the inverse of another expression.
Definition Inverse.h:44
Eigen::MatrixBase
Base class for all dense matrices, vectors, and expressions.
Definition MatrixBase.h:50
Eigen::MatrixBase::computeInverseWithCheck
void computeInverseWithCheck(ResultType &inverse, bool &invertible, const RealScalar &absDeterminantThreshold=NumTraits< Scalar >::dummy_precision()) const
Definition InverseImpl.h:418
Eigen::MatrixBase::computeInverseAndDetWithCheck
void computeInverseAndDetWithCheck(ResultType &inverse, typename ResultType::Scalar &determinant, bool &invertible, const RealScalar &absDeterminantThreshold=NumTraits< Scalar >::dummy_precision()) const
Definition InverseImpl.h:377
Eigen::MatrixBase::inverse
EIGEN_DEVICE_FUNC const Inverse< Derived > inverse() const
Definition InverseImpl.h:348
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition Matrix.h:180
Eigen::Matrix::coeffRef
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar & coeffRef(Index rowId, Index colId)
Definition PlainObjectBase.h:175
Eigen
Namespace containing all symbols from the Eigen library.
Definition common.h:81
Eigen::Index
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition Meta.h:74
Eigen::NumTraits
Holds information about the various numeric (i.e. scalar) types allowed by Eigen.
Definition NumTraits.h:233
Eigen::internal::Assignment
Definition AssignEvaluator.h:824
Eigen::internal::Dense2Dense
Definition AssignEvaluator.h:814
Eigen::internal::assign_op
Definition AssignmentFunctors.h:21
Eigen::internal::compute_inverse_and_det_with_check
Definition InverseImpl.h:33
Eigen::internal::compute_inverse_size4
Definition InverseImpl.h:235
Eigen::internal::compute_inverse
Definition InverseImpl.h:24
Eigen::internal::conditional
Definition Meta.h:109
Eigen::internal::evaluator
Definition CoreEvaluators.h:91
Eigen::internal::remove_all
Definition Meta.h:126
Eigen::internal::true_type
Definition Meta.h:96