diff --git a/include/Kyoto/Math/CAxisAngle.hpp b/include/Kyoto/Math/CAxisAngle.hpp deleted file mode 100644 index 5095fdb7..00000000 --- a/include/Kyoto/Math/CAxisAngle.hpp +++ /dev/null @@ -1,9 +0,0 @@ -#ifndef __CAXISANGLE_HPP__ -#define __CAXISANGLE_HPP__ - -#include "Kyoto/Math/CVector3f.hpp" - -class CAxisAngle : public CVector3f { -public: -}; -#endif // __CAXISANGLE_HPP__ diff --git a/include/Kyoto/Math/CMatrix3f.hpp b/include/Kyoto/Math/CMatrix3f.hpp index d618a1b1..ec4838f5 100644 --- a/include/Kyoto/Math/CMatrix3f.hpp +++ b/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; diff --git a/include/MetroidPrime/CAxisAngle.hpp b/include/MetroidPrime/CAxisAngle.hpp index 8ce3ec8a..0dce6be7 100644 --- a/include/MetroidPrime/CAxisAngle.hpp +++ b/include/MetroidPrime/CAxisAngle.hpp @@ -6,25 +6,15 @@ #include #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); diff --git a/include/MetroidPrime/CPhysicsActor.hpp b/include/MetroidPrime/CPhysicsActor.hpp index e8fdcd54..5b32a146 100644 --- a/include/MetroidPrime/CPhysicsActor.hpp +++ b/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; } diff --git a/include/MetroidPrime/CPhysicsState.hpp b/include/MetroidPrime/CPhysicsState.hpp index 08d7357f..b8d738a2 100644 --- a/include/MetroidPrime/CPhysicsState.hpp +++ b/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; diff --git a/src/MetroidPrime/CPhysicsActor.cpp b/src/MetroidPrime/CPhysicsActor.cpp index 58c70a62..88f4064f 100644 --- a/src/MetroidPrime/CPhysicsActor.cpp +++ b/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(); +}