Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +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');