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] 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

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

@@ -403,6 +403,24 @@ class WPILIB_DLLEXPORT Rotation3d {
return units::radian_t{2.0 * gcem::atan2(norm, m_q.W())};
}
/**
* Returns rotation matrix representation of this rotation.
*/
constexpr Eigen::Matrix3d ToMatrix() const {
double w = m_q.W();
double x = m_q.X();
double y = m_q.Y();
double z = m_q.Z();
// https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Quaternion-derived_rotation_matrix
return Eigen::Matrix3d{{1.0 - 2.0 * (y * y + z * z), 2.0 * (x * y - w * z),
2.0 * (x * z + w * y)},
{2.0 * (x * y + w * z), 1.0 - 2.0 * (x * x + z * z),
2.0 * (y * z - w * x)},
{2.0 * (x * z - w * y), 2.0 * (y * z + w * x),
1.0 - 2.0 * (x * x + y * y)}};
}
/**
* Returns a Rotation2d representing this Rotation3d projected into the X-Y
* plane.