Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Use ct_matrix instead of Eigen/LU for determinant in headers (#7600)

Browse Source 
This caught a bug in ct_matrix's 3x3 determinant.
This commit is contained in:
Tyler Veness
2024-12-28 14:03:29 -08:00
committed by GitHub
parent 46d401553e
commit 57344ef3b2
3 changed files with 12 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
wpimath/src/main/native/include/frc/ct_matrix.h
View File

@@ -334,7 +334,7 @@ class ct_matrix {
requires(Rows == 3 && Cols == 3)
{
// |a b c|
// |d e f| = aei + bfg + cgh - ceg - bdi - afh
// |d e f| = aei + bfg + cdh - ceg - bdi - afh
// |g h i|
Scalar a = (*this)(0, 0);
Scalar b = (*this)(0, 1);
@@ -345,7 +345,7 @@ class ct_matrix {
Scalar g = (*this)(2, 0);
Scalar h = (*this)(2, 1);
Scalar i = (*this)(2, 2);
return a * e * i + b * f * g + c * g * h - c * e * g - b * d * i -
return a * e * i + b * f * g + c * d * h - c * e * g - b * d * i -
a * f * h;
}

7
wpimath/src/main/native/include/frc/geometry/Rotation2d.h
View File

@@ -8,7 +8,6 @@
#include <utility>
#include <Eigen/Core>
#include <Eigen/LU>
#include <gcem.hpp>
#include <wpi/StackTrace.h>
#include <wpi/SymbolExports.h>
@@ -84,7 +83,11 @@ class WPILIB_DLLEXPORT Rotation2d {
if ((R * R.transpose() - Matrix2d::Identity()).norm() > 1e-9) {
throw std::domain_error("Rotation matrix isn't orthogonal");
}
if (gcem::abs(R.determinant() - 1.0) > 1e-9) {
// HACK: Uses ct_matrix instead of <Eigen/LU> for determinant because
// including <Eigen/LU> doubles compilation times on MSVC, even if
// this constructor is unused. MSVC's frontend inefficiently parses
// large headers; GCC and Clang are largely unaffected.
if (gcem::abs(ct_matrix{R}.determinant() - 1.0) > 1e-9) {
throw std::domain_error(
"Rotation matrix is orthogonal but not special orthogonal");
}

7
wpimath/src/main/native/include/frc/geometry/Rotation3d.h
View File

@@ -8,7 +8,6 @@
#include <type_traits>
#include <Eigen/Core>
#include <Eigen/LU>
#include <fmt/format.h>
#include <gcem.hpp>
#include <wpi/SymbolExports.h>
@@ -114,7 +113,11 @@ class WPILIB_DLLEXPORT Rotation3d {
if ((R * R.transpose() - Matrix3d::Identity()).norm() > 1e-9) {
throw std::domain_error("Rotation matrix isn't orthogonal");
}
if (gcem::abs(R.determinant() - 1.0) > 1e-9) {
// HACK: Uses ct_matrix instead of <Eigen/LU> for determinant because
// including <Eigen/LU> doubles compilation times on MSVC, even if
// this constructor is unused. MSVC's frontend inefficiently parses
// large headers; GCC and Clang are largely unaffected.
if (gcem::abs(ct_matrix{R}.determinant() - 1.0) > 1e-9) {
throw std::domain_error(
"Rotation matrix is orthogonal but not special orthogonal");
}