From 629a01420729a00a7e29183167fddcd2bf654cca Mon Sep 17 00:00:00 2001 From: Henrique Gemignani Passos Lima Date: Tue, 18 Oct 2022 02:53:49 +0300 Subject: [PATCH] Improving match of CPhysicsActor --- include/Kyoto/Math/CNUQuaternion.hpp | 9 +++ include/Kyoto/Math/CQuaternion.hpp | 2 + include/MetroidPrime/CPhysicsActor.hpp | 5 ++ src/MetroidPrime/CPhysicsActor.cpp | 90 +++++++++++++++++--------- 4 files changed, 75 insertions(+), 31 deletions(-) diff --git a/include/Kyoto/Math/CNUQuaternion.hpp b/include/Kyoto/Math/CNUQuaternion.hpp index 71fb02c5..6c1e2d4c 100644 --- a/include/Kyoto/Math/CNUQuaternion.hpp +++ b/include/Kyoto/Math/CNUQuaternion.hpp @@ -4,13 +4,19 @@ #include "types.h" class CMatrix3f; +class CVector3f; +class CQuaternion; + class CNUQuaternion { public: CNUQuaternion(float w, float x, float y, float z) : w(w), x(x), y(y), z(z) {} + CNUQuaternion(float, const CVector3f&); static CNUQuaternion BuildFromMatrix3f(const CMatrix3f& matrix); static CNUQuaternion BuildFromQuaternion(const CQuaternion& quat); + CNUQuaternion operator*(const CNUQuaternion&) const; + private: float w; float x; @@ -18,4 +24,7 @@ private: float z; }; +CNUQuaternion operator*(float f, const CNUQuaternion&); +CNUQuaternion operator*(const CNUQuaternion&, float f); + #endif // _CNUQUATERNION diff --git a/include/Kyoto/Math/CQuaternion.hpp b/include/Kyoto/Math/CQuaternion.hpp index c82c194a..93d906c5 100644 --- a/include/Kyoto/Math/CQuaternion.hpp +++ b/include/Kyoto/Math/CQuaternion.hpp @@ -8,6 +8,7 @@ class CRelAngle; class CUnitVector3f; +class CNUQuaternion; class CQuaternion { public: @@ -57,6 +58,7 @@ public: static CQuaternion FromMatrixRows(const CVector3f&, const CVector3f&, const CVector3f&); static CQuaternion FromMatrix(const CMatrix3f&); static CQuaternion FromMatrix(const CTransform4f&); + static CQuaternion FromNUQuaternion(const CNUQuaternion&); static const CQuaternion& NoRotation() { return sNoRotation; } diff --git a/include/MetroidPrime/CPhysicsActor.hpp b/include/MetroidPrime/CPhysicsActor.hpp index 396872d1..ef276688 100644 --- a/include/MetroidPrime/CPhysicsActor.hpp +++ b/include/MetroidPrime/CPhysicsActor.hpp @@ -41,6 +41,11 @@ public: , x1c_velocity(velocity) , x28_angularMomentum(angularMomentum) {} + const CVector3f& GetTranslation() const { return x0_translation; } + const CNUQuaternion& GetOrientation() const { return xc_orientation; } + const CVector3f& GetVelocity() const { return x1c_velocity; } + const CAxisAngle& GetAngularMomentum() const { return x28_angularMomentum; } + private: CVector3f x0_translation; CNUQuaternion xc_orientation; diff --git a/src/MetroidPrime/CPhysicsActor.cpp b/src/MetroidPrime/CPhysicsActor.cpp index 8f1b779b..efbd7d7f 100644 --- a/src/MetroidPrime/CPhysicsActor.cpp +++ b/src/MetroidPrime/CPhysicsActor.cpp @@ -91,8 +91,8 @@ CPhysicsState CPhysicsActor::GetPhysicsState() const { void CPhysicsActor::SetPhysicsState(const CPhysicsState& state) { SetTranslation(state.GetTranslation()); - CQuaternion quat = state.GetOrientationWR(); - CVector3f translation = GetTranslation(); + const CQuaternion& quat = state.GetOrientationWR(); + const CVector3f& translation = GetTranslation(); SetTransform(quat.BuildTransform4f(translation)); SetConstantForceWR(state.GetConstantForceWR()); SetAngularMomentumWR(state.GetAngularMomentumWR()); @@ -105,43 +105,71 @@ void CPhysicsActor::SetPhysicsState(const CPhysicsState& state) { } 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)); + return x138_velocity + xec_massRecip * (x168_impulse + x18c_moveImpulse); } -CMotionState CPhysicsActor::PredictMotion(float dt) const {} +CMotionState CPhysicsActor::PredictMotion(float dt) const { + const CMotionState& msl = PredictLinearMotion(dt); + CVector3f translation = msl.GetTranslation(); + CVector3f velocity = msl.GetVelocity(); -CMotionState CPhysicsActor::PredictAngularMotion(float dt) const {} + const CMotionState& msa = PredictAngularMotion(dt); + CNUQuaternion orientation = msa.GetOrientation(); + CAxisAngle angularMomentum = msa.GetAngularMomentum(); -CMotionState CPhysicsActor::PredictLinearMotion(float dt) const {} + return CMotionState(translation, orientation, velocity, angularMomentum); +} -CMotionState CPhysicsActor::PredictMotion_Internal(float dt) const {} +CMotionState CPhysicsActor::PredictAngularMotion(float dt) const { + CVector3f v1 = (x180_angularImpulse.GetVector() + x198_moveAngularImpulse.GetVector()) * + xf4_inertiaTensorRecip; + CVector3f v2 = x144_angularVelocity.GetVector() + v1; -void CPhysicsActor::SetMotionState(const CMotionState& state) {} + CNUQuaternion q3 = (0.5f * CNUQuaternion(0.f, v2)) * + CNUQuaternion::BuildFromQuaternion(CQuaternion::FromMatrix(GetTransform())); + CAxisAngle torque = x174_torque; + + return CMotionState(CVector3f::Zero(), q3 * dt, CVector3f::Zero(), + (torque * dt) + x180_angularImpulse); +} + +CMotionState CPhysicsActor::PredictLinearMotion(float dt) const { + CVector3f velocity = CalculateNewVelocityWR_UsingImpulses(); + CVector3f sum = x15c_force + x150_momentum; + + return CMotionState(dt * velocity, CNUQuaternion(0.0f, CVector3f::Zero()), + dt * sum + x168_impulse, CAxisAngle::Identity()); +} + +CMotionState CPhysicsActor::PredictMotion_Internal(float dt) const { + if (!xf8_25_angularEnabled) { + + const CMotionState& msl = PredictLinearMotion(dt); + CVector3f translation = msl.GetTranslation(); + CVector3f velocity = msl.GetVelocity(); + + const CMotionState& msa = PredictAngularMotion(dt); + CNUQuaternion orientation = msa.GetOrientation(); + CAxisAngle angularMomentum = msa.GetAngularMomentum(); + + return CMotionState(translation, orientation, velocity, angularMomentum); + } else { + return PredictLinearMotion(dt); + } +} + +void CPhysicsActor::SetMotionState(const CMotionState& state) { + const CQuaternion& q = CQuaternion::FromNUQuaternion(state.GetOrientation()); + SetTransform(q.BuildTransform4f(x34_transform.GetTranslation())); + SetTranslation(state.GetTranslation()); + + xfc_constantForce = state.GetVelocity(); + x108_angularMomentum = state.GetAngularMomentum(); + ComputeDerivedQuantities(); +} CMotionState CPhysicsActor::GetMotionState() const { - CNUQuaternion nquat(CNUQuaternion::BuildFromQuaternion(GetRotation())); + const CNUQuaternion& nquat = CNUQuaternion::BuildFromQuaternion(GetRotation()); return CMotionState(GetTranslation(), nquat, GetConstantForceWR(), GetAngularMomentumWR()); }