Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-30 02:31:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Prevent CoordinateSystem from accepting left-handed systems (#8750)

Browse Source
This commit is contained in:
Tricks1228
2026-04-11 15:53:36 -05:00
committed by GitHub
parent c6f54e963c
commit 042567d0ba
4 changed files with 45 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
wpimath/src/main/java/org/wpilib/math/geometry/CoordinateSystem.java
View File

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

10
wpimath/src/main/native/include/wpi/math/geometry/CoordinateSystem.hpp
View File

@@ -4,10 +4,13 @@
#pragma once
#include <gcem.hpp>
#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};

14
wpimath/src/test/java/org/wpilib/math/geometry/CoordinateSystemTest.java
View File

@@ -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()));
}
}

12
wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp
View File

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