More CPhysicsActor

include/Kyoto/Math/CAxisAngle.hpp

@@ -1,9 +0,0 @@
-#ifndef __CAXISANGLE_HPP__
-#define __CAXISANGLE_HPP__
-
-#include "Kyoto/Math/CVector3f.hpp"
-
-class CAxisAngle : public CVector3f {
-public:
-};
-#endif // __CAXISANGLE_HPP__

include/Kyoto/Math/CMatrix3f.hpp

@@ -9,8 +9,9 @@ class CMatrix3f {
   static const CMatrix3f sIdentity;
 public:
   CMatrix3f(const CMatrix3f&);
+  const CMatrix3f& operator=(const CMatrix3f& other);
 
-  static CMatrix3f Identity() { return sIdentity; }
+  static const CMatrix3f& Identity() { return sIdentity; }
 private:
   // TODO maybe individual f32s
   CVector3f m0;

include/MetroidPrime/CAxisAngle.hpp

@@ -6,25 +6,15 @@
 #include <math.h>
 #include "Kyoto/Math/CVector3f.hpp"
 
-class CAxisAngle : protected CVector3f {
+class CAxisAngle {
 public:
-  CAxisAngle(f32 x, f32 y, f32 z) : CVector3f(x, y, z) {}
-  //CAxisAngle(const CAxisAngle& other) : CVector3f(other) {}
-
-  static const CAxisAngle& Identity();
-  
+  CAxisAngle(f32 x, f32 y, f32 z) : mVector(x, y, z) {}
+  explicit CAxisAngle(const CVector3f& vec);
+  static const CAxisAngle& Identity();  
   const CVector3f& GetVector() const;
-/*
-  CAxisAngle& operator=(const CAxisAngle& other) {
-    int otherX = __HI(other.mX);
-    __HI(mX) = otherX;
-      int otherY = __HI(other.mY);
-    __HI(mY) = otherY;
-    int otherZ = __HI(other.mZ);
-    __HI(mZ) = otherZ;
-    return *this;
-  }
-  */
+  
+private:
+  CVector3f mVector;
 };
 
 CAxisAngle operator+(const CAxisAngle& lhs, const CAxisAngle& rhs);

include/MetroidPrime/CPhysicsActor.hpp

@@ -5,6 +5,7 @@
 
 #include "MetroidPrime/CActor.hpp"
 #include "MetroidPrime/CAxisAngle.hpp"
+#include "MetroidPrime/CPhysicsState.hpp"
 
 #include "Kyoto/Math/CAABox.hpp"
 #include "Kyoto/Math/CMatrix3f.hpp"
@@ -74,6 +75,12 @@ public:
   CAABox GetBoundingBox() const;
 
   void ApplyImpulseWR(const CVector3f& impulse, const CAxisAngle& angularImpulse);
+  void ApplyTorqueWR(const CVector3f& torque);
+  void ApplyForceWR(const CVector3f& force, const CAxisAngle& torque);
+
+  void ApplyImpulseOR(const CVector3f& impulse, const CAxisAngle& angularImpulse);
+  void ApplyForceOR(const CVector3f& impulse, const CAxisAngle& torque);
+
   void MoveCollisionPrimitive(const CVector3f&);
   void SetVelocityWR(const CVector3f&);
   void SetAngularVelocityWR(const CAxisAngle& angVel);
@@ -84,9 +91,12 @@ public:
   void ComputeDerivedQuantities();
   void Stop();
 
+  CPhysicsState GetPhysicsState() const;
+  void SetPhysicsState(const CPhysicsState& state);
   CMotionState GetMotionState() const;
   void SetMotionState(const CMotionState& state);
 
+
   bool GetMovable() const { return xf8_24_movable; }
   void SetMovable(bool v) { xf8_24_movable = v; }
 

include/MetroidPrime/CPhysicsState.hpp

@@ -1,7 +1,8 @@
 #ifndef __CPHYSICSSTATE_HPP__
 #define __CPHYSICSSTATE_HPP__
 
-#include "Kyoto/Math/CAxisAngle.hpp"
+#include "MetroidPrime/CAxisAngle.hpp"
+
 #include "Kyoto/Math/CQuaternion.hpp"
 #include "Kyoto/Math/CVector3f.hpp"
 
@@ -12,6 +13,16 @@ public:
                 const CVector3f& momentum, const CVector3f& force, const CVector3f& impulse,
                 const CAxisAngle& torque, const CAxisAngle& angularImpulse);
 
+  CVector3f GetTranslation() const { return x0_translation; }
+  CQuaternion GetOrientationWR() const { return xc_orientation; }
+  CVector3f GetConstantForceWR() const { return x1c_constantForce; }
+  CAxisAngle GetAngularMomentumWR() const { return x28_angularMomentum; }
+  CVector3f GetMomentumWR() const { return x34_momentum; }
+  CVector3f GetForceWR() const { return x40_force; }
+  CVector3f GetImpulseWR() const { return x4c_impulse; }
+  CAxisAngle GetTorque() const { return x58_torque; }
+  CAxisAngle GetAngularImpulseWR() const { return x64_angularImpulse; }
+
 private:
   CVector3f x0_translation;
   CQuaternion xc_orientation;

src/MetroidPrime/CPhysicsActor.cpp

@@ -8,24 +8,26 @@ CPhysicsActor::CPhysicsActor(TUniqueId uid, bool active, const rstl::string& nam
 : CActor(uid, active, name, info, xf, mData, matList, actParams, kInvalidUniqueId)
 , xe8_mass(moverData.x30_mass)
 , xec_massRecip(moverData.x30_mass > 0.f ? 1.f / moverData.x30_mass : 1.f)
+, xf0_inertiaTensor(0.f)
+, xf4_inertiaTensorRecip(0.f)
 , xf8_24_movable(true)
 , xf8_25_angularEnabled(false)
 , xf9_standardCollider(false)
-, xfc_constantForce(CVector3f::Zero())
+, xfc_constantForce(CVector3f(0.f, 0.f, 0.f))
 , x108_angularMomentum(CAxisAngle::Identity())
 , x114_(CMatrix3f::Identity())
-, x138_velocity(CVector3f::Zero())
+, x138_velocity(CVector3f(0.f, 0.f, 0.f))
 , x144_angularVelocity(CAxisAngle::Identity())
 , x150_momentum(moverData.x18_momentum)
-, x15c_force(CVector3f::Zero())
-, x168_impulse(CVector3f::Zero())
+, x15c_force(CVector3f(0.f, 0.f, 0.f))
+, x168_impulse(CVector3f(0.f, 0.f, 0.f))
 , x174_torque(CAxisAngle::Identity())
 , x180_angularImpulse(CAxisAngle::Identity())
-, x18c_moveImpulse(CVector3f::Zero())
+, x18c_moveImpulse(CVector3f(0.f, 0.f, 0.f))
 , x198_moveAngularImpulse(CAxisAngle::Identity())
 , x1a4_baseBoundingBox(aabb)
 , x1c0_collisionPrimitive(aabb, matList)
-, x1e8_primitiveOffset(CVector3f::Zero())
+, x1e8_primitiveOffset(xf.GetTranslation())
 , x1f4_lastNonCollidingState(xf.GetTranslation(),
                              CNUQuaternion::BuildFromMatrix3f(xf.BuildMatrix3f()),
                              CVector3f::Zero(), CAxisAngle::Identity())
@@ -43,10 +45,57 @@ CPhysicsActor::CPhysicsActor(TUniqueId uid, bool active, const rstl::string& nam
   ComputeDerivedQuantities();
 }
 
-
 CPhysicsActor::~CPhysicsActor() {}
 
 void CPhysicsActor::ApplyImpulseWR(const CVector3f& impulse, const CAxisAngle& angularImpulse) {
-  x168_impulse =  x168_impulse + impulse;
+  x168_impulse = x168_impulse + impulse;
   x180_angularImpulse = x180_angularImpulse + angularImpulse;
 }
+
+void CPhysicsActor::ApplyTorqueWR(const CVector3f& torque) {
+  x174_torque = x174_torque + CAxisAngle(torque);
+}
+
+void CPhysicsActor::ApplyForceWR(const CVector3f& force, const CAxisAngle& torque) {
+  x15c_force = x15c_force + force;
+  x174_torque = x174_torque + torque;
+}
+
+void CPhysicsActor::ApplyImpulseOR(const CVector3f& impulse, const CAxisAngle& angle) {
+  x168_impulse = x168_impulse + x34_transform.Rotate(impulse);
+  CAxisAngle rotatedAngle(x34_transform.Rotate(angle.GetVector()));
+  x180_angularImpulse = x180_angularImpulse + rotatedAngle;
+}
+
+void CPhysicsActor::ApplyForceOR(const CVector3f& force, const CAxisAngle& torque) {
+  x15c_force = x15c_force + x34_transform.Rotate(force);
+  CAxisAngle rotatedTorque(x34_transform.Rotate(torque.GetVector()));
+  x174_torque = x174_torque + rotatedTorque;
+}
+
+void CPhysicsActor::ComputeDerivedQuantities() {
+  x138_velocity = xfc_constantForce * xec_massRecip;
+  x114_ = x34_transform.BuildMatrix3f();
+  x144_angularVelocity = CAxisAngle(x108_angularMomentum.GetVector() * xf4_inertiaTensorRecip);
+}
+
+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);
+}
+
+
+void CPhysicsActor::SetPhysicsState(const CPhysicsState& state) {
+  SetTranslation(state.GetTranslation());
+  CQuaternion quat(state.GetOrientationWR());
+  SetTransform(quat.BuildTransform4f(GetTranslation()));
+  xfc_constantForce = state.GetConstantForceWR();
+  x108_angularMomentum = state.GetAngularMomentumWR();
+  x150_momentum = state.GetMomentumWR();
+  x15c_force = state.GetForceWR();
+  x168_impulse = state.GetImpulseWR();
+  x174_torque = state.GetTorque();
+  x180_angularImpulse = state.GetAngularImpulseWR();
+  ComputeDerivedQuantities();
+}