Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-04 03:11:43 +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};