From d305487e8fe4e19c8e5ee226b783ef56b1114433 Mon Sep 17 00:00:00 2001
From: Rtch90 <ritchie.cunningham@protonmail.com>
Date: Sun, 8 Apr 2018 18:59:33 +0100
Subject: [PATCH] [Add] Linear collision response between dynamic bodies.

---
 src/main.cpp  | 26 ++++++++++++++------------
 src/space.cpp | 33 +++++++++++++++++++++++++++------
 2 files changed, 41 insertions(+), 18 deletions(-)

diff --git a/src/main.cpp b/src/main.cpp
index 4e5a609..3d0ae2a 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -166,18 +166,20 @@ void L3D::HandleEvents(void) {
       if(event.key.keysym.sym == SDLK_i) L3D::showDebugInfo = !L3D::showDebugInfo;
 #ifdef DEBUG
       if(event.key.keysym.sym == SDLK_F12) {
-        /* Add test object. */
-        /*Ship* body = new Ship(ShipType::LADYBIRD);
-        body->SetLabel("A friend");
-        body->SetFrame(L3D::player->GetFrame());
-        body->SetPosition(L3D::player->GetPosition()+vector3d(0,0,-1000));
-        Space::AddBody(body);*/
-        SpaceStation* station = new SpaceStation(SpaceStation::JJHOOP);
-        station->SetLabel("Poemi-chan's Folly");
-        station->SetFrame(L3D::player->GetFrame());
-        station->SetRotMatrix(matrix4x4d::RotateZMatrix(M_PI));
-        station->SetPosition(L3D::player->GetPosition() + vector3d(0, 0, -5000));
-        Space::AddBody(station);
+        if(KeyState(SDLK_LSHIFT)) {
+          SpaceStation* station = new SpaceStation(SpaceStation::JJHOOP);
+          station->SetLabel("Poemi-chan's Folly");
+          station->SetFrame(L3D::player->GetFrame());
+          station->SetRotMatrix(matrix4x4d::RotateZMatrix(M_PI));
+          station->SetPosition(L3D::player->GetPosition()+vector3d(0,0,-5000));
+          Space::AddBody(station);
+        } else {
+          Ship* body = new Ship(ShipType::LADYBIRD);
+          body->SetLabel("A Friend");
+          body->SetFrame(L3D::player->GetFrame());
+          body->SetPosition(L3D::player->GetPosition()+vector3d(0,0,-1000));
+          Space::AddBody(body);
+        }
       }
 #endif
       if(event.key.keysym.sym == SDLK_F11) SDL_WM_ToggleFullScreen(L3D::scrSurface);
diff --git a/src/space.cpp b/src/space.cpp
index 8d479b4..a0991c9 100644
--- a/src/space.cpp
+++ b/src/space.cpp
@@ -327,7 +327,7 @@ static bool _OnCollision2(Object* o1, Object* o2, CollisionContact* c) {
 }
 
 static void hitCallback(CollisionContact* c) {
-  printf("AUCH!! %x (depth %f)\n", SDL_GetTicks(), c->depth);
+  //printf("AUCH!! %x (depth %f)\n", SDL_GetTicks(), c->depth);
 
   Object* po1 = static_cast<Object*>(c->userData1);
   Object* po2 = static_cast<Object*>(c->userData2);
@@ -340,17 +340,38 @@ static void hitCallback(CollisionContact* c) {
   assert(po1_isDynBody || po2_isDynBody);
 
   if(po1_isDynBody && po2_isDynBody) {
+    DynamicBody* b1 = static_cast<DynamicBody*>(po1);
+    DynamicBody* b2 = static_cast<DynamicBody*>(po2);
+    vector3d vel1 = b1->GetVelocity();
+    vector3d vel2 = b2->GetVelocity();
+    const vector3d relVel = vel2 - vel1;
+    const double invMass1 = 1.0 / b1->GetMass();
+    const double invMass2 = 1.0 / b2->GetMass();
 
+    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));
+
+    /* Step back. */
+    b1->TimeStepUpdate(-L3D::GetTimeStep());
+    b2->TimeStepUpdate(-L3D::GetTimeStep());
+    /* Apply impulse. */
+    b1->SetVelocity(vel1 - (j*c->normal)*invMass1);
+    b2->SetVelocity(vel2 + (j*c->normal)*invMass2);
   } else {
     /* One body is static. */
     vector3d hitNormal;
-    if(po2_isDynBody) hitNormal = -c->normal;
-    else hitNormal = c->normal;
-    printf("Hi! %f,%f,%f\n", hitNormal.x, hitNormal.y, hitNormal.z);
 
     DynamicBody* mover;
-    if(po1_isDynBody) mover = static_cast<DynamicBody*>(po1);
-    else  mover = static_cast<DynamicBody*>(po2);
+
+    if(po1_isDynBody) {
+      mover = static_cast<DynamicBody*>(po1);
+      hitNormal = c->normal;
+    } else {
+      mover = static_cast<DynamicBody*>(po2);
+      hitNormal = c->normal;
+    }
 
     const vector3d vel = mover->GetVelocity();
     vector3d reflect = vel - (hitNormal * vector3d::Dot(vel, hitNormal) * 2.0f);