00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #ifndef EIGEN_INVERSE_H
00026 #define EIGEN_INVERSE_H
00027
00028 namespace Eigen {
00029
00030 namespace internal {
00031
00032
00033
00034
00035
00036 template<typename MatrixType, typename ResultType, int Size = MatrixType::RowsAtCompileTime>
00037 struct compute_inverse
00038 {
00039 static inline void run(const MatrixType& matrix, ResultType& result)
00040 {
00041 result = matrix.partialPivLu().inverse();
00042 }
00043 };
00044
00045 template<typename MatrixType, typename ResultType, int Size = MatrixType::RowsAtCompileTime>
00046 struct compute_inverse_and_det_with_check { };
00047
00048
00049
00050
00051
00052 template<typename MatrixType, typename ResultType>
00053 struct compute_inverse<MatrixType, ResultType, 1>
00054 {
00055 static inline void run(const MatrixType& matrix, ResultType& result)
00056 {
00057 typedef typename MatrixType::Scalar Scalar;
00058 result.coeffRef(0,0) = Scalar(1) / matrix.coeff(0,0);
00059 }
00060 };
00061
00062 template<typename MatrixType, typename ResultType>
00063 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 1>
00064 {
00065 static inline void run(
00066 const MatrixType& matrix,
00067 const typename MatrixType::RealScalar& absDeterminantThreshold,
00068 ResultType& result,
00069 typename ResultType::Scalar& determinant,
00070 bool& invertible
00071 )
00072 {
00073 determinant = matrix.coeff(0,0);
00074 invertible = abs(determinant) > absDeterminantThreshold;
00075 if(invertible) result.coeffRef(0,0) = typename ResultType::Scalar(1) / determinant;
00076 }
00077 };
00078
00079
00080
00081
00082
00083 template<typename MatrixType, typename ResultType>
00084 inline void compute_inverse_size2_helper(
00085 const MatrixType& matrix, const typename ResultType::Scalar& invdet,
00086 ResultType& result)
00087 {
00088 result.coeffRef(0,0) = matrix.coeff(1,1) * invdet;
00089 result.coeffRef(1,0) = -matrix.coeff(1,0) * invdet;
00090 result.coeffRef(0,1) = -matrix.coeff(0,1) * invdet;
00091 result.coeffRef(1,1) = matrix.coeff(0,0) * invdet;
00092 }
00093
00094 template<typename MatrixType, typename ResultType>
00095 struct compute_inverse<MatrixType, ResultType, 2>
00096 {
00097 static inline void run(const MatrixType& matrix, ResultType& result)
00098 {
00099 typedef typename ResultType::Scalar Scalar;
00100 const Scalar invdet = typename MatrixType::Scalar(1) / matrix.determinant();
00101 compute_inverse_size2_helper(matrix, invdet, result);
00102 }
00103 };
00104
00105 template<typename MatrixType, typename ResultType>
00106 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 2>
00107 {
00108 static inline void run(
00109 const MatrixType& matrix,
00110 const typename MatrixType::RealScalar& absDeterminantThreshold,
00111 ResultType& inverse,
00112 typename ResultType::Scalar& determinant,
00113 bool& invertible
00114 )
00115 {
00116 typedef typename ResultType::Scalar Scalar;
00117 determinant = matrix.determinant();
00118 invertible = abs(determinant) > absDeterminantThreshold;
00119 if(!invertible) return;
00120 const Scalar invdet = Scalar(1) / determinant;
00121 compute_inverse_size2_helper(matrix, invdet, inverse);
00122 }
00123 };
00124
00125
00126
00127
00128
00129 template<typename MatrixType, int i, int j>
00130 inline typename MatrixType::Scalar cofactor_3x3(const MatrixType& m)
00131 {
00132 enum {
00133 i1 = (i+1) % 3,
00134 i2 = (i+2) % 3,
00135 j1 = (j+1) % 3,
00136 j2 = (j+2) % 3
00137 };
00138 return m.coeff(i1, j1) * m.coeff(i2, j2)
00139 - m.coeff(i1, j2) * m.coeff(i2, j1);
00140 }
00141
00142 template<typename MatrixType, typename ResultType>
00143 inline void compute_inverse_size3_helper(
00144 const MatrixType& matrix,
00145 const typename ResultType::Scalar& invdet,
00146 const Matrix<typename ResultType::Scalar,3,1>& cofactors_col0,
00147 ResultType& result)
00148 {
00149 result.row(0) = cofactors_col0 * invdet;
00150 result.coeffRef(1,0) = cofactor_3x3<MatrixType,0,1>(matrix) * invdet;
00151 result.coeffRef(1,1) = cofactor_3x3<MatrixType,1,1>(matrix) * invdet;
00152 result.coeffRef(1,2) = cofactor_3x3<MatrixType,2,1>(matrix) * invdet;
00153 result.coeffRef(2,0) = cofactor_3x3<MatrixType,0,2>(matrix) * invdet;
00154 result.coeffRef(2,1) = cofactor_3x3<MatrixType,1,2>(matrix) * invdet;
00155 result.coeffRef(2,2) = cofactor_3x3<MatrixType,2,2>(matrix) * invdet;
00156 }
00157
00158 template<typename MatrixType, typename ResultType>
00159 struct compute_inverse<MatrixType, ResultType, 3>
00160 {
00161 static inline void run(const MatrixType& matrix, ResultType& result)
00162 {
00163 typedef typename ResultType::Scalar Scalar;
00164 Matrix<typename MatrixType::Scalar,3,1> cofactors_col0;
00165 cofactors_col0.coeffRef(0) = cofactor_3x3<MatrixType,0,0>(matrix);
00166 cofactors_col0.coeffRef(1) = cofactor_3x3<MatrixType,1,0>(matrix);
00167 cofactors_col0.coeffRef(2) = cofactor_3x3<MatrixType,2,0>(matrix);
00168 const Scalar det = (cofactors_col0.cwiseProduct(matrix.col(0))).sum();
00169 const Scalar invdet = Scalar(1) / det;
00170 compute_inverse_size3_helper(matrix, invdet, cofactors_col0, result);
00171 }
00172 };
00173
00174 template<typename MatrixType, typename ResultType>
00175 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 3>
00176 {
00177 static inline void run(
00178 const MatrixType& matrix,
00179 const typename MatrixType::RealScalar& absDeterminantThreshold,
00180 ResultType& inverse,
00181 typename ResultType::Scalar& determinant,
00182 bool& invertible
00183 )
00184 {
00185 typedef typename ResultType::Scalar Scalar;
00186 Matrix<Scalar,3,1> cofactors_col0;
00187 cofactors_col0.coeffRef(0) = cofactor_3x3<MatrixType,0,0>(matrix);
00188 cofactors_col0.coeffRef(1) = cofactor_3x3<MatrixType,1,0>(matrix);
00189 cofactors_col0.coeffRef(2) = cofactor_3x3<MatrixType,2,0>(matrix);
00190 determinant = (cofactors_col0.cwiseProduct(matrix.col(0))).sum();
00191 invertible = abs(determinant) > absDeterminantThreshold;
00192 if(!invertible) return;
00193 const Scalar invdet = Scalar(1) / determinant;
00194 compute_inverse_size3_helper(matrix, invdet, cofactors_col0, inverse);
00195 }
00196 };
00197
00198
00199
00200
00201
00202 template<typename Derived>
00203 inline const typename Derived::Scalar general_det3_helper
00204 (const MatrixBase<Derived>& matrix, int i1, int i2, int i3, int j1, int j2, int j3)
00205 {
00206 return matrix.coeff(i1,j1)
00207 * (matrix.coeff(i2,j2) * matrix.coeff(i3,j3) - matrix.coeff(i2,j3) * matrix.coeff(i3,j2));
00208 }
00209
00210 template<typename MatrixType, int i, int j>
00211 inline typename MatrixType::Scalar cofactor_4x4(const MatrixType& matrix)
00212 {
00213 enum {
00214 i1 = (i+1) % 4,
00215 i2 = (i+2) % 4,
00216 i3 = (i+3) % 4,
00217 j1 = (j+1) % 4,
00218 j2 = (j+2) % 4,
00219 j3 = (j+3) % 4
00220 };
00221 return general_det3_helper(matrix, i1, i2, i3, j1, j2, j3)
00222 + general_det3_helper(matrix, i2, i3, i1, j1, j2, j3)
00223 + general_det3_helper(matrix, i3, i1, i2, j1, j2, j3);
00224 }
00225
00226 template<int Arch, typename Scalar, typename MatrixType, typename ResultType>
00227 struct compute_inverse_size4
00228 {
00229 static void run(const MatrixType& matrix, ResultType& result)
00230 {
00231 result.coeffRef(0,0) = cofactor_4x4<MatrixType,0,0>(matrix);
00232 result.coeffRef(1,0) = -cofactor_4x4<MatrixType,0,1>(matrix);
00233 result.coeffRef(2,0) = cofactor_4x4<MatrixType,0,2>(matrix);
00234 result.coeffRef(3,0) = -cofactor_4x4<MatrixType,0,3>(matrix);
00235 result.coeffRef(0,2) = cofactor_4x4<MatrixType,2,0>(matrix);
00236 result.coeffRef(1,2) = -cofactor_4x4<MatrixType,2,1>(matrix);
00237 result.coeffRef(2,2) = cofactor_4x4<MatrixType,2,2>(matrix);
00238 result.coeffRef(3,2) = -cofactor_4x4<MatrixType,2,3>(matrix);
00239 result.coeffRef(0,1) = -cofactor_4x4<MatrixType,1,0>(matrix);
00240 result.coeffRef(1,1) = cofactor_4x4<MatrixType,1,1>(matrix);
00241 result.coeffRef(2,1) = -cofactor_4x4<MatrixType,1,2>(matrix);
00242 result.coeffRef(3,1) = cofactor_4x4<MatrixType,1,3>(matrix);
00243 result.coeffRef(0,3) = -cofactor_4x4<MatrixType,3,0>(matrix);
00244 result.coeffRef(1,3) = cofactor_4x4<MatrixType,3,1>(matrix);
00245 result.coeffRef(2,3) = -cofactor_4x4<MatrixType,3,2>(matrix);
00246 result.coeffRef(3,3) = cofactor_4x4<MatrixType,3,3>(matrix);
00247 result /= (matrix.col(0).cwiseProduct(result.row(0).transpose())).sum();
00248 }
00249 };
00250
00251 template<typename MatrixType, typename ResultType>
00252 struct compute_inverse<MatrixType, ResultType, 4>
00253 : compute_inverse_size4<Architecture::Target, typename MatrixType::Scalar,
00254 MatrixType, ResultType>
00255 {
00256 };
00257
00258 template<typename MatrixType, typename ResultType>
00259 struct compute_inverse_and_det_with_check<MatrixType, ResultType, 4>
00260 {
00261 static inline void run(
00262 const MatrixType& matrix,
00263 const typename MatrixType::RealScalar& absDeterminantThreshold,
00264 ResultType& inverse,
00265 typename ResultType::Scalar& determinant,
00266 bool& invertible
00267 )
00268 {
00269 determinant = matrix.determinant();
00270 invertible = abs(determinant) > absDeterminantThreshold;
00271 if(invertible) compute_inverse<MatrixType, ResultType>::run(matrix, inverse);
00272 }
00273 };
00274
00275
00276
00277
00278
00279 template<typename MatrixType>
00280 struct traits<inverse_impl<MatrixType> >
00281 {
00282 typedef typename MatrixType::PlainObject ReturnType;
00283 };
00284
00285 template<typename MatrixType>
00286 struct inverse_impl : public ReturnByValue<inverse_impl<MatrixType> >
00287 {
00288 typedef typename MatrixType::Index Index;
00289 typedef typename internal::eval<MatrixType>::type MatrixTypeNested;
00290 typedef typename remove_all<MatrixTypeNested>::type MatrixTypeNestedCleaned;
00291 MatrixTypeNested m_matrix;
00292
00293 inverse_impl(const MatrixType& matrix)
00294 : m_matrix(matrix)
00295 {}
00296
00297 inline Index rows() const { return m_matrix.rows(); }
00298 inline Index cols() const { return m_matrix.cols(); }
00299
00300 template<typename Dest> inline void evalTo(Dest& dst) const
00301 {
00302 const int Size = EIGEN_PLAIN_ENUM_MIN(MatrixType::ColsAtCompileTime,Dest::ColsAtCompileTime);
00303 EIGEN_ONLY_USED_FOR_DEBUG(Size);
00304 eigen_assert(( (Size<=1) || (Size>4) || (extract_data(m_matrix)!=extract_data(dst)))
00305 && "Aliasing problem detected in inverse(), you need to do inverse().eval() here.");
00306
00307 compute_inverse<MatrixTypeNestedCleaned, Dest>::run(m_matrix, dst);
00308 }
00309 };
00310
00311 }
00312
00330 template<typename Derived>
00331 inline const internal::inverse_impl<Derived> MatrixBase<Derived>::inverse() const
00332 {
00333 EIGEN_STATIC_ASSERT(!NumTraits<Scalar>::IsInteger,THIS_FUNCTION_IS_NOT_FOR_INTEGER_NUMERIC_TYPES)
00334 eigen_assert(rows() == cols());
00335 return internal::inverse_impl<Derived>(derived());
00336 }
00337
00356 template<typename Derived>
00357 template<typename ResultType>
00358 inline void MatrixBase<Derived>::computeInverseAndDetWithCheck(
00359 ResultType& inverse,
00360 typename ResultType::Scalar& determinant,
00361 bool& invertible,
00362 const RealScalar& absDeterminantThreshold
00363 ) const
00364 {
00365
00366 eigen_assert(rows() == cols());
00367
00368
00369 typedef typename internal::conditional<
00370 RowsAtCompileTime == 2,
00371 typename internal::remove_all<typename internal::nested<Derived, 2>::type>::type,
00372 PlainObject
00373 >::type MatrixType;
00374 internal::compute_inverse_and_det_with_check<MatrixType, ResultType>::run
00375 (derived(), absDeterminantThreshold, inverse, determinant, invertible);
00376 }
00377
00395 template<typename Derived>
00396 template<typename ResultType>
00397 inline void MatrixBase<Derived>::computeInverseWithCheck(
00398 ResultType& inverse,
00399 bool& invertible,
00400 const RealScalar& absDeterminantThreshold
00401 ) const
00402 {
00403 RealScalar determinant;
00404
00405 eigen_assert(rows() == cols());
00406 computeInverseAndDetWithCheck(inverse,determinant,invertible,absDeterminantThreshold);
00407 }
00408
00409 }
00410
00411 #endif // EIGEN_INVERSE_H