25 #ifndef EIGEN_INVERSE_H
26 #define EIGEN_INVERSE_H
36 template<
typename MatrixType,
typename ResultType,
int Size = MatrixType::RowsAtCompileTime>
37 struct compute_inverse
39 static inline void run(
const MatrixType& matrix, ResultType& result)
41 result = matrix.partialPivLu().inverse();
45 template<
typename MatrixType,
typename ResultType,
int Size = MatrixType::RowsAtCompileTime>
46 struct compute_inverse_and_det_with_check { };
52 template<
typename MatrixType,
typename ResultType>
53 struct compute_inverse<MatrixType, ResultType, 1>
55 static inline void run(
const MatrixType& matrix, ResultType& result)
57 typedef typename MatrixType::Scalar Scalar;
58 result.coeffRef(0,0) = Scalar(1) / matrix.coeff(0,0);
62 template<
typename MatrixType,
typename ResultType>
63 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 1>
65 static inline void run(
66 const MatrixType& matrix,
67 const typename MatrixType::RealScalar& absDeterminantThreshold,
69 typename ResultType::Scalar& determinant,
73 determinant = matrix.coeff(0,0);
74 invertible =
abs(determinant) > absDeterminantThreshold;
75 if(invertible) result.coeffRef(0,0) =
typename ResultType::Scalar(1) / determinant;
83 template<
typename MatrixType,
typename ResultType>
85 const MatrixType& matrix,
const typename ResultType::Scalar& invdet,
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;
94 template<
typename MatrixType,
typename ResultType>
95 struct compute_inverse<MatrixType, ResultType, 2>
97 static inline void run(
const MatrixType& matrix, ResultType& result)
99 typedef typename ResultType::Scalar Scalar;
100 const Scalar invdet =
typename MatrixType::Scalar(1) / matrix.determinant();
105 template<
typename MatrixType,
typename ResultType>
106 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 2>
108 static inline void run(
109 const MatrixType& matrix,
110 const typename MatrixType::RealScalar& absDeterminantThreshold,
112 typename ResultType::Scalar& determinant,
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;
129 template<
typename MatrixType,
int i,
int j>
138 return m.coeff(i1, j1) * m.coeff(i2, j2)
139 - m.coeff(i1, j2) * m.coeff(i2, j1);
142 template<
typename MatrixType,
typename ResultType>
144 const MatrixType& matrix,
145 const typename ResultType::Scalar& invdet,
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;
158 template<
typename MatrixType,
typename ResultType>
159 struct compute_inverse<MatrixType, ResultType, 3>
161 static inline void run(
const MatrixType& matrix, ResultType& result)
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;
174 template<
typename MatrixType,
typename ResultType>
175 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 3>
177 static inline void run(
178 const MatrixType& matrix,
179 const typename MatrixType::RealScalar& absDeterminantThreshold,
181 typename ResultType::Scalar& determinant,
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;
202 template<
typename Derived>
206 return matrix.coeff(i1,j1)
207 * (matrix.coeff(i2,j2) * matrix.coeff(i3,j3) - matrix.coeff(i2,j3) * matrix.coeff(i3,j2));
210 template<
typename MatrixType,
int i,
int j>
211 inline typename MatrixType::Scalar
cofactor_4x4(
const MatrixType& matrix)
226 template<
int Arch,
typename Scalar,
typename MatrixType,
typename ResultType>
227 struct compute_inverse_size4
229 static void run(
const MatrixType& matrix, ResultType& result)
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();
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>
258 template<
typename MatrixType,
typename ResultType>
259 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 4>
261 static inline void run(
262 const MatrixType& matrix,
263 const typename MatrixType::RealScalar& absDeterminantThreshold,
265 typename ResultType::Scalar& determinant,
269 determinant = matrix.determinant();
270 invertible =
abs(determinant) > absDeterminantThreshold;
271 if(invertible) compute_inverse<MatrixType, ResultType>::run(matrix, inverse);
279 template<
typename MatrixType>
280 struct traits<inverse_impl<MatrixType> >
282 typedef typename MatrixType::PlainObject ReturnType;
285 template<
typename MatrixType>
286 struct inverse_impl :
public ReturnByValue<inverse_impl<MatrixType> >
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;
293 inverse_impl(
const MatrixType& matrix)
297 inline Index
rows()
const {
return m_matrix.rows(); }
298 inline Index
cols()
const {
return m_matrix.cols(); }
300 template<
typename Dest>
inline void evalTo(Dest& dst)
const
305 &&
"Aliasing problem detected in inverse(), you need to do inverse().eval() here.");
307 compute_inverse<MatrixTypeNestedCleaned, Dest>::run(m_matrix, dst);
330 template<
typename Derived>
335 return internal::inverse_impl<Derived>(derived());
356 template<
typename Derived>
357 template<
typename ResultType>
360 typename ResultType::Scalar& determinant,
369 typedef typename internal::conditional<
370 RowsAtCompileTime == 2,
371 typename internal::remove_all<typename internal::nested<Derived, 2>::type>::type,
374 internal::compute_inverse_and_det_with_check<MatrixType, ResultType>::run
375 (derived(), absDeterminantThreshold, inverse, determinant, invertible);
395 template<
typename Derived>
396 template<
typename ResultType>
406 computeInverseAndDetWithCheck(inverse,determinant,invertible,absDeterminantThreshold);
411 #endif // EIGEN_INVERSE_H