More CPhysicsActor

Former-commit-id: bc81e35c6b

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;

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;

include/MetroidPrime/CAxisAngle.hpp

@@ -13,6 +13,7 @@ public:
   static const CAxisAngle& Identity();  
   const CVector3f& GetVector() const;
 
+  const CAxisAngle& operator+=(const CAxisAngle& rhs);
 private:
   CVector3f mVector;
 };

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:

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