josh/j3ml
1
0
Fork 1
Code Issues 12 Pull Requests Actions Packages Projects Releases 41 Wiki Activity

Implement EulerAngle::ToQuaternion EulerAngle::ToAxisAngle

Browse Source
This commit is contained in:
josh
2024-07-01 14:14:19 -04:00
parent 70aa74719a
commit cc564b14fe
2 changed files with 38 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
include/J3ML/LinearAlgebra/EulerAngle.h
View File

@@ -18,6 +18,8 @@ public:
AxisAngle ToAxisAngle() const;
[[nodiscard]] Quaternion ToQuaternion() const;
explicit EulerAngle(const Quaternion& rhs);
explicit EulerAngle(const AxisAngle& rhs);

36
src/J3ML/LinearAlgebra/EulerAngle.cpp
View File

@@ -89,6 +89,42 @@ namespace J3ML::LinearAlgebra {
roll = std::atan2(x * s - y * z * t, 1 - (x*x + z*z) * t);
}
AxisAngle EulerAngle::ToAxisAngle() const {
auto c1 = std::cos(yaw / 2);
auto c2 = std::cos(pitch / 2);
auto c3 = std::cos(roll / 2);
auto s1 = std::sin(yaw / 2);
auto s2 = std::sin(pitch / 2);
auto s3 = std::sin(roll / 2);
auto angle = 2 * std::acos(c1*c2*c3 - s1*s2*s3);
auto x = s1*s2*c3 + c1*c2*s3;
auto y = s1*c2*c3 + c1*s2*s3;
auto z = c1*s2*c3 - s1*c2*s3;
// todo: normalize?
// sqrt(x^2 + y^2 + z^2) = sqrt((s1 s2 c3 +c1 c2 s3)^2+(s1 c2 c3 + c1 s2 s3)^2+(c1 s2 c3 - s1 c2 s3)^2)
return {{x,y,z}, angle};
}
Quaternion EulerAngle::ToQuaternion() const {
auto c1 = std::cos(yaw / 2);
auto c2 = std::cos(pitch / 2);
auto c3 = std::cos(roll / 2);
auto s1 = std::sin(yaw / 2);
auto s2 = std::sin(pitch / 2);
auto s3 = std::sin(roll / 2);
auto w = c1*c2*c3 - s1*s2*s3;
auto x = s1*s2*c3 + c1*c2*s3;
auto y = s1*c2*c3 + c1*s2*s3;
auto z = c1*s2*c3 - s1*c2*s3;
return {w,x,y,z};
}
EulerAngle::EulerAngle(const Quaternion &rhs) {
double test = rhs.x * rhs.y + rhs.z * rhs.w;
if (test > 0.499) { // Singularity at north pole