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

[wpimath] Replace constexpr coeff() and coeffRef() with operator() (#7391)

Browse Source
This commit is contained in:
Tyler Veness
2024-11-16 07:44:20 -08:00
committed by GitHub
parent ca51197486
commit aa7dd258c4
74 changed files with 1953 additions and 1329 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
wpimath/src/main/native/include/frc/StateSpaceUtil.h
View File

@@ -41,7 +41,7 @@ constexpr Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCostMatrix(
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
if (row != col) {
result.coeffRef(row, col) = 0.0;
result(row, col) = 0.0;
}
}
}
@@ -49,9 +49,9 @@ constexpr Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCostMatrix(
wpi::for_each(
[&](int i, double tolerance) {
if (tolerance == std::numeric_limits<double>::infinity()) {
result.coeffRef(i, i) = 0.0;
result(i, i) = 0.0;
} else {
result.coeffRef(i, i) = 1.0 / (tolerance * tolerance);
result(i, i) = 1.0 / (tolerance * tolerance);
}
},
tolerances...);
@@ -78,14 +78,13 @@ constexpr Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCovMatrix(Ts... stdDevs) {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
if (row != col) {
result.coeffRef(row, col) = 0.0;
result(row, col) = 0.0;
}
}
}
wpi::for_each(
[&](int i, double stdDev) { result.coeffRef(i, i) = stdDev * stdDev; },
stdDevs...);
wpi::for_each([&](int i, double stdDev) { result(i, i) = stdDev * stdDev; },
stdDevs...);
return result;
}
@@ -111,12 +110,12 @@ constexpr Matrixd<N, N> MakeCostMatrix(const std::array<double, N>& costs) {
for (int col = 0; col < result.cols(); ++col) {
if (row == col) {
if (costs[row] == std::numeric_limits<double>::infinity()) {
result.coeffRef(row, col) = 0.0;
result(row, col) = 0.0;
} else {
result.coeffRef(row, col) = 1.0 / (costs[row] * costs[row]);
result(row, col) = 1.0 / (costs[row] * costs[row]);
}
} else {
result.coeffRef(row, col) = 0.0;
result(row, col) = 0.0;
}
}
}
@@ -143,9 +142,9 @@ constexpr Matrixd<N, N> MakeCovMatrix(const std::array<double, N>& stdDevs) {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
if (row == col) {
result.coeffRef(row, col) = stdDevs[row] * stdDevs[row];
result(row, col) = stdDevs[row] * stdDevs[row];
} else {
result.coeffRef(row, col) = 0.0;
result(row, col) = 0.0;
}
}
}
@@ -334,7 +333,7 @@ constexpr Vectord<Inputs> ClampInputMaxMagnitude(const Vectord<Inputs>& u,
const Vectord<Inputs>& umax) {
Vectord<Inputs> result;
for (int i = 0; i < Inputs; ++i) {
result.coeffRef(i) = std::clamp(u.coeff(i), umin.coeff(i), umax.coeff(i));
result(i) = std::clamp(u(i), umin(i), umax(i));
}
return result;
}

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

@@ -62,7 +62,7 @@ class ct_matrix {
* @param col Column index.
*/
constexpr const Scalar& operator()(int row, int col) const {
return m_storage.coeff(row, col);
return m_storage(row, col);
}
/**
@@ -71,9 +71,7 @@ class ct_matrix {
* @param row Row index.
* @param col Column index.
*/
constexpr Scalar& operator()(int row, int col) {
return m_storage.coeffRef(row, col);
}
constexpr Scalar& operator()(int row, int col) { return m_storage(row, col); }
/**
* Returns reference to matrix element.
@@ -83,7 +81,7 @@ class ct_matrix {
constexpr const Scalar& operator()(int index) const
requires(Rows == 1 || Cols == 1)
{
return m_storage.coeff(index);
return m_storage(index);
}
/**
@@ -94,7 +92,7 @@ class ct_matrix {
constexpr Scalar& operator()(int index)
requires(Rows == 1 || Cols == 1)
{
return m_storage.coeffRef(index);
return m_storage(index);
}
/**

10
wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h
View File

@@ -35,12 +35,10 @@ class WPILIB_DLLEXPORT CoordinateSystem {
// the NWU coordinate system. Each column vector in the change of basis
// matrix is one of the old basis vectors mapped to its representation in
// the new basis.
Eigen::Matrix3d R{{positiveX.m_axis.coeff(0), positiveY.m_axis.coeff(0),
positiveZ.m_axis.coeff(0)},
{positiveX.m_axis.coeff(1), positiveY.m_axis.coeff(1),
positiveZ.m_axis.coeff(1)},
{positiveX.m_axis.coeff(2), positiveY.m_axis.coeff(2),
positiveZ.m_axis.coeff(2)}};
Eigen::Matrix3d R{
{positiveX.m_axis(0), positiveY.m_axis(0), positiveZ.m_axis(0)},
{positiveX.m_axis(1), positiveY.m_axis(1), positiveZ.m_axis(1)},
{positiveX.m_axis(2), positiveY.m_axis(2), positiveZ.m_axis(2)}};
// The change of basis matrix should be a pure rotation. The Rotation3d
// constructor will verify this by checking for special orthogonality.

6
wpimath/src/main/native/include/frc/geometry/Pose3d.h
View File

@@ -281,9 +281,9 @@ constexpr Eigen::Matrix3d RotationVectorToMatrix(
// [ 0 -c b]
// Omega = [ c 0 -a]
// [-b a 0]
return Eigen::Matrix3d{{0.0, -rotation.coeff(2), rotation.coeff(1)},
{rotation.coeff(2), 0.0, -rotation.coeff(0)},
{-rotation.coeff(1), rotation.coeff(0), 0.0}};
return Eigen::Matrix3d{{0.0, -rotation(2), rotation(1)},
{rotation(2), 0.0, -rotation(0)},
{-rotation(1), rotation(0), 0.0}};
}
} // namespace detail

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

@@ -299,7 +299,7 @@ class WPILIB_DLLEXPORT Quaternion {
// 𝑞 = std::cos(θ/2) + std::sin(θ/2) * v̂
// 𝑞 = std::cos(θ/2) + std::sin(θ/2) / θ * 𝑣⃗
double theta = gcem::hypot(rvec.coeff(0), rvec.coeff(1), rvec.coeff(2));
double theta = gcem::hypot(rvec(0), rvec(1), rvec(2));
double cos = gcem::cos(theta / 2);
double axial_scalar;
@@ -312,9 +312,8 @@ class WPILIB_DLLEXPORT Quaternion {
axial_scalar = gcem::sin(theta / 2) / theta;
}
return Quaternion{cos, axial_scalar * rvec.coeff(0),
axial_scalar * rvec.coeff(1),
axial_scalar * rvec.coeff(2)};
return Quaternion{cos, axial_scalar * rvec(0), axial_scalar * rvec(1),
axial_scalar * rvec(2)};
}
private:

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

@@ -85,11 +85,10 @@ class WPILIB_DLLEXPORT Rotation3d {
}
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Definition
Eigen::Vector3d v{{axis.coeff(0) / norm * units::math::sin(angle / 2.0),
axis.coeff(1) / norm * units::math::sin(angle / 2.0),
axis.coeff(2) / norm * units::math::sin(angle / 2.0)}};
m_q = Quaternion{units::math::cos(angle / 2.0), v.coeff(0), v.coeff(1),
v.coeff(2)};
Eigen::Vector3d v{{axis(0) / norm * units::math::sin(angle / 2.0),
axis(1) / norm * units::math::sin(angle / 2.0),
axis(2) / norm * units::math::sin(angle / 2.0)}};
m_q = Quaternion{units::math::cos(angle / 2.0), v(0), v(1), v(2)};
}
/**
@@ -191,9 +190,9 @@ class WPILIB_DLLEXPORT Rotation3d {
// rotation is required. Any other vector can be used to generate an
// orthogonal one.
double x = gcem::abs(initial.coeff(0));
double y = gcem::abs(initial.coeff(1));
double z = gcem::abs(initial.coeff(2));
double x = gcem::abs(initial(0));
double y = gcem::abs(initial(1));
double z = gcem::abs(initial(2));
// Find vector that is most orthogonal to initial vector
Eigen::Vector3d other;

3
wpimath/src/main/native/include/frc/geometry/Translation2d.h
View File

@@ -60,8 +60,7 @@ class WPILIB_DLLEXPORT Translation2d {
* @param vector The translation vector to represent.
*/
constexpr explicit Translation2d(const Eigen::Vector2d& vector)
: m_x{units::meter_t{vector.coeff(0)}},
m_y{units::meter_t{vector.coeff(1)}} {}
: m_x{units::meter_t{vector(0)}}, m_y{units::meter_t{vector(1)}} {}
/**
* Calculates the distance between two translations in 2D space.

2
wpimath/src/main/native/include/frc/system/LinearSystem.h
View File

@@ -55,7 +55,7 @@ class LinearSystem {
if (std::is_constant_evaluated()) {
for (int row = 0; row < mat.rows(); ++row) {
for (int col = 0; col < mat.cols(); ++col) {
if (!gcem::internal::is_finite(mat.coeff(row, col))) {
if (!gcem::internal::is_finite(mat(row, col))) {
return false;
}
}