From aa1df3e7d7bac6a99b0749a8c1ffa99c30eb2f2e Mon Sep 17 00:00:00 2001
From: Phillip Stephens <antidote.crk@gmail.com>
Date: Wed, 5 Oct 2022 10:53:15 -0700
Subject: [PATCH] Get/SetPhysicsState fixes

Former-commit-id: 75909783bc68d0fbd7a4605bbc0e5862b28d2dff
---
 include/MetroidPrime/CActor.hpp        |  5 ++++-
 include/MetroidPrime/CPhysicsActor.hpp | 13 +++++++++++++
 include/MetroidPrime/CPhysicsState.hpp |  2 +-
 src/MetroidPrime/CPhysicsActor.cpp     | 11 ++++++-----
 4 files changed, 24 insertions(+), 7 deletions(-)

diff --git a/include/MetroidPrime/CActor.hpp b/include/MetroidPrime/CActor.hpp
index 8d557282..face437c 100644
--- a/include/MetroidPrime/CActor.hpp
+++ b/include/MetroidPrime/CActor.hpp
@@ -265,7 +265,10 @@ public:
   void SetRotation(const CQuaternion& rot) {
     SetTransform(rot.BuildTransform4f(GetTranslation()));
   }
-  CVector3f GetTranslation() const { return x34_transform.GetTranslation(); }
+  const CQuaternion& GetRotation() const { 
+    return CQuaternion::FromMatrix(GetTransform());
+  }
+  const CVector3f& GetTranslation() const { return x34_transform.GetTranslation(); }
   void SetTranslation(const CVector3f& vec);
   CTransform4f GetLocatorTransform(const rstl::string& segName) const;
   CTransform4f GetScaledLocatorTransform(const rstl::string& segName) const;
diff --git a/include/MetroidPrime/CPhysicsActor.hpp b/include/MetroidPrime/CPhysicsActor.hpp
index 5b32a146..105b33cb 100644
--- a/include/MetroidPrime/CPhysicsActor.hpp
+++ b/include/MetroidPrime/CPhysicsActor.hpp
@@ -52,6 +52,8 @@ CHECK_SIZEOF(CMotionState, 0x34)
 class CCollisionInfoList;
 
 class CPhysicsActor : public CActor {
+  static const float skGravityConstant;
+
 public:
   CPhysicsActor(TUniqueId uid, bool active, const rstl::string& name, const CEntityInfo& info,
                 const CTransform4f& xf, const CModelData& mData, const CMaterialList& matList,
@@ -91,6 +93,15 @@ public:
   void ComputeDerivedQuantities();
   void Stop();
 
+  const CVector3f& GetConstantForceWR() const { return xfc_constantForce; }
+  void SetConstantForceWR(const CVector3f& force) { xfc_constantForce = force; }
+  const CAxisAngle& GetAngularMomentumWR() const { return x108_angularMomentum; }
+  const CVector3f& GetMomentumWR() const { return x150_momentum; }
+  const CVector3f& GetForceWR() const { return x15c_force; }
+  const CVector3f& GetImpulseWR() const { return x168_impulse; }
+  const CAxisAngle& GetTorqueWR() const { return x174_torque; }
+  const CAxisAngle& GetAngularImpulseWR() const { return x180_angularImpulse; }
+
   CPhysicsState GetPhysicsState() const;
   void SetPhysicsState(const CPhysicsState& state);
   CMotionState GetMotionState() const;
@@ -100,6 +111,8 @@ public:
   bool GetMovable() const { return xf8_24_movable; }
   void SetMovable(bool v) { xf8_24_movable = v; }
 
+  static float GetGravityConstant() { return skGravityConstant; }
+
 private:
   f32 xe8_mass;
   f32 xec_massRecip;
diff --git a/include/MetroidPrime/CPhysicsState.hpp b/include/MetroidPrime/CPhysicsState.hpp
index b8d738a2..c37811e2 100644
--- a/include/MetroidPrime/CPhysicsState.hpp
+++ b/include/MetroidPrime/CPhysicsState.hpp
@@ -14,7 +14,7 @@ public:
                 const CAxisAngle& torque, const CAxisAngle& angularImpulse);
 
   CVector3f GetTranslation() const { return x0_translation; }
-  CQuaternion GetOrientationWR() const { return xc_orientation; }
+  const CQuaternion& GetOrientationWR() const { return xc_orientation; }
   CVector3f GetConstantForceWR() const { return x1c_constantForce; }
   CAxisAngle GetAngularMomentumWR() const { return x28_angularMomentum; }
   CVector3f GetMomentumWR() const { return x34_momentum; }
diff --git a/src/MetroidPrime/CPhysicsActor.cpp b/src/MetroidPrime/CPhysicsActor.cpp
index 88f4064f..d44db0ce 100644
--- a/src/MetroidPrime/CPhysicsActor.cpp
+++ b/src/MetroidPrime/CPhysicsActor.cpp
@@ -1,5 +1,7 @@
 #include "MetroidPrime/CPhysicsActor.hpp"
 
+const float CPhysicsActor::skGravityConstant = 9.81f * 2.5f;
+
 CPhysicsActor::CPhysicsActor(TUniqueId uid, bool active, const rstl::string& name,
                              const CEntityInfo& info, const CTransform4f& xf,
                              const CModelData& mData, const CMaterialList& matList,
@@ -80,15 +82,14 @@ void CPhysicsActor::ComputeDerivedQuantities() {
 }
 
 CPhysicsState CPhysicsActor::GetPhysicsState() const {
-  CQuaternion quat(CQuaternion::FromMatrix(x34_transform));
-  return CPhysicsState(GetTranslation(), quat, xfc_constantForce, x108_angularMomentum,
-                       x150_momentum, x15c_force, x168_impulse, x174_torque, x180_angularImpulse);
+  return CPhysicsState(GetTranslation(), GetRotation(), GetConstantForceWR(),
+                       GetAngularMomentumWR(), GetMomentumWR(), GetForceWR(), GetImpulseWR(),
+                       GetTorqueWR(), GetAngularImpulseWR());
 }
 
-
 void CPhysicsActor::SetPhysicsState(const CPhysicsState& state) {
   SetTranslation(state.GetTranslation());
-  CQuaternion quat(state.GetOrientationWR());
+  CQuaternion quat = state.GetOrientationWR();
   SetTransform(quat.BuildTransform4f(GetTranslation()));
   xfc_constantForce = state.GetConstantForceWR();
   x108_angularMomentum = state.GetAngularMomentumWR();