| 67 | |
| 68 | template <typename T> |
| 69 | void CheckRotMat(const Eigen::Matrix<T, 3, 3>& R) |
| 70 | { |
| 71 | // See `ExpectRotMat`. |
| 72 | const T identity_error |
| 73 | = (R * R.transpose() - Eigen::Matrix<T, 3, 3>::Identity()) |
| 74 | .array() |
| 75 | .abs() |
| 76 | .maxCoeff(); |
| 77 | DART_UNUSED(identity_error); |
| 78 | DART_ASSERT( |
| 79 | identity_error < kCheckTolerance && "Rotation matrix is not orthonormal"); |
| 80 | const T det_error = fabs(R.determinant() - 1); |
| 81 | DART_UNUSED(det_error); |
| 82 | DART_ASSERT( |
| 83 | det_error < kCheckTolerance |
| 84 | && "Rotation matrix violates right-hand rule"); |
| 85 | } |
| 86 | |
| 87 | template <typename T> |
| 88 | void CheckIsometry(const Eigen::Transform<T, 3, Eigen::Isometry>& X) |