10 #ifndef EIGEN_INVERSE_H
11 #define EIGEN_INVERSE_H
21 template<
typename MatrixType,
typename ResultType,
int Size = MatrixType::RowsAtCompileTime>
22 struct compute_inverse
24 static inline void run(
const MatrixType& matrix, ResultType& result)
26 result = matrix.partialPivLu().inverse();
30 template<
typename MatrixType,
typename ResultType,
int Size = MatrixType::RowsAtCompileTime>
31 struct compute_inverse_and_det_with_check { };
37 template<
typename MatrixType,
typename ResultType>
38 struct compute_inverse<MatrixType, ResultType, 1>
40 static inline void run(
const MatrixType& matrix, ResultType& result)
42 typedef typename MatrixType::Scalar Scalar;
43 result.coeffRef(0,0) = Scalar(1) / matrix.coeff(0,0);
47 template<
typename MatrixType,
typename ResultType>
48 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 1>
50 static inline void run(
51 const MatrixType& matrix,
52 const typename MatrixType::RealScalar& absDeterminantThreshold,
54 typename ResultType::Scalar& determinant,
58 determinant = matrix.coeff(0,0);
59 invertible =
abs(determinant) > absDeterminantThreshold;
60 if(invertible) result.coeffRef(0,0) =
typename ResultType::Scalar(1) / determinant;
68 template<
typename MatrixType,
typename ResultType>
69 inline void compute_inverse_size2_helper(
70 const MatrixType& matrix,
const typename ResultType::Scalar& invdet,
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;
79 template<
typename MatrixType,
typename ResultType>
80 struct compute_inverse<MatrixType, ResultType, 2>
82 static inline void run(
const MatrixType& matrix, ResultType& result)
84 typedef typename ResultType::Scalar Scalar;
85 const Scalar invdet =
typename MatrixType::Scalar(1) / matrix.determinant();
86 compute_inverse_size2_helper(matrix, invdet, result);
90 template<
typename MatrixType,
typename ResultType>
91 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 2>
93 static inline void run(
94 const MatrixType& matrix,
95 const typename MatrixType::RealScalar& absDeterminantThreshold,
97 typename ResultType::Scalar& determinant,
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);
114 template<
typename MatrixType,
int i,
int j>
115 inline typename MatrixType::Scalar cofactor_3x3(
const MatrixType& m)
123 return m.coeff(i1, j1) * m.coeff(i2, j2)
124 - m.coeff(i1, j2) * m.coeff(i2, j1);
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,
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;
143 template<
typename MatrixType,
typename ResultType>
144 struct compute_inverse<MatrixType, ResultType, 3>
146 static inline void run(
const MatrixType& matrix, ResultType& result)
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);
159 template<
typename MatrixType,
typename ResultType>
160 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 3>
162 static inline void run(
163 const MatrixType& matrix,
164 const typename MatrixType::RealScalar& absDeterminantThreshold,
166 typename ResultType::Scalar& determinant,
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);
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)
191 return matrix.coeff(i1,j1)
192 * (matrix.coeff(i2,j2) * matrix.coeff(i3,j3) - matrix.coeff(i2,j3) * matrix.coeff(i3,j2));
195 template<
typename MatrixType,
int i,
int j>
196 inline typename MatrixType::Scalar cofactor_4x4(
const MatrixType& matrix)
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);
211 template<
int Arch,
typename Scalar,
typename MatrixType,
typename ResultType>
212 struct compute_inverse_size4
214 static void run(
const MatrixType& matrix, ResultType& result)
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();
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>
243 template<
typename MatrixType,
typename ResultType>
244 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 4>
246 static inline void run(
247 const MatrixType& matrix,
248 const typename MatrixType::RealScalar& absDeterminantThreshold,
250 typename ResultType::Scalar& determinant,
254 determinant = matrix.determinant();
255 invertible =
abs(determinant) > absDeterminantThreshold;
256 if(invertible) compute_inverse<MatrixType, ResultType>::run(matrix, inverse);
264 template<
typename MatrixType>
265 struct traits<inverse_impl<MatrixType> >
267 typedef typename MatrixType::PlainObject ReturnType;
270 template<
typename MatrixType>
271 struct inverse_impl :
public ReturnByValue<inverse_impl<MatrixType> >
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;
278 inverse_impl(
const MatrixType& matrix)
282 inline Index rows()
const {
return m_matrix.rows(); }
283 inline Index cols()
const {
return m_matrix.cols(); }
285 template<
typename Dest>
inline void evalTo(Dest& dst)
const
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.");
292 compute_inverse<MatrixTypeNestedCleaned, Dest>::run(m_matrix, dst);
315 template<
typename Derived>
319 eigen_assert(rows() == cols());
320 return internal::inverse_impl<Derived>(derived());
341 template<
typename Derived>
342 template<
typename ResultType>
345 typename ResultType::Scalar& determinant,
347 const RealScalar& absDeterminantThreshold
351 eigen_assert(rows() == cols());
354 typedef typename internal::conditional<
355 RowsAtCompileTime == 2,
356 typename internal::remove_all<typename internal::nested<Derived, 2>::type>::type,
359 internal::compute_inverse_and_det_with_check<MatrixType, ResultType>::run
360 (derived(), absDeterminantThreshold, inverse, determinant, invertible);
380 template<
typename Derived>
381 template<
typename ResultType>
385 const RealScalar& absDeterminantThreshold
388 RealScalar determinant;
390 eigen_assert(rows() == cols());
391 computeInverseAndDetWithCheck(inverse,determinant,invertible,absDeterminantThreshold);
396 #endif // EIGEN_INVERSE_H