Minor CEulerAngles progress

Former-commit-id: 67f01bc9cd

asm/MetroidPrime/CAutoMapper.s

@@ -5023,7 +5023,7 @@ lbl_80099984:
 /* 80099994 000968F4  4B F7 0F D5 */	bl GetCurrentCameraTransform__14CCameraManagerCFRC13CStateManager
 /* 80099998 000968F8  38 61 00 C8 */	addi r3, r1, 0xc8
 /* 8009999C 000968FC  38 81 03 A0 */	addi r4, r1, 0x3a0
-/* 800999A0 00096900  4B F8 1E 61 */	bl FromMatrix3f__12CEulerAnglesFRC9CMatrix3f
+/* 800999A0 00096900  4B F8 1E 61 */	bl FromTransform__12CEulerAnglesFRC12CTransform4f
 /* 800999A4 00096904  C0 81 00 D0 */	lfs f4, 0xd0(r1)
 /* 800999A8 00096908  3C 00 43 30 */	lis r0, 0x4330
 /* 800999AC 0009690C  C0 02 8B A4 */	lfs f0, lbl_805AA8C4@sda21(r2)

asm/MetroidPrime/CEulerAngles.s

@@ -207,8 +207,8 @@ lbl_8001B7C4:
 /* 8001B7F8 00018758  38 21 00 60 */	addi r1, r1, 0x60
 /* 8001B7FC 0001875C  4E 80 00 20 */	blr
 
-.global FromMatrix3f__12CEulerAnglesFRC9CMatrix3f
+.global FromTransform__12CEulerAnglesFRC12CTransform4f
-FromMatrix3f__12CEulerAnglesFRC9CMatrix3f:
+FromTransform__12CEulerAnglesFRC12CTransform4f:
 /* 8001B800 00018760  94 21 FF 90 */	stwu r1, -0x70(r1)
 /* 8001B804 00018764  7C 08 02 A6 */	mflr r0
 /* 8001B808 00018768  90 01 00 74 */	stw r0, 0x74(r1)

include/Kyoto/Math/CMatrix3f.hpp

@@ -3,6 +3,7 @@
 
 #include "types.h"
 
+#include "Kyoto/Math/CTransform4f.hpp"
 #include "Kyoto/Math/CVector3f.hpp"
 
 class CMatrix3f {
@@ -10,6 +11,13 @@ class CMatrix3f {
 
 public:
   CMatrix3f(const CMatrix3f&);
+  CMatrix3f(const CVector3f& m0, const CVector3f& m1, const CVector3f& m2);
+  // fake but useful for CEulerAngles?
+  CMatrix3f(const CTransform4f& xf);/*
+  : m0(xf.GetRow(kDX))
+  , m1(xf.GetRow(kDY))
+  , m2(xf.GetRow(kDZ)) {}*/
+
   const CMatrix3f& operator=(const CMatrix3f& other);
 
   static const CMatrix3f& Identity() { return sIdentity; }
@@ -21,8 +29,22 @@ public:
   inline const CVector3f& GetRow(EDimY dim) const { return m1; }
   inline const CVector3f& GetRow(EDimZ dim) const { return m2; }
 
+  float Get00() const { return m0.GetX(); }
+  float Get01() const { return m0.GetY(); }
+  float Get02() const { return m0.GetZ(); }
+  float Get10() const { return m1.GetX(); }
+  float Get11() const { return m1.GetY(); }
+  float Get12() const { return m1.GetZ(); }
+  float Get20() const { return m2.GetX(); }
+  float Get21() const { return m2.GetY(); }
+  float Get22() const { return m2.GetZ(); }
+
   inline CVector3f GetColumn(EDimY dim) const { return CVector3f(m0.GetY(), m1.GetY(), m2.GetY()); }
 
+  static CMatrix3f FromTransform(const CTransform4f& xf);/* {
+    return CMatrix3f(xf.GetRow(kDX), xf.GetRow(kDY), xf.GetRow(kDZ));
+  }*/
+
 private:
   // TODO maybe individual floats
   CVector3f m0;

include/MetroidPrime/CEulerAngles.hpp

@@ -3,12 +3,18 @@
 
 #include "Kyoto/Math/CVector3f.hpp"
 
+class CTransform4f;
 class CQuaternion;
 
 class CEulerAngles : public CVector3f {
 public:
-  CEulerAngles(float x, float y, float z) : CVector3f(x, y, z) {}
+  CEulerAngles(float roll, float pitch, float yaw) : CVector3f(roll, pitch, yaw) {}
+
+  static CEulerAngles FromTransform(const CTransform4f&);
   static CEulerAngles FromQuaternion(const CQuaternion&);
+
+private:
+  static CEulerAngles sIdentity;
 };
 
 #endif // _CEULERANGLES

src/MetroidPrime/CEulerAngles.cpp

@@ -1,9 +1,29 @@
 #include "MetroidPrime/CEulerAngles.hpp"
 
 #include "Kyoto/Math/CMath.hpp"
+#include "Kyoto/Math/CMatrix3f.hpp"
 #include "Kyoto/Math/CQuaternion.hpp"
+#include "Kyoto/Math/CTransform4f.hpp"
 #include "Kyoto/Math/CloseEnough.hpp"
 
+CEulerAngles CEulerAngles::sIdentity(0.f, 0.f, 0.f);
+
+CEulerAngles CEulerAngles::FromTransform(const CTransform4f& xf) {
+  // TODO: actually CMatrix3f::FromTransform and uninlined, why?
+  CMatrix3f mtx(xf);
+  float sq = sqrt(mtx.Get11() * mtx.Get11() + mtx.Get01() * mtx.Get01());
+  if (!close_enough(sq, 0.f)) {
+    double yaw = -atan2(mtx.Get01(), mtx.Get11());
+    double pitch = -atan2(mtx.Get20(), mtx.Get22());
+    double roll = -atan2(-mtx.Get21(), sq);
+    return CEulerAngles(roll, pitch, yaw);
+  } else {
+    double pitch = -atan2(-mtx.Get02(), mtx.Get00());
+    double roll = -atan2(-mtx.Get21(), sq);
+    return CEulerAngles(roll, pitch, 0.f);
+  }
+}
+
 CEulerAngles CEulerAngles::FromQuaternion(const CQuaternion& quat) {
   float quatDot = CQuaternion::Dot(quat, quat);
   float t0 = 0.f;