Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add tolerance for Rotation3d rotation matrix special orthogonality (#4744)

Browse Source
This commit is contained in:
Tyler Veness
2022-11-30 19:51:31 -08:00
committed by GitHub
parent 4c4545fb4b
commit eae68fc165
2 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
wpimath/src/main/native/cpp/geometry/Rotation3d.cpp
View File

@@ -54,14 +54,14 @@ Rotation3d::Rotation3d(const Matrixd<3, 3>& rotationMatrix) {
// Require that the rotation matrix is special orthogonal. This is true if the
// matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1).
if (R * R.transpose() != Matrixd<3, 3>::Identity()) {
if ((R * R.transpose() - Matrixd<3, 3>::Identity()).norm() > 1e-9) {
std::string msg =
fmt::format("Rotation matrix isn't orthogonal\n\nR =\n{}\n", R);
wpi::math::MathSharedStore::ReportError(msg);
throw std::domain_error(msg);
}
if (R.determinant() != 1.0) {
if (std::abs(R.determinant() - 1.0) > 1e-9) {
std::string msg = fmt::format(
"Rotation matrix is orthogonal but not special orthogonal\n\nR =\n{}\n",
R);