diff --git a/include/Kyoto/Math/CQuaternion.hpp b/include/Kyoto/Math/CQuaternion.hpp index 87ebb18b..83a335c3 100644 --- a/include/Kyoto/Math/CQuaternion.hpp +++ b/include/Kyoto/Math/CQuaternion.hpp @@ -71,6 +71,9 @@ public: float GetX() const { return x; } float GetY() const { return y; } float GetZ() const { return z; } + const CVector3f& GetImaginary() const { + // TODO: hack! has a Vector3f field? + return *reinterpret_cast(&x); } private: float w; diff --git a/include/MetroidPrime/CPhysicsActor.hpp b/include/MetroidPrime/CPhysicsActor.hpp index 5490f5eb..65217796 100644 --- a/include/MetroidPrime/CPhysicsActor.hpp +++ b/include/MetroidPrime/CPhysicsActor.hpp @@ -141,7 +141,7 @@ public: void MoveToWR(const CVector3f&, float); void MoveToInOneFrameWR(const CVector3f&, float); CVector3f GetMoveToORImpulseWR(const CVector3f& impulse, float d) const; - CAxisAngle GetRotateToORAngularMomentumWR(const CQuaternion& q, float d) const; + CVector3f GetRotateToORAngularMomentumWR(const CQuaternion& q, float d) const; void RotateToWR(const CQuaternion&, float); void MoveToOR(const CVector3f&, float); diff --git a/libc/math.h b/libc/math.h index 885a0815..e1f4d940 100644 --- a/libc/math.h +++ b/libc/math.h @@ -76,6 +76,7 @@ double atan2(double y, double x); _MATH_INLINE float sinf(float x) { return (float)sin((double)x); } _MATH_INLINE float cosf(float x) { return (float)cos((double)x); } float tanf(float x); +double acos(double x); float acosf(float x); double ldexp(double x, int exp); diff --git a/src/MetroidPrime/CPhysicsActor.cpp b/src/MetroidPrime/CPhysicsActor.cpp index bcc243ac..a21b8d6a 100644 --- a/src/MetroidPrime/CPhysicsActor.cpp +++ b/src/MetroidPrime/CPhysicsActor.cpp @@ -229,7 +229,13 @@ CVector3f CPhysicsActor::GetMoveToORImpulseWR(const CVector3f& trans, float d) c return (GetMass() * impulse) * (1.f / d); } -CAxisAngle CPhysicsActor::GetRotateToORAngularMomentumWR(const CQuaternion& q, float d) const { - // TODO - return CAxisAngle(); +CVector3f CPhysicsActor::GetRotateToORAngularMomentumWR(const CQuaternion& q, float d) const { + if (q.GetW() > 0.99999976f) { + return CVector3f::Zero(); + } else { + const CVector3f rotated = GetTransform().Rotate(q.GetImaginary()); + + float ac = acos(q.GetW()); + return rotated.AsNormalized() * ((ac * 2.0f) * (1.0f / d)) * xf0_inertiaTensor; + } }