From 8c36552659b26ddef8d28e623b17020278595dd3 Mon Sep 17 00:00:00 2001
From: jacob <jacob@cagori.com>
Date: Tue, 3 Sep 2024 17:00:40 -0500
Subject: [PATCH] collision resolution testing (janky bc angular velocity not
 taken into account)

---
 src/game.c | 34 +++++++++++++++++++++++++++-------
 src/gjk.c  |  6 +++---
 2 files changed, 30 insertions(+), 10 deletions(-)

diff --git a/src/game.c b/src/game.c
index f349b764..4804b02e 100644
--- a/src/game.c
+++ b/src/game.c
@@ -133,7 +133,7 @@ INTERNAL void spawn_test_entities(void)
     {
         struct v2 pos = V2(-1, -1);
         struct v2 size = V2(1, 1);
-        f32 r = 0;
+        f32 r = PI / 4;
         f32 skew = 0;
 
         struct entity *e = entity_alloc(root);
@@ -613,7 +613,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
             /* Ground friction */
             if (!v2_eq(velocity, V2(0, 0))) {
                 /* FIXME: Incorrect behavior at low FPS & low entity density */
-                const f32 clamp_epsilon = 0.001;
+                const f32 clamp_epsilon = 0.01;
                 f32 velocity_len = v2_len(velocity);
                 if (velocity_len >= clamp_epsilon) {
                     f32 force_len = -velocity_len * ent->ground_friction;
@@ -799,8 +799,15 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
                 };
             }
 
+#if 1
+            struct v2 e0_velocity = v2_div(v2_sub(e0_xf.og, e0->verlet_xform.og), dt);
+            struct v2 e1_velocity = v2_div(v2_sub(e1_xf.og, e1->verlet_xform.og), dt);
+            struct v2 pendir = v2_sub(e0_velocity, e1_velocity);  /* What if both point in opposite directions? */
+            pendir = v2_norm(pendir);
+#else
             struct v2 pendir = V2(0, 0);
             //struct v2 pendir = V2(0, 99999);
+#endif
 
             struct gjk_extended_result res = gjk_extended(e0_poly, e1_poly, pendir);
 
@@ -811,16 +818,29 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
             //final_simplex = res.simplex;
 
             if (colliding) {
+                struct v2 pen = v2_sub(point1, point0);
 
-                //pen = epa_colliding_pen(e0_poly, e1_poly, simplex);
 #if 0
-            /* Pen movement test */
+                /* Pen movement test */
+                f32 e0_ratio = v2_dot(pendir, e0_velocity);
+                //f32 e1_ratio = v2_dot(pendir, e1_velocity);
+                f32 e1_ratio = v2_dot(pendir, e1_velocity);
                 {
-                    struct xform xf = e1_xf;
+                    struct v2 pen = v2_mul(e0_velocity, e0_ratio);
+
+                    struct xform xf = e0_xf;
                     xf.og = v2_add(xf.og, pen);
-                    entity_set_xform(e1, xf);
-                    //e1->verlet_xform.og = v2_add(e1->verlet_xform.og, pen);
+                    entity_set_xform(e0, xf);
+
+                    //e0->verlet_xform.og = v2_add(e0->verlet_xform.og, pen);
                 }
+#else
                f32 epsilon = 0.0001;
                pen = v2_add(pen, v2_mul(v2_norm(pen), epsilon));
+
+                struct xform xf = e0_xf;
+                xf.og = v2_add(xf.og, pen);
+                entity_set_xform(e0, xf);
+                e0->verlet_xform.og = v2_add(e0->verlet_xform.og, pen);
+                //e0->verlet_xform.og = xf.og;
 #endif
             }
         }
diff --git a/src/gjk.c b/src/gjk.c
index 6c146c13..1970ab17 100644
--- a/src/gjk.c
+++ b/src/gjk.c
@@ -123,7 +123,7 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array
                 /* Third point is support point in direction of line normal towards origin */
                 dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p));
                 m = menkowski_point_extended(shape0, shape1, dir);
-                if (v2_dot(dir, m.p) < 0) {
+                if (v2_dot(dir, m.p) <= 0) {
                     /* New point did not cross origin, collision impossible */
                     break;
                 }
@@ -310,7 +310,7 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array
             /* Second point is support point towards origin */
             if (s.len == 1) {
                 m = menkowski_point_extended(shape0, shape1, v2_neg(s.a.p));
-                if (v2_eq(m.p, s.a.p)) {
+                if (v2_eq(m.p, s.a.p) || v2_eq(m.p, V2(0, 0))) {
                     break;
                 }
 
@@ -324,7 +324,7 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array
 
             if (s.len == 2) {
                 m = menkowski_point_extended(shape0, shape1, dir);
-                if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_sub(m.p, s.a.p))) < unique_epsilon) {
+                if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_sub(m.p, s.a.p))) < unique_epsilon || v2_eq(m.p, V2(0, 0))) {
                     /* New point is already on the current line */
                     break;
                 }