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

[wpimath] Add affine transformation constructors and getters to geometry API (#7430)

Browse Source 
Fixes #7429.
This commit is contained in:
Tyler Veness
2024-12-07 15:49:17 -08:00
committed by GitHub
parent f772bb141d
commit e222efaa01
32 changed files with 615 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
wpimath/src/main/native/include/frc/geometry/Transform2d.h
View File

@@ -49,6 +49,21 @@ class WPILIB_DLLEXPORT Transform2d {
constexpr Transform2d(units::meter_t x, units::meter_t y, Rotation2d rotation)
: m_translation{x, y}, m_rotation{std::move(rotation)} {}
/**
* Constructs a pose with the specified affine transformation matrix.
*
* @param matrix The affine transformation matrix.
* @throws std::domain_error if the affine transformation matrix is invalid.
*/
constexpr explicit Transform2d(const Eigen::Matrix3d& matrix)
: m_translation{Eigen::Vector2d{{matrix(0, 2)}, {matrix(1, 2)}}},
m_rotation{Eigen::Matrix2d{{matrix(0, 0), matrix(0, 1)},
{matrix(1, 0), matrix(1, 1)}}} {
if (matrix(2, 0) != 0.0 || matrix(2, 1) != 0.0 || matrix(2, 2) != 1.0) {
throw std::domain_error("Affine transformation matrix is invalid");
}
}
/**
* Constructs the identity transform -- maps an initial pose to itself.
*/
@@ -75,6 +90,18 @@ class WPILIB_DLLEXPORT Transform2d {
*/
constexpr units::meter_t Y() const { return m_translation.Y(); }
/**
* Returns an affine transformation matrix representation of this
* transformation.
*/
constexpr Eigen::Matrix3d ToMatrix() const {
auto vec = m_translation.ToVector();
auto mat = m_rotation.ToMatrix();
return Eigen::Matrix3d{{mat(0, 0), mat(0, 1), vec(0)},
{mat(1, 0), mat(1, 1), vec(1)},
{0.0, 0.0, 1.0}};
}
/**
* Returns the rotational component of the transformation.
*