| 12 | |
| 13 | template<typename MatrixType> |
| 14 | bool equalsIdentity(const MatrixType& A) |
| 15 | { |
| 16 | typedef typename MatrixType::Scalar Scalar; |
| 17 | Scalar zero = static_cast<Scalar>(0); |
| 18 | |
| 19 | bool offDiagOK = true; |
| 20 | for (Index i = 0; i < A.rows(); ++i) { |
| 21 | for (Index j = i+1; j < A.cols(); ++j) { |
| 22 | offDiagOK = offDiagOK && (A(i,j) == zero); |
| 23 | } |
| 24 | } |
| 25 | for (Index i = 0; i < A.rows(); ++i) { |
| 26 | for (Index j = 0; j < (std::min)(i, A.cols()); ++j) { |
| 27 | offDiagOK = offDiagOK && (A(i,j) == zero); |
| 28 | } |
| 29 | } |
| 30 | |
| 31 | bool diagOK = (A.diagonal().array() == 1).all(); |
| 32 | return offDiagOK && diagOK; |
| 33 | |
| 34 | } |
| 35 | |
| 36 | template<typename VectorType> |
| 37 | void check_extremity_accuracy(const VectorType &v, const typename VectorType::Scalar &low, const typename VectorType::Scalar &high) |