From 027789bcd8f9072b5d3f462980d76ae4501ddc5f Mon Sep 17 00:00:00 2001
From: Cameron Reikes <cameronreikes@gmail.com>
Date: Thu, 2 Feb 2023 22:45:43 -0800
Subject: [PATCH] Equation from wikipedia, doesn't make sense in 2d

---
 physics.jai | 27 +++++++++++++++++++--------
 1 file changed, 19 insertions(+), 8 deletions(-)

diff --git a/physics.jai b/physics.jai
index f7cb9b7..f8f7827 100644
--- a/physics.jai
+++ b/physics.jai
@@ -6,6 +6,7 @@ Input :: #import "Input";
 Simp :: #import "Simp";
 
 TIMESTEP: float = 1.0 / 60.0;
+DEBUGGING :: true;
 window_width  : s32 = 1280;
 window_height : s32 = 720;
 
@@ -112,18 +113,19 @@ apply_force_at_point :: (r: *Rect, force: Vector2, point_world_space: Vector2) {
     r.torque += offset_from_center_of_mass.x * force.y - offset_from_center_of_mass.y * force.x;
 }
 apply_impulse_at_point :: (using r: *Rect, impulse: Vector2, point_world_space: Vector2) {
-    #if false
-    {
-    apply_force_at_point(r, impulse / TIMESTEP, point_world_space);
-} else {
     if !static
     {
-        vel += impulse / r.mass;
-        angle_vel += length(impulse) * (cross(point_world_space - pos, normalize_or_zero(impulse))/moment_of_inertia(r));
-        if isnan(angle_vel) Debug.breakpoint();
+        vel += impulse / mass;
+        offset_from_center_of_mass := point_world_space - pos;
+        angle_vel += cross(offset_from_center_of_mass, impulse) / moment_of_inertia(r);
+        #if DEBUGGING { if isnan(angle_vel) Debug.breakpoint(); }
     }
 }
+velocity_of_point :: (using r: *Rect, point: Vector2) -> Vector2 #must {
+    r: Vector2 = point - pos;
+    return vel + perp(r) * angle_vel;
 }
+
 // everything needed to resolve the collision
 Manifold :: struct {
     a, b: *Rect;
@@ -148,7 +150,16 @@ handle_collision :: (m: Manifold, dt: float) {
         total_momentum: float = length(a.vel) * a.mass + length(b.vel) * b.mass;
         impulse_strength: float = 0.0;
         //impulse_strength += m.depths[it] * 3.0;
-        impulse_strength += total_momentum / 2.0;
+        //impulse_strength += total_momentum / 2.0;
+        restitution: float = 0.5;
+        v_point_a := velocity_of_point(a, m.contact_points[it]);
+        v_point_b := velocity_of_point(b, m.contact_points[it]);
+        vr := v_point_b - v_point_a;
+        r1 := m.contact_points[it] - a.pos;
+        r2 := m.contact_points[it] - b.pos;
+        I1 := moment_of_inertia(a);
+        I2 := moment_of_inertia(b);
+        impulse_strength += (-(1.0 + restitution) * dot(vr, m.normal)) / ( 1.0/a.mass + 1.0/b.mass + dot(cross(cross(r1, m.normal)/I1, r1) + cross(cross(r2, m.normal), r2)/I2, m.normal));
 
         // the momentum would be unused if put into static
         if a.static || b.static impulse_strength *= 2.0;