From 33838c759059636b44d0a9fe4bc12bce2c1e2755 Mon Sep 17 00:00:00 2001
From: Rtch90 <ritchie.cunningham@protonmail.com>
Date: Tue, 10 Apr 2018 19:56:13 +0100
Subject: [PATCH] [Add] Angular effects on collision with static objects.

---
 src/space.cpp | 16 +++++++++++++---
 1 file changed, 13 insertions(+), 3 deletions(-)

diff --git a/src/space.cpp b/src/space.cpp
index c159742..7a098a3 100644
--- a/src/space.cpp
+++ b/src/space.cpp
@@ -379,15 +379,25 @@ static void hitCallback(CollisionContact* c) {
       hitNormal = c->normal;
     }
 
+    const double coeff_rest = 0.5;
     const vector3d vel = mover->GetVelocity();
     vector3d reflect = vel - (hitNormal * vector3d::Dot(vel, hitNormal) * 2.0f);
-    /* Dampen a little. */
-    reflect = reflect * 0.5;
 
     /* Step back. */
     mover->UndoTimestep();
     /* and set altered velocity. */
-    mover->SetVelocity(reflect);
+    mover->SetVelocity(reflect * coeff_rest);
+
+    /* Angular effects. */
+    const double invMass1 = 1.0 / mover->GetMass();
+
+    const vector3d hitPos1 = c->pos - mover->GetPosition();
+    const double j = (-(1+coeff_rest) * (vector3d::Dot(mover->GetVelocity(), c->normal))) /
+      (invMass1 +
+      (vector3d::Dot(c->normal, vector3d::Cross(vector3d::Cross(hitPos1, c->normal) * (1.0/mover->GetAngularInertia()), hitPos1)))
+      );
+
+    mover->SetAngVelocity(mover->GetAngVelocity() - vector3d::Cross(hitPos1, (j*c->normal))*(1.0/mover->GetAngularInertia()));
   }
 }