Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-28 02:11: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/java/edu/wpi/first/math/geometry/Rotation3d.java
View File

@@ -101,7 +101,7 @@ public class Rotation3d implements Interpolatable<Rotation3d> {
// 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.times(R.transpose()).equals(Matrix.eye(Nat.N3()))) {
if (R.times(R.transpose()).minus(Matrix.eye(Nat.N3())).normF() > 1e-9) {
var builder = new StringBuilder("Rotation matrix isn't orthogonal\n\nR =\n");
builder.append(R.getStorage().toString()).append('\n');
@@ -109,7 +109,7 @@ public class Rotation3d implements Interpolatable<Rotation3d> {
MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
throw new IllegalArgumentException(msg);
}
if (R.det() != 1.0) {
if (Math.abs(R.det() - 1.0) > 1e-9) {
var builder =
new StringBuilder("Rotation matrix is orthogonal but not special orthogonal\n\nR =\n");
builder.append(R.getStorage().toString()).append('\n');

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);