Add the rest of CPhysicsActor

asm/MetroidPrime/CPhysicsActor.s

@@ -3,8 +3,8 @@
 .section .data
 .balign 8
 
-.global lbl_803E0628
+.global __vt__13CPhysicsActor
-lbl_803E0628:
+__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<c,Q24rstl14char_traits<c>,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)

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:

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<const CVector3f*>(&x); }
+    return *reinterpret_cast<const CVector3f*>(&x);
+  }
 
 private:
   float w;

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; }
 

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;
+}