diff --git a/include/Kyoto/Math/CQuaternion.hpp b/include/Kyoto/Math/CQuaternion.hpp index 6e2a7847..c82c194a 100644 --- a/include/Kyoto/Math/CQuaternion.hpp +++ b/include/Kyoto/Math/CQuaternion.hpp @@ -60,6 +60,15 @@ public: static const CQuaternion& NoRotation() { return sNoRotation; } + static float Dot(const CQuaternion& a, const CQuaternion& b) { + return (a.GetW() * b.GetW()) + (a.GetX() * b.GetX()) + (a.GetY() * b.GetY()) + (a.GetZ() * b.GetZ()); + } + + float GetW() const { return w; } + float GetX() const { return x; } + float GetY() const { return y; } + float GetZ() const { return z; } + private: float w; float x; diff --git a/include/MetroidPrime/CEulerAngles.hpp b/include/MetroidPrime/CEulerAngles.hpp new file mode 100644 index 00000000..e873375d --- /dev/null +++ b/include/MetroidPrime/CEulerAngles.hpp @@ -0,0 +1,14 @@ +#ifndef _CEULERANGLES +#define _CEULERANGLES + +#include "Kyoto/Math/CVector3f.hpp" + +class CQuaternion; + +class CEulerAngles : public CVector3f { +public: + CEulerAngles(float x, float y, float z) : CVector3f(x, y, z) {} + static CEulerAngles FromQuaternion(const CQuaternion&); +}; + +#endif // _CEULERANGLES diff --git a/src/MetroidPrime/CEulerAngles.cpp b/src/MetroidPrime/CEulerAngles.cpp new file mode 100644 index 00000000..38bf41d8 --- /dev/null +++ b/src/MetroidPrime/CEulerAngles.cpp @@ -0,0 +1,36 @@ +#include "MetroidPrime/CEulerAngles.hpp" + +#include "Kyoto/Math/CMath.hpp" +#include "Kyoto/Math/CQuaternion.hpp" +#include "Kyoto/Math/CloseEnough.hpp" + +CEulerAngles CEulerAngles::FromQuaternion(const CQuaternion& quat) { + float quatDot = CQuaternion::Dot(quat, quat); + float t0 = 0.f; + if (quatDot > 0.f) + t0 = 2.f / quatDot; + + double t1 = 1.0 - (t0 * quat.GetX() * quat.GetX() + t0 * quat.GetZ() * quat.GetZ()); + double t2 = t0 * quat.GetY() * quat.GetX() - t0 * quat.GetZ() * quat.GetW(); + double t3 = t1 * t1 + t2 * t2; + + double t4 = 0.0; + if (t3 > 0.0) + t4 = CMath::SqrtD(t3); + + double t5 = t0 * quat.GetZ() * quat.GetY() + t0 * quat.GetX() * quat.GetW(); + + if (close_enough(t4, 0.0)) { + return CEulerAngles( + -atan2(-t5, t4), + -atan2(-(t0 * quat.GetZ() * quat.GetX() + t0 * quat.GetY() * quat.GetW()), + 1.0 - (t0 * quat.GetY() * quat.GetY() + t0 * quat.GetZ() * quat.GetZ())), + 0.0f); + } else { + return CEulerAngles( + -atan2(-t5, t4), + -atan2(t0 * quat.GetZ() * quat.GetX() - t0 * quat.GetY() * quat.GetW(), + 1.0 - (t0 * quat.GetX() * quat.GetX() + t0 * quat.GetY() * quat.GetY())), + -atan2(t2, t1)); + } +}