From 7c8204f02d80ad9853087896492d18230169a673 Mon Sep 17 00:00:00 2001
From: Rtch90 <ritchie.cunningham@protonmail.com>
Date: Sun, 8 Apr 2018 17:18:07 +0100
Subject: [PATCH] [Change] Some further work on new dynamics. [Fix] Load/Save
 works again. [Meh] Collision test ok, but no response.

---
 src/dynamic_body.cpp | 38 +++++++++++++++++++++++++++-----------
 src/model_body.cpp   |  2 +-
 src/model_body.h     |  2 +-
 src/space.cpp        |  3 ++-
 4 files changed, 31 insertions(+), 14 deletions(-)

diff --git a/src/dynamic_body.cpp b/src/dynamic_body.cpp
index 246bf77..b1d402e 100644
--- a/src/dynamic_body.cpp
+++ b/src/dynamic_body.cpp
@@ -43,18 +43,31 @@ void DynamicBody::AddRelTorque(const vector3d t) {
 }
 
 void DynamicBody::Save(void) {
-  assert(0); /* Add new dynamics stuff. */
   using namespace Serializer::Write;
   ModelBody::Save();
-  wr_vector3d(GetAngVelocity());
-  wr_vector3d(GetVelocity());
+  for(int i = 0; i < 16; i++) wr_double(m_orient[i]);
+  wr_vector3d(m_force);
+  wr_vector3d(m_torque);
+  wr_vector3d(m_vel);
+  wr_vector3d(m_angVel);
+  wr_double(m_mass);
+  wr_double(m_massRadius);
+  wr_double(m_angInertia);
+  wr_bool(m_enabled);
 }
 
 void DynamicBody::Load(void) {
   using namespace Serializer::Read;
   ModelBody::Load();
-  SetAngVelocity(rd_vector3d());
-  SetVelocity(rd_vector3d());
+  for(int i = 0; i < 16; i++) m_orient[i] = rd_double();
+  m_force       = rd_vector3d();
+  m_torque      = rd_vector3d();
+  m_vel         = rd_vector3d();
+  m_angVel      = rd_vector3d();
+  m_mass        = rd_double();
+  m_massRadius  = rd_double();
+  m_angInertia  = rd_double();
+  m_enabled     = rd_bool();
 }
 
 void DynamicBody::SetTorque(const vector3d t) {
@@ -84,7 +97,7 @@ void DynamicBody::TimeStepUpdate(const float timeStep) {
     m_vel += timeStep * m_force * (1.0/m_mass);
     m_angVel += timeStep * m_torque * (1.0 / m_angInertia);
 
-    const vector3d pos = GetPosition();
+    vector3d pos = GetPosition();
     /* Applying angular velocity :/ */
     {
       double len = m_angVel.Length();
@@ -96,22 +109,25 @@ void DynamicBody::TimeStepUpdate(const float timeStep) {
       }
     }
 
-    SetPosition(pos + m_vel * timeStep);
+    pos += m_vel * timeStep;
+    m_orient[12] = pos.x;
+    m_orient[13] = pos.y;
+    m_orient[14] = pos.z;
+    TriMeshUpdateLastPos(m_orient);
+
     m_force = vector3d(0.0);
     m_torque = vector3d(0.0);
-
-    TriMeshUpdateLastPos(m_orient);
   }
 }
 
 void DynamicBody::Enable(void) {
   ModelBody::Enable();
-  //dBodyEnable(m_body);
+  m_enabled = true;
 }
 
 void DynamicBody::Disable(void) {
   ModelBody::Disable();
-  //dBodyDisable(m_body);
+  m_enabled = false;
 }
 
 void DynamicBody::SetRotMatrix(const matrix4x4d& r) {
diff --git a/src/model_body.cpp b/src/model_body.cpp
index 25a2254..d208ebe 100644
--- a/src/model_body.cpp
+++ b/src/model_body.cpp
@@ -98,7 +98,7 @@ void ModelBody::SetFrame(Frame* f) {
   }
 }
 
-void ModelBody::TriMeshUpdateLastPos(matrix4x4d currentTransform) {
+void ModelBody::TriMeshUpdateLastPos(const matrix4x4d& currentTransform) {
   m_geom->MoveTo(currentTransform);
 }
 
diff --git a/src/model_body.h b/src/model_body.h
index a3e95fc..0f3e853 100644
--- a/src/model_body.h
+++ b/src/model_body.h
@@ -26,7 +26,7 @@ public:
   virtual void Enable(void);
   void GetAabb(Aabb& aabb);
 
-  void TriMeshUpdateLastPos(matrix4x4d currentTransform);
+  void TriMeshUpdateLastPos(const matrix4x4d& currentTransform);
   void SetModel(int sbreModel);
 
   void RenderSbreModel(const Frame* camFrame, int model, ObjParams* params);
diff --git a/src/space.cpp b/src/space.cpp
index 9eebe0a..a1407fb 100644
--- a/src/space.cpp
+++ b/src/space.cpp
@@ -333,10 +333,11 @@ static bool _OnCollision2(Object* o1, Object* o2, CollisionContact* c) {
 }
 
 #define MAX_CONTACTS 1
+/* XXX SO REESTED!! 1 CONTACT PER PHYSICS TICK?! WTH?! */
 static int contact_num;
 static void hitCallback(CollisionContact* c) {
   if(contact_num++ >= MAX_CONTACTS) return;
-  printf("AUCH!! %x\n", SDL_GetTicks());
+  printf("AUCH!! %x (depth %f)\n", SDL_GetTicks(), c->depth);
 #if 0
   dContact contact;