From 9fbf5962f59545a798f01f067eea58a715875c5c Mon Sep 17 00:00:00 2001 From: Henrique Gemignani Passos Lima Date: Fri, 4 Nov 2022 14:22:09 +0200 Subject: [PATCH] Add the rest of CPhysicsActor Former-commit-id: 55608d26be4554cbabb2d82df7e4d67b040f08cc --- asm/MetroidPrime/CPhysicsActor.s | 12 +- include/Kyoto/Math/CNUQuaternion.hpp | 1 + include/Kyoto/Math/CQuaternion.hpp | 3 +- include/MetroidPrime/CPhysicsActor.hpp | 17 ++- src/MetroidPrime/CPhysicsActor.cpp | 166 ++++++++++++++++++++++++- 5 files changed, 188 insertions(+), 11 deletions(-) diff --git a/asm/MetroidPrime/CPhysicsActor.s b/asm/MetroidPrime/CPhysicsActor.s index fa0f8c20..0b493854 100644 --- a/asm/MetroidPrime/CPhysicsActor.s +++ b/asm/MetroidPrime/CPhysicsActor.s @@ -3,8 +3,8 @@ .section .data .balign 8 -.global lbl_803E0628 -lbl_803E0628: +.global __vt__13CPhysicsActor +__vt__13CPhysicsActor: # ROM: 0x3DD628 .4byte 0 .4byte 0 @@ -2322,9 +2322,9 @@ __dt__13CPhysicsActorFv: /* 8011C428 00119388 93 C1 00 08 */ stw r30, 8(r1) /* 8011C42C 0011938C 7C 7E 1B 79 */ or. r30, r3, r3 /* 8011C430 00119390 41 82 00 64 */ beq lbl_8011C494 -/* 8011C434 00119394 3C 60 80 3E */ lis r3, lbl_803E0628@ha +/* 8011C434 00119394 3C 60 80 3E */ lis r3, __vt__13CPhysicsActor@ha /* 8011C438 00119398 34 1E 02 28 */ addic. r0, r30, 0x228 -/* 8011C43C 0011939C 38 03 06 28 */ addi r0, r3, lbl_803E0628@l +/* 8011C43C 0011939C 38 03 06 28 */ addi r0, r3, __vt__13CPhysicsActor@l /* 8011C440 001193A0 90 1E 00 00 */ stw r0, 0(r30) /* 8011C444 001193A4 41 82 00 0C */ beq lbl_8011C450 /* 8011C448 001193A8 38 00 00 00 */ li r0, 0 @@ -2384,9 +2384,9 @@ lbl_8011C494: /* 8011C508 00119468 38 81 00 14 */ addi r4, r1, 0x14 /* 8011C50C 0011946C 90 01 00 0C */ stw r0, 0xc(r1) /* 8011C510 00119470 4B F3 93 11 */ bl "__ct__6CActorF9TUniqueIdbRCQ24rstl66basic_string,Q24rstl17rmemory_allocator>RC11CEntityInfoRC12CTransform4fRC10CModelDataRC13CMaterialListRC16CActorParameters9TUniqueId" -/* 8011C514 00119474 3C 60 80 3E */ lis r3, lbl_803E0628@ha +/* 8011C514 00119474 3C 60 80 3E */ lis r3, __vt__13CPhysicsActor@ha /* 8011C518 00119478 C0 02 96 A4 */ lfs f0, lbl_805AB3C4@sda21(r2) -/* 8011C51C 0011947C 38 03 06 28 */ addi r0, r3, lbl_803E0628@l +/* 8011C51C 0011947C 38 03 06 28 */ addi r0, r3, __vt__13CPhysicsActor@l /* 8011C520 00119480 90 1D 00 00 */ stw r0, 0(r29) /* 8011C524 00119484 C0 3F 00 30 */ lfs f1, 0x30(r31) /* 8011C528 00119488 D0 3D 00 E8 */ stfs f1, 0xe8(r29) diff --git a/include/Kyoto/Math/CNUQuaternion.hpp b/include/Kyoto/Math/CNUQuaternion.hpp index 6c1e2d4c..dc8ea806 100644 --- a/include/Kyoto/Math/CNUQuaternion.hpp +++ b/include/Kyoto/Math/CNUQuaternion.hpp @@ -15,6 +15,7 @@ public: static CNUQuaternion BuildFromMatrix3f(const CMatrix3f& matrix); static CNUQuaternion BuildFromQuaternion(const CQuaternion& quat); + CNUQuaternion& operator+=(const CNUQuaternion&); CNUQuaternion operator*(const CNUQuaternion&) const; private: diff --git a/include/Kyoto/Math/CQuaternion.hpp b/include/Kyoto/Math/CQuaternion.hpp index 83a335c3..d1cb695b 100644 --- a/include/Kyoto/Math/CQuaternion.hpp +++ b/include/Kyoto/Math/CQuaternion.hpp @@ -73,7 +73,8 @@ public: float GetZ() const { return z; } const CVector3f& GetImaginary() const { // TODO: hack! has a Vector3f field? - return *reinterpret_cast(&x); } + return *reinterpret_cast(&x); + } private: float w; diff --git a/include/MetroidPrime/CPhysicsActor.hpp b/include/MetroidPrime/CPhysicsActor.hpp index 65217796..9f24fe2e 100644 --- a/include/MetroidPrime/CPhysicsActor.hpp +++ b/include/MetroidPrime/CPhysicsActor.hpp @@ -83,8 +83,12 @@ public: virtual float GetWeight() const; float GetMass() const { return xe8_mass; } void SetMass(float mass); + void SetInertiaTensorScalar(float tensor); + const CAABox& GetBaseBoundingBox() const; CAABox GetBoundingBox() const; + void SetBoundingBox(const CAABox& box); + CAABox GetMotionVolume(float dt) const; void ApplyImpulseWR(const CVector3f& impulse, const CAxisAngle& angularImpulse); void ApplyTorqueWR(const CVector3f& torque); @@ -106,6 +110,7 @@ public: bool WillMove(const CStateManager& mgr); void Stop(); + CVector3f GetPrimitiveOffset() const; const CVector3f& GetConstantForceWR() const { return xfc_constantForce; } void SetConstantForceWR(const CVector3f& force) { xfc_constantForce = force; } const CAxisAngle& GetAngularMomentumWR() const { return x108_angularMomentum; } @@ -125,6 +130,13 @@ public: x180_angularImpulse = angularImpulse; } + float GetCoefficientOfRestitutionModifier() const; + void SetCoefficientOfRestitutionModifier(float modifier); + float GetCollisionAccuracyModifier() const; + void SetCollisionAccuracyModifier(float modifier); + float GetMaximumCollisionVelocity() const; + void SetMaximumCollisionVelocity(float velocity); + CPhysicsState GetPhysicsState() const; void SetPhysicsState(const CPhysicsState& state); CMotionState GetMotionState() const; @@ -143,11 +155,14 @@ public: CVector3f GetMoveToORImpulseWR(const CVector3f& impulse, float d) const; CVector3f GetRotateToORAngularMomentumWR(const CQuaternion& q, float d) const; void RotateToWR(const CQuaternion&, float); + + void MoveInOneFrameOR(const CVector3f& trans, float d); + void RotateInOneFrameOR(const CQuaternion&, float); void MoveToOR(const CVector3f&, float); void RotateToOR(const CQuaternion&, float); - CVector3f GetTotalForcesWR() const; + CVector3f GetTotalForceWR() const; static float GravityConstant() { return kGravityAccel; } diff --git a/src/MetroidPrime/CPhysicsActor.cpp b/src/MetroidPrime/CPhysicsActor.cpp index a21b8d6a..b8ea648c 100644 --- a/src/MetroidPrime/CPhysicsActor.cpp +++ b/src/MetroidPrime/CPhysicsActor.cpp @@ -2,6 +2,8 @@ #include "Kyoto/Math/CloseEnough.hpp" +#include "rstl/math.hpp" + const float CPhysicsActor::kGravityAccel = 9.81f * 2.5f; CPhysicsActor::CPhysicsActor(TUniqueId uid, bool active, const rstl::string& name, @@ -173,7 +175,23 @@ CMotionState CPhysicsActor::GetMotionState() const { return CMotionState(GetTranslation(), nquat, GetConstantForceWR(), GetAngularMomentumWR()); } -void CPhysicsActor::AddMotionState(const CMotionState& state) {} +void CPhysicsActor::AddMotionState(const CMotionState& state) { + CNUQuaternion q(CNUQuaternion::BuildFromMatrix3f(GetTransform().BuildMatrix3f())); + q += state.GetOrientation(); + const CQuaternion& quat = CQuaternion::FromNUQuaternion(q); + + CVector3f transPos = GetTransform().GetTranslation(); + SetTransform(quat.BuildTransform4f(transPos)); + + transPos += state.GetTranslation(); + SetTranslation(transPos); + + xfc_constantForce += state.GetVelocity(); + x108_angularMomentum += state.GetAngularMomentum(); + + ComputeDerivedQuantities(); + +} bool CPhysicsActor::WillMove(const CStateManager& mgr) { if (close_enough(x138_velocity, CVector3f::Zero()) && @@ -183,7 +201,7 @@ bool CPhysicsActor::WillMove(const CStateManager& mgr) { 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())) { + close_enough(GetTotalForceWR(), CVector3f::Zero())) { return false; } @@ -234,8 +252,150 @@ CVector3f CPhysicsActor::GetRotateToORAngularMomentumWR(const CQuaternion& q, fl return CVector3f::Zero(); } else { const CVector3f rotated = GetTransform().Rotate(q.GetImaginary()); - + float ac = acos(q.GetW()); return rotated.AsNormalized() * ((ac * 2.0f) * (1.0f / d)) * xf0_inertiaTensor; } } + +void CPhysicsActor::MoveToOR(const CVector3f& trans, float d) { + xfc_constantForce = GetMoveToORImpulseWR(trans, d); + ComputeDerivedQuantities(); +} + +void CPhysicsActor::RotateToOR(const CQuaternion& q, float d) { + const CVector3f& vec = GetRotateToORAngularMomentumWR(q, d); + x108_angularMomentum = CAxisAngle(vec); + ComputeDerivedQuantities(); +} + +void CPhysicsActor::MoveInOneFrameOR(const CVector3f& trans, float d) { + x18c_moveImpulse += GetMoveToORImpulseWR(trans, d); +} + +void CPhysicsActor::RotateInOneFrameOR(const CQuaternion& q, float d) { + const CVector3f& vec = GetRotateToORAngularMomentumWR(q, d); + x198_moveAngularImpulse += CAxisAngle(vec); +} + +void CPhysicsActor::SetVelocityOR(const CVector3f& vel) { + SetVelocityWR(GetTransform().Rotate(vel)); +} + +CVector3f CPhysicsActor::GetTotalForceWR() const { return x15c_force + x150_momentum; } + +void CPhysicsActor::SetVelocityWR(const CVector3f& vel) { + x138_velocity = vel; + xfc_constantForce = xe8_mass * x138_velocity; +} + +void CPhysicsActor::SetAngularVelocityWR(const CAxisAngle& angVel) { + x144_angularVelocity = angVel; + x108_angularMomentum = CAxisAngle(x144_angularVelocity.GetVector() * xf0_inertiaTensor); +} + +CAxisAngle CPhysicsActor::GetAngularVelocityOR() const { + return CAxisAngle(GetTransform().TransposeRotate(x144_angularVelocity.GetVector())); +} + +void CPhysicsActor::SetAngularVelocityOR(const CAxisAngle& angVel) { + x144_angularVelocity = CAxisAngle(GetTransform().Rotate(angVel.GetVector())); + x108_angularMomentum = CAxisAngle(x144_angularVelocity.GetVector() * xf0_inertiaTensor); +} + +void CPhysicsActor::SetMass(float mass) { + xe8_mass = mass; + xec_massRecip = (xe8_mass > 0.0f) ? (1.0f / xe8_mass) : 1.0f; + SetInertiaTensorScalar(0.16666667f * xe8_mass); +} + +void CPhysicsActor::SetInertiaTensorScalar(float tensor) { + xf0_inertiaTensor = (tensor > 0.0f) ? tensor : 1.0f; + xf4_inertiaTensorRecip = 1.0f / xf0_inertiaTensor; +} + +const CCollisionPrimitive* CPhysicsActor::GetCollisionPrimitive() const { + return &x1c0_collisionPrimitive; +} + +void CPhysicsActor::MoveCollisionPrimitive(const CVector3f& offset) { + x1e8_primitiveOffset = offset; +} + +CTransform4f CPhysicsActor::GetPrimitiveTransform() const { + return CTransform4f::Translate(GetTransform().GetTranslation() + x1e8_primitiveOffset); +} + +void CPhysicsActor::CollidedWith(const TUniqueId& id, const CCollisionInfoList& list, + CStateManager& mgr) {} + +const CAABox& CPhysicsActor::GetBaseBoundingBox() const { return x1a4_baseBoundingBox; } + +CAABox CPhysicsActor::GetBoundingBox() const { + CVector3f off = x1e8_primitiveOffset + GetTransform().GetTranslation(); + return CAABox(x1a4_baseBoundingBox.GetMinPoint() + off, x1a4_baseBoundingBox.GetMaxPoint() + off); +} + +CAABox CPhysicsActor::GetMotionVolume(float dt) const { + CAABox aabox = GetCollisionPrimitive()->CalculateAABox(GetPrimitiveTransform()); + CVector3f velocity = CalculateNewVelocityWR_UsingImpulses(); + + const CVector3f dv = (dt * velocity); + aabox.AccumulateBounds(aabox.GetMaxPoint() + dv); + aabox.AccumulateBounds(aabox.GetMinPoint() + dv); + + float up = rstl::max_val(GetStepUpHeight(), 0.f); + aabox.AccumulateBounds(aabox.GetMaxPoint() + CVector3f(0.5f, 0.5f, up + 1.f)); + + float down = rstl::max_val(GetStepDownHeight(), 0.f); + aabox.AccumulateBounds(aabox.GetMinPoint() - CVector3f(0.5f, 0.5f, down + 1.5f)); + return aabox; +} + +void CPhysicsActor::SetBoundingBox(const CAABox& box) { + x1a4_baseBoundingBox = box; + MoveCollisionPrimitive(CVector3f::Zero()); +} + +float CPhysicsActor::GetWeight() const { return CPhysicsActor::GravityConstant() * GetMass(); } + +CVector3f CPhysicsActor::GetPrimitiveOffset() const { return x1e8_primitiveOffset; } + +float CPhysicsActor::GetStepDownHeight() const { return x240_stepDownHeight; } + +float CPhysicsActor::GetStepUpHeight() const { return x23c_stepUpHeight; } + +CVector3f CPhysicsActor::GetOrbitPosition(const CStateManager&) const { + return GetBoundingBox().GetCenterPoint(); +} + +CVector3f CPhysicsActor::GetAimPosition(const CStateManager&, float dt) const { + if (dt > 0.0f) { + CVector3f trans = PredictMotion(dt).GetTranslation(); + return GetBoundingBox().GetCenterPoint() + trans; + } else { + return GetBoundingBox().GetCenterPoint(); + } +} + +void CPhysicsActor::Render(const CStateManager& mgr) const { CActor::Render(mgr); } + +void CPhysicsActor::SetCoefficientOfRestitutionModifier(float modifier) { + x244_restitutionCoefModifier = modifier; +} + +float CPhysicsActor::GetCoefficientOfRestitutionModifier() const { + return x244_restitutionCoefModifier; +} + +float CPhysicsActor::GetCollisionAccuracyModifier() const { return x248_collisionAccuracyModifier; } + +void CPhysicsActor::SetCollisionAccuracyModifier(float modifier) { + x248_collisionAccuracyModifier = modifier; +} + +float CPhysicsActor::GetMaximumCollisionVelocity() const { return x238_maximumCollisionVelocity; } + +void CPhysicsActor::SetMaximumCollisionVelocity(float velocity) { + x238_maximumCollisionVelocity = velocity; +}