diff --git a/wpimath/src/main/java/org/wpilib/math/geometry/CoordinateSystem.java b/wpimath/src/main/java/org/wpilib/math/geometry/CoordinateSystem.java index 79a4dacb33..b5198fe0b5 100644 --- a/wpimath/src/main/java/org/wpilib/math/geometry/CoordinateSystem.java +++ b/wpimath/src/main/java/org/wpilib/math/geometry/CoordinateSystem.java @@ -5,6 +5,7 @@ package org.wpilib.math.geometry; import org.wpilib.math.linalg.Matrix; +import org.wpilib.math.util.MathSharedStore; import org.wpilib.math.util.Nat; /** A helper class that converts Pose3d objects between different standard coordinate frames. */ @@ -37,6 +38,14 @@ public class CoordinateSystem { R.assignBlock(0, 1, positiveY.m_axis); R.assignBlock(0, 2, positiveZ.m_axis); + // If determinant is -1, coordinate system is left-handed + if (Math.abs(R.det() + 1.0) < 1e-9) { + var msg = + "CoordinateSystem requires a right-handed system, but a left-handed one was provided"; + MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace()); + throw new IllegalArgumentException(msg); + } + // The change of basis matrix should be a pure rotation. The Rotation3d // constructor will verify this by checking for special orthogonality. m_rotation = new Rotation3d(R); diff --git a/wpimath/src/main/native/include/wpi/math/geometry/CoordinateSystem.hpp b/wpimath/src/main/native/include/wpi/math/geometry/CoordinateSystem.hpp index ded8b55273..8108a4df79 100644 --- a/wpimath/src/main/native/include/wpi/math/geometry/CoordinateSystem.hpp +++ b/wpimath/src/main/native/include/wpi/math/geometry/CoordinateSystem.hpp @@ -4,10 +4,13 @@ #pragma once +#include + #include "wpi/math/geometry/CoordinateAxis.hpp" #include "wpi/math/geometry/Pose3d.hpp" #include "wpi/math/geometry/Rotation3d.hpp" #include "wpi/math/geometry/Translation3d.hpp" +#include "wpi/math/linalg/ct_matrix.hpp" #include "wpi/util/SymbolExports.hpp" namespace wpi::math { @@ -39,6 +42,13 @@ class WPILIB_DLLEXPORT CoordinateSystem { {positiveX.m_axis(1), positiveY.m_axis(1), positiveZ.m_axis(1)}, {positiveX.m_axis(2), positiveY.m_axis(2), positiveZ.m_axis(2)}}; + // If determinant is -1, coordinate system is left-handed + if (gcem::abs(ct_matrix{R}.determinant() + 1.0) < 1e-9) { + throw std::domain_error( + "CoordinateSystem requires a right-handed system, but a left-handed " + "one was provided"); + } + // The change of basis matrix should be a pure rotation. The Rotation3d // constructor will verify this by checking for special orthogonality. m_rotation = Rotation3d{R}; diff --git a/wpimath/src/test/java/org/wpilib/math/geometry/CoordinateSystemTest.java b/wpimath/src/test/java/org/wpilib/math/geometry/CoordinateSystemTest.java index 5769cbb2dd..774999dce6 100644 --- a/wpimath/src/test/java/org/wpilib/math/geometry/CoordinateSystemTest.java +++ b/wpimath/src/test/java/org/wpilib/math/geometry/CoordinateSystemTest.java @@ -5,6 +5,7 @@ package org.wpilib.math.geometry; import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertThrows; import org.junit.jupiter.api.Test; import org.wpilib.math.util.Units; @@ -232,4 +233,17 @@ class CoordinateSystemTest { CoordinateSystem.EDN(), CoordinateSystem.NED()); } + + @Test + void testLeftHandedSystemThrowsException() { + assertThrows( + IllegalArgumentException.class, + () -> new CoordinateSystem(CoordinateAxis.N(), CoordinateAxis.E(), CoordinateAxis.U())); + assertThrows( + IllegalArgumentException.class, + () -> new CoordinateSystem(CoordinateAxis.E(), CoordinateAxis.U(), CoordinateAxis.N())); + assertThrows( + IllegalArgumentException.class, + () -> new CoordinateSystem(CoordinateAxis.N(), CoordinateAxis.W(), CoordinateAxis.D())); + } } diff --git a/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp b/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp index bb6f96483b..e9d7421ae0 100644 --- a/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp @@ -155,3 +155,15 @@ TEST(CoordinateSystemTest, Transform3dEDNtoNED) { Rotation3d{45_deg, 0_deg, 0_deg}}, CoordinateSystem::EDN(), CoordinateSystem::NED()); } + +TEST(CoordinateSystemTest, LeftHandedSystemThrowsException) { + EXPECT_THROW(CoordinateSystem(CoordinateAxis::N(), CoordinateAxis::E(), + CoordinateAxis::U()), + std::domain_error); + EXPECT_THROW(CoordinateSystem(CoordinateAxis::E(), CoordinateAxis::U(), + CoordinateAxis::N()), + std::domain_error); + EXPECT_THROW(CoordinateSystem(CoordinateAxis::N(), CoordinateAxis::W(), + CoordinateAxis::D()), + std::domain_error); +}