From f546c292db827cdb2e06e89259d709d8c0fcb339 Mon Sep 17 00:00:00 2001
From: Rtch90 <ritchie.cunningham@protonmail.com>
Date: Tue, 10 Apr 2018 19:48:34 +0100
Subject: [PATCH] [Add] Angular response for dynamic on dynamic collisions.

---
 src/dynamic_body.cpp | 18 +++++++++++++-----
 src/dynamic_body.h   |  4 ++++
 src/space.cpp        | 16 +++++++++++-----
 3 files changed, 28 insertions(+), 10 deletions(-)

diff --git a/src/dynamic_body.cpp b/src/dynamic_body.cpp
index 063609e..7906396 100644
--- a/src/dynamic_body.cpp
+++ b/src/dynamic_body.cpp
@@ -6,8 +6,8 @@
 
 DynamicBody::DynamicBody(void) : ModelBody() {
   m_flags = Body::FLAG_CAN_MOVE_FRAME;
-
   m_orient      = matrix4x4d::Identity();
+  m_oldOrient   = m_orient;
   m_force       = vector3d(0.0);
   m_torque      = vector3d(0.0);
   m_vel         = vector3d(0.0);
@@ -26,6 +26,10 @@ void DynamicBody::AddForce(const vector3d f) {
   m_force += f;
 }
 
+void DynamicBody::AddTorque(const vector3d t) {
+  m_torque += t;
+}
+
 void DynamicBody::AddForceAtPos(const vector3d force, const vector3d pos) {
   assert(0);
   /*
@@ -60,6 +64,7 @@ void DynamicBody::Load(void) {
   using namespace Serializer::Read;
   ModelBody::Load();
   for(int i = 0; i < 16; i++) m_orient[i] = rd_double();
+  m_oldOrient   = m_orient;
   m_force       = rd_vector3d();
   m_torque      = rd_vector3d();
   m_vel         = rd_vector3d();
@@ -93,11 +98,8 @@ vector3d DynamicBody::GetPosition(void) const {
 }
 
 void DynamicBody::TimeStepUpdate(const float timeStep) {
-  /*
-   * XXX Remember this is used to step back also in the collision code.
-   * very silly btw.
-   */
   if(m_enabled) {
+    m_oldOrient = m_orient;
     m_vel += timeStep * m_force * (1.0/m_mass);
     m_angVel += timeStep * m_torque * (1.0 / m_angInertia);
 
@@ -124,6 +126,12 @@ void DynamicBody::TimeStepUpdate(const float timeStep) {
   }
 }
 
+void DynamicBody::UndoTimestep(void) {
+  m_orient = m_oldOrient;
+  TriMeshUpdateLastPos(m_orient);
+  TriMeshUpdateLastPos(m_orient);
+}
+
 void DynamicBody::Enable(void) {
   ModelBody::Enable();
   m_enabled = true;
diff --git a/src/dynamic_body.h b/src/dynamic_body.h
index 3642280..f3a6aee 100644
--- a/src/dynamic_body.h
+++ b/src/dynamic_body.h
@@ -20,6 +20,7 @@ public:
   void SetMesh(ObjMesh* m);
   virtual bool OnCollision(Body* b, Uint32 flags) { return true; }
   vector3d GetAngularMomentum(void);
+  double GetAngularInertia(void) const { return m_angInertia; }
   void SetMassDistributionFromCollMesh(const CollMesh* m);
   void DisableBodyOnly(void) { m_enabled = false; }
   bool IsEnabled(void) { return m_enabled; }
@@ -27,10 +28,12 @@ public:
   virtual void Enable(void);
   virtual double GetMass(void) const { return m_mass; }
   virtual void TimeStepUpdate(const float timeStep);
+  void UndoTimestep(void);
 
   void SetMass(double);
   void AddForceAtPos(const vector3d force, const vector3d pos);
   void AddForce(const vector3d);
+  void AddTorque(const vector3d);
   void SetForce(const vector3d);
   void SetTorque(const vector3d);
   /* body-relative forces. */
@@ -44,6 +47,7 @@ protected:
 private:
   /* New odeless stuff. */
   matrix4x4d m_orient; /* Contains pos. */
+  matrix4x4d m_oldOrient;
   vector3d m_force;
   vector3d m_torque;
   vector3d m_vel;
diff --git a/src/space.cpp b/src/space.cpp
index a0991c9..c159742 100644
--- a/src/space.cpp
+++ b/src/space.cpp
@@ -342,6 +342,8 @@ static void hitCallback(CollisionContact* c) {
   if(po1_isDynBody && po2_isDynBody) {
     DynamicBody* b1 = static_cast<DynamicBody*>(po1);
     DynamicBody* b2 = static_cast<DynamicBody*>(po2);
+    vector3d hitPos1 = c->pos - b1->GetPosition();
+    vector3d hitPos2 = c->pos - b2->GetPosition();
     vector3d vel1 = b1->GetVelocity();
     vector3d vel2 = b2->GetVelocity();
     const vector3d relVel = vel2 - vel1;
@@ -350,15 +352,19 @@ static void hitCallback(CollisionContact* c) {
 
     const double coeff_rest = 0.8;
     const double j = (-(1+coeff_rest) * (vector3d::Dot(relVel, c->normal))) /
-    (vector3d::Dot(c->normal, c->normal) *
-     (invMass1 + invMass2));
+      ( (invMass1 + invMass2) +
+      (vector3d::Dot(c->normal, vector3d::Cross(vector3d::Cross(hitPos1, c->normal) * (1.0/b1->GetAngularInertia()), hitPos1))) +
+      (vector3d::Dot(c->normal, vector3d::Cross(vector3d::Cross(hitPos2, c->normal) * (1.0/b2->GetAngularInertia()), hitPos2)))
+      );
 
     /* Step back. */
-    b1->TimeStepUpdate(-L3D::GetTimeStep());
-    b2->TimeStepUpdate(-L3D::GetTimeStep());
+    //b1->UndoTimestep();
+    //b2->UndoTimestep();
     /* Apply impulse. */
     b1->SetVelocity(vel1 - (j*c->normal)*invMass1);
     b2->SetVelocity(vel2 + (j*c->normal)*invMass2);
+    b1->SetAngVelocity(b1->GetAngVelocity() - vector3d::Cross(hitPos1, (j*c->normal))*(1.0/b1->GetAngularInertia()));
+    b2->SetAngVelocity(b2->GetAngVelocity() + vector3d::Cross(hitPos2, (j*c->normal))*(1.0/b2->GetAngularInertia()));
   } else {
     /* One body is static. */
     vector3d hitNormal;
@@ -379,7 +385,7 @@ static void hitCallback(CollisionContact* c) {
     reflect = reflect * 0.5;
 
     /* Step back. */
-    mover->TimeStepUpdate(-L3D::GetTimeStep());
+    mover->UndoTimestep();
     /* and set altered velocity. */
     mover->SetVelocity(reflect);
   }