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

[wpimath] Fix Pose3d exp()/log() and add rotation vector constructor to Rotation3d (#5072)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Jordan McMichael
2023-02-09 00:31:03 -05:00
committed by GitHub
parent 37f065032f
commit 59be120982
7 changed files with 93 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
wpimath/src/main/native/cpp/geometry/Rotation3d.cpp
View File

@@ -38,6 +38,9 @@ Rotation3d::Rotation3d(units::radian_t roll, units::radian_t pitch,
cr * sp * cy + sr * cp * sy, cr * cp * sy - sr * sp * cy};
}
Rotation3d::Rotation3d(const Eigen::Vector3d& rvec)
: Rotation3d{rvec, units::radian_t{rvec.norm()}} {}
Rotation3d::Rotation3d(const Vectord<3>& axis, units::radian_t angle) {
double norm = axis.norm();
if (norm == 0.0) {