diff --git a/asm/MetroidPrime/CAutoMapper.s b/asm/MetroidPrime/CAutoMapper.s index 889e1097..347c6025 100644 --- a/asm/MetroidPrime/CAutoMapper.s +++ b/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) diff --git a/asm/MetroidPrime/CEulerAngles.s b/asm/MetroidPrime/CEulerAngles.s index c5a52bb0..11137a58 100644 --- a/asm/MetroidPrime/CEulerAngles.s +++ b/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 -FromMatrix3f__12CEulerAnglesFRC9CMatrix3f: +.global FromTransform__12CEulerAnglesFRC12CTransform4f +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) diff --git a/include/Kyoto/Math/CMatrix3f.hpp b/include/Kyoto/Math/CMatrix3f.hpp index c65f6251..4002b28c 100644 --- a/include/Kyoto/Math/CMatrix3f.hpp +++ b/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; diff --git a/include/MetroidPrime/CEulerAngles.hpp b/include/MetroidPrime/CEulerAngles.hpp index e873375d..aabc5eb6 100644 --- a/include/MetroidPrime/CEulerAngles.hpp +++ b/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 diff --git a/src/MetroidPrime/CEulerAngles.cpp b/src/MetroidPrime/CEulerAngles.cpp index 38bf41d8..1d395134 100644 --- a/src/MetroidPrime/CEulerAngles.cpp +++ b/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;