From bc81e35c6bc9cdc17d972dce1aae2499c463db06 Mon Sep 17 00:00:00 2001 From: Phillip Stephens Date: Wed, 5 Oct 2022 15:50:12 -0700 Subject: [PATCH] More CPhysicsActor --- include/Kyoto/Math/CNUQuaternion.hpp | 1 + include/Kyoto/Math/CVector3f.hpp | 8 +++ include/MetroidPrime/CAxisAngle.hpp | 3 +- include/MetroidPrime/CPhysicsActor.hpp | 17 ++++- src/MetroidPrime/CPhysicsActor.cpp | 92 ++++++++++++++++++++++++++ 5 files changed, 118 insertions(+), 3 deletions(-) diff --git a/include/Kyoto/Math/CNUQuaternion.hpp b/include/Kyoto/Math/CNUQuaternion.hpp index d2a7db1c..ce2f1f99 100644 --- a/include/Kyoto/Math/CNUQuaternion.hpp +++ b/include/Kyoto/Math/CNUQuaternion.hpp @@ -9,6 +9,7 @@ public: CNUQuaternion(f32 w, f32 x, f32 y, f32 z) : w(w), x(x), y(y), z(z) {} static CNUQuaternion BuildFromMatrix3f(const CMatrix3f& matrix); + static CNUQuaternion BuildFromQuaternion(const CQuaternion& quat); private: f32 w; f32 x; diff --git a/include/Kyoto/Math/CVector3f.hpp b/include/Kyoto/Math/CVector3f.hpp index e58197e9..36276636 100644 --- a/include/Kyoto/Math/CVector3f.hpp +++ b/include/Kyoto/Math/CVector3f.hpp @@ -149,6 +149,14 @@ inline CVector3f operator*(const CVector3f& vec, f32 f) { f32 z = vec.GetZ() * f; return CVector3f(x, y, z); } + +inline CVector3f operator*(f32 f, const CVector3f& vec) { + f32 x = f * vec.GetX(); + f32 y = f * vec.GetY(); + f32 z = f * vec.GetZ(); + return CVector3f(x, y, z); +} + inline CVector3f operator/(const CVector3f& vec, f32 f) { f32 x = vec.GetX() / f; f32 y = vec.GetY() / f; diff --git a/include/MetroidPrime/CAxisAngle.hpp b/include/MetroidPrime/CAxisAngle.hpp index 0dce6be7..761c8f5b 100644 --- a/include/MetroidPrime/CAxisAngle.hpp +++ b/include/MetroidPrime/CAxisAngle.hpp @@ -12,7 +12,8 @@ public: explicit CAxisAngle(const CVector3f& vec); static const CAxisAngle& Identity(); const CVector3f& GetVector() const; - + + const CAxisAngle& operator+=(const CAxisAngle& rhs); private: CVector3f mVector; }; diff --git a/include/MetroidPrime/CPhysicsActor.hpp b/include/MetroidPrime/CPhysicsActor.hpp index 84740f63..04c77f0f 100644 --- a/include/MetroidPrime/CPhysicsActor.hpp +++ b/include/MetroidPrime/CPhysicsActor.hpp @@ -72,6 +72,7 @@ public: virtual f32 GetStepDownHeight() const; virtual f32 GetStepUpHeight() const; virtual f32 GetWeight() const; + float GetMass() const { return xe8_mass; } void SetMass(float mass); CAABox GetBoundingBox() const; @@ -90,7 +91,10 @@ public: CAxisAngle GetAngularVelocityOR() const; void SetAngularVelocityOR(const CAxisAngle& angleVel); void ClearForcesAndTorques(); + void ClearImpulses(); void ComputeDerivedQuantities(); + void UseCollisionImpulses(); + bool WillMove(const CStateManager& mgr); void Stop(); const CVector3f& GetConstantForceWR() const { return xfc_constantForce; } @@ -106,14 +110,23 @@ public: void SetPhysicsState(const CPhysicsState& state); CMotionState GetMotionState() const; void SetMotionState(const CMotionState& state); - - + CVector3f CalculateNewVelocityWR_UsingImpulses() const; + CMotionState PredictMotion(float dt) const; + CMotionState PredictAngularMotion(float dt) const; + CMotionState PredictLinearMotion(float dt) const; + CMotionState PredictMotion_Internal(float dt) const; + void AddMotionState(const CMotionState& state); bool GetMovable() const { return xf8_24_movable; } void SetMovable(bool v) { xf8_24_movable = v; } + void MoveToWR(const CVector3f&, float); + void RotateToWR(const CQuaternion&, float); + void MoveToOR(const CVector3f&, float); void RotateToOR(const CQuaternion&, float); + CVector3f GetTotalForcesWR() const; + static float GetGravityConstant() { return skGravityConstant; } private: diff --git a/src/MetroidPrime/CPhysicsActor.cpp b/src/MetroidPrime/CPhysicsActor.cpp index d44db0ce..3fe54389 100644 --- a/src/MetroidPrime/CPhysicsActor.cpp +++ b/src/MetroidPrime/CPhysicsActor.cpp @@ -1,5 +1,7 @@ #include "MetroidPrime/CPhysicsActor.hpp" +#include "Kyoto/Math/CloseEnough.hpp" + const float CPhysicsActor::skGravityConstant = 9.81f * 2.5f; CPhysicsActor::CPhysicsActor(TUniqueId uid, bool active, const rstl::string& name, @@ -100,3 +102,93 @@ void CPhysicsActor::SetPhysicsState(const CPhysicsState& state) { x180_angularImpulse = state.GetAngularImpulseWR(); ComputeDerivedQuantities(); } + +CVector3f CPhysicsActor::CalculateNewVelocityWR_UsingImpulses() const { + float impX; + float impY; + float impZ; + float masRecip; + float moveImpX; + float moveImpY; + float moveImpZ; + float velX; + float velY; + float velZ; + + impX = x168_impulse.GetX(); + impY = x168_impulse.GetY(); + impZ = x168_impulse.GetZ(); + moveImpX = x18c_moveImpulse.GetX(); + moveImpY = x18c_moveImpulse.GetY(); + moveImpZ = x18c_moveImpulse.GetZ(); + masRecip = xec_massRecip; + velY = x138_velocity.GetY(); + velZ = x138_velocity.GetZ(); + + return CVector3f(velX + masRecip * (impX + moveImpX), velY + masRecip * (impY + moveImpY), + velZ + masRecip * (impZ + moveImpZ)); +} + +CMotionState CPhysicsActor::PredictMotion(float dt) const {} + +CMotionState CPhysicsActor::PredictAngularMotion(float dt) const {} + +CMotionState CPhysicsActor::PredictLinearMotion(float dt) const {} + +CMotionState CPhysicsActor::PredictMotion_Internal(float dt) const {} + +void CPhysicsActor::SetMotionState(const CMotionState& state) {} + +CMotionState CPhysicsActor::GetMotionState() const { + CNUQuaternion nquat(CNUQuaternion::BuildFromQuaternion(GetRotation())); + return CMotionState(GetTranslation(), nquat, GetConstantForceWR(), GetAngularMomentumWR()); +} + +void CPhysicsActor::AddMotionState(const CMotionState& state) {} + +bool CPhysicsActor::WillMove(const CStateManager& mgr) { + if (close_enough(x138_velocity, CVector3f::Zero()) && + close_enough(x168_impulse, CVector3f::Zero()) && + close_enough(x174_torque.GetVector(), CVector3f::Zero()) && + close_enough(x18c_moveImpulse, CVector3f::Zero()) && + close_enough(x144_angularVelocity.GetVector(), CVector3f::Zero()) && + close_enough(x180_angularImpulse.GetVector(), CVector3f::Zero()) && + close_enough(x198_moveAngularImpulse.GetVector(), CVector3f::Zero()) && + close_enough(GetTotalForcesWR(), CVector3f::Zero())) { + return false; + } + + return true; +} + +void CPhysicsActor::Stop() { + ClearForcesAndTorques(); + xfc_constantForce = CVector3f::Zero(); + x108_angularMomentum = CAxisAngle::Identity(); + ComputeDerivedQuantities(); +} + +void CPhysicsActor::ClearForcesAndTorques() { + x15c_force = x168_impulse = x18c_moveImpulse = CVector3f::Zero(); + x174_torque = x180_angularImpulse = x198_moveAngularImpulse = CAxisAngle::Identity(); +} + +void CPhysicsActor::ClearImpulses() { + x168_impulse = x18c_moveImpulse = CVector3f::Zero(); + x180_angularImpulse = x198_moveAngularImpulse = CAxisAngle::Identity(); +} + +void CPhysicsActor::UseCollisionImpulses() { + xfc_constantForce += x168_impulse; + x108_angularMomentum += x180_angularImpulse; + x168_impulse = CVector3f::Zero(); + x180_angularImpulse = CAxisAngle::Identity(); + ComputeDerivedQuantities(); +} + +void CPhysicsActor::MoveToWR(const CVector3f& trans, float d) { + float f = (1.f / d); + CVector3f diff = trans - GetTranslation(); + xfc_constantForce = f * GetMass() * diff; + ComputeDerivedQuantities(); +}