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] Improve Euler angle calculations in gimbal lock (#5437)

Browse Source
This commit is contained in:
Joseph Eng
2023-07-17 17:19:42 -07:00
committed by GitHub
parent daf022d3da
commit 593767c8c7
5 changed files with 365 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -187,9 +187,15 @@ units::radian_t Rotation3d::X() const {
double y = m_q.Y();
double z = m_q.Z();
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_(in_3-2-1_sequence)_conversion
return units::radian_t{
std::atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y))};
// wpimath/algorithms.md
double cxcy = 1.0 - 2.0 * (x * x + y * y);
double sxcy = 2.0 * (w * x + y * z);
double cy_sq = cxcy * cxcy + sxcy * sxcy;
if (cy_sq > 1e-20) {
return units::radian_t{std::atan2(sxcy, cxcy)};
} else {
return 0_rad;
}
}
units::radian_t Rotation3d::Y() const {
@@ -213,9 +219,15 @@ units::radian_t Rotation3d::Z() const {
double y = m_q.Y();
double z = m_q.Z();
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_(in_3-2-1_sequence)_conversion
return units::radian_t{
std::atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z))};
// wpimath/algorithms.md
double cycz = 1.0 - 2.0 * (y * y + z * z);
double cysz = 2.0 * (w * z + x * y);
double cy_sq = cycz * cycz + cysz * cysz;
if (cy_sq > 1e-20) {
return units::radian_t{std::atan2(cysz, cycz)};
} else {
return units::radian_t{std::atan2(2.0 * w * z, w * w - z * z)};
}
}
Vectord<3> Rotation3d::Axis() const {