From 7c6766e874cfbd69e007c389bb6783c16e847344 Mon Sep 17 00:00:00 2001
From: Cameron Reikes <cameronreikes@gmail.com>
Date: Wed, 1 Feb 2023 17:21:55 -0800
Subject: [PATCH] Scuffed momentum

---
 physics.jai | 73 ++++++++++++++++++++++++++++++++++++++++-------------
 1 file changed, 55 insertions(+), 18 deletions(-)

diff --git a/physics.jai b/physics.jai
index 260781e..f5ed0e2 100644
--- a/physics.jai
+++ b/physics.jai
@@ -94,6 +94,7 @@ Rect :: struct {
     halfsize: Vector2;
     pos  , vel      , force : Vector2; // pos is in world space
     angle, angle_vel, torque: float;
+    static: bool;
     mass: float = 1.0;
 };
 moment_of_inertia :: (using r: Rect) -> float {
@@ -106,6 +107,7 @@ apply_force_at_point :: (r: *Rect, force: Vector2, point_world_space: Vector2) {
 }
 // everything needed to resolve the collision
 Manifold :: struct {
+    a, b: *Rect;
     count: int;
     depths: [2] float;
     contact_points: [2] Vector2; // in absolute coordinates
@@ -119,9 +121,24 @@ perp :: (v: Vector2) -> Vector2
 {
 	return xy(-v.y, v.x);
 }
-handle_collision :: (m: Manifold, a: Rect, b: Rect) {
+handle_collision :: (m: Manifold, dt: float) {
+    a := m.a;
+    b := m.b;
     if m.count == 0 return;
-    k_scalar :: (a: Rectangle, b: Rectangle, r1: Vector2, r2: Vector2, n: Vector2)  {
+    for 0..m.count-1
+    {
+        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;
+
+        if a.static || b.static impulse_strength *= 2.0;
+
+        impulse := m.normal * impulse_strength;
+        apply_force_at_point(b, impulse / dt, m.contact_points[it]);
+        apply_force_at_point(a, -impulse / dt, m.contact_points[it]);
+    }
+    /*k_scalar :: (a: Rectangle, b: Rectangle, r1: Vector2, r2: Vector2, n: Vector2)  {
         k_scalar_rect :: (a: Rectangle, r: Vector2, n: Vector2)
         {
             rcn := cross(r, n);
@@ -134,7 +151,7 @@ handle_collision :: (m: Manifold, a: Rect, b: Rect) {
     }
     r1 := m.contact_points[0] - a.pos;
     r2 := m.contact_points[0] - b.pos;
-    nMass := 1.0 / k_scalar(a, b, r1, r2, 
+    nMass := 1.0 / k_scalar(a, b, r1, r2, */
     
 }
 MAX_POLYGON_VERTS :: 8;
@@ -232,7 +249,7 @@ check_faces :: (a: Polygon, b: Polygon) -> (separation: float, face_index: int)
     return sep, index;
 }
 
-rect_to_rect :: (a: Rect, b: Rect) -> Manifold {
+rect_to_rect :: (a: *Rect, b: *Rect) -> Manifold {
     to_return : Manifold;
 
     poly_a := poly_from(a);
@@ -361,6 +378,8 @@ rect_to_rect :: (a: Rect, b: Rect) -> Manifold {
     for 0..m.count-1 {
         push_pip(m.contact_points[it]);
     }
+    m.a = a;
+    m.b = b;
     return m;
 }
 dump_polygon :: (poly: Polygon) {
@@ -379,7 +398,7 @@ do_test :: () {
 #if true {
     print("A %\n", A_rect);
     print("B %\n", B_rect);
-    print("Collision %\n", rect_to_rect(A_rect, B_rect));
+    print("Collision %\n", rect_to_rect(*A_rect, *B_rect));
 }
     
 
@@ -488,7 +507,8 @@ main :: () {
     rects: [..]Rect;
     array_add(*rects, .{pos = #run xy(0.0,  0.0), halfsize = #run xy(0.3)});
     array_add(*rects, .{pos = #run xy(2.5, 0.0), halfsize = #run xy(0.3)});
-
+    array_add(*rects, .{pos = #run xy(-2.5, 0.0), halfsize = #run xy(0.3)});
+    array_add(*rects, .{pos = #run xy(0.0, -3.0), halfsize = #run xy(3.0, 0.2), static = true});
 
     quit := false;
     last_time := get_time();
@@ -536,26 +556,43 @@ main :: () {
                 {
                     apply_force_at_point(*rects[0], xy(3, 0), rects[0].pos + xy(-0.1, 0.03));
                 }
-                for * rects
-                {
-                    defer { it.force = .{}; it.torque = 0.0; };
-                    it.vel += (it.force/MASS) * TIMESTEP;
-                    it.pos += it.vel * TIMESTEP;
-
-                    from_rect := it;
+                collisions: [..] Manifold;
+                defer array_free(collisions);
+                for * from_rect: rects {
                     for * to_rect: rects {
                         if to_rect != from_rect
                         {
                             manifold_out := rect_to_rect(from_rect, to_rect);
                             if manifold_out.count > 0 {
-                                handle_collision(manifold_out, from_rect, to_rect);
-                                //from_rect.vel *= -1.0;
+                                unique := true;
+                                for collisions {
+                                    if (it.a == manifold_out.a && it.b == manifold_out.b) || (it.a == manifold_out.b && it.b == manifold_out.a) {
+                                        unique = false;
+                                        break;
+                                    }
+                                }
+                                if unique array_add(*collisions, manifold_out);
                             }
                         }
                     }
-
-                    it.angle_vel += (it.torque / moment_of_inertia(it)) * TIMESTEP;
-                    it.angle += it.angle_vel * TIMESTEP;
+                }
+                for collisions {
+                    handle_collision(it, TIMESTEP);
+                }
+                for * rects {
+                    defer it.force = .{};
+                    defer it.torque = 0.0;
+
+                    // gravity
+                    it.force.y += -9.81;
+
+                    if !it.static
+                    {
+                        it.vel += (it.force/it.mass) * TIMESTEP;
+                        it.pos += it.vel * TIMESTEP;
+                        it.angle_vel += (it.torque / moment_of_inertia(it)) * TIMESTEP;
+                        it.angle += it.angle_vel * TIMESTEP;
+                    }
                 }
             }
         }