Add a disaster called CEulerAngles

Former-commit-id: c93d6060e8

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;

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

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));
+  }
+}