From beccc17b7fa18315c05fcfe9b8d5b535f02cd0d4 Mon Sep 17 00:00:00 2001
From: jacob <jacob@cagori.com>
Date: Tue, 3 Sep 2024 12:04:22 -0500
Subject: [PATCH] working gjk penetration direction test

---
 src/game.c |   2 +-
 src/math.h |  12 +-
 src/util.c | 366 +++++++++++++++++++++++++++++++++++++++--------------
 src/util.h |   4 +-
 4 files changed, 283 insertions(+), 101 deletions(-)

diff --git a/src/game.c b/src/game.c
index 800084b1..2f24daa8 100644
--- a/src/game.c
+++ b/src/game.c
@@ -451,7 +451,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
             }
 
             /* Movement */
-            if (v2_len_squared(move) > 1) {
+            if (v2_len_sq(move) > 1) {
                 /* Cap movement vector magnitude at 1 */
                 move = v2_norm(move);
             }
diff --git a/src/math.h b/src/math.h
index 175edb75..477e3eb9 100644
--- a/src/math.h
+++ b/src/math.h
@@ -630,7 +630,7 @@ INLINE f32 v2_len(struct v2 a)
     return math_sqrt(a.x * a.x + a.y * a.y);
 }
 
-INLINE f32 v2_len_squared(struct v2 a)
+INLINE f32 v2_len_sq(struct v2 a)
 {
     return a.x * a.x + a.y * a.y;
 }
@@ -649,7 +649,7 @@ INLINE struct v2 v2_perp_ccw(struct v2 a)
 
 INLINE struct v2 v2_norm(struct v2 a)
 {
-    f32 l = v2_len_squared(a);
+    f32 l = v2_len_sq(a);
     if (l != 0) {
         f32 denom = 1.f / math_sqrt(l);
         a.x *= denom;
@@ -1011,10 +1011,10 @@ INLINE struct v2 xform_invert_mul_v2(struct xform xf, struct v2 v)
 INLINE struct quad xform_mul_quad(struct xform xf, struct quad quad)
 {
     return (struct quad) {
-            xform_mul_v2(xf, quad.p1),
-            xform_mul_v2(xf, quad.p2),
-            xform_mul_v2(xf, quad.p3),
-            xform_mul_v2(xf, quad.p4)
+        xform_mul_v2(xf, quad.p1),
+        xform_mul_v2(xf, quad.p2),
+        xform_mul_v2(xf, quad.p3),
+        xform_mul_v2(xf, quad.p4)
     };
 }
 
diff --git a/src/util.c b/src/util.c
index bac0ec64..e3274037 100644
--- a/src/util.c
+++ b/src/util.c
@@ -45,31 +45,6 @@ struct string util_file_name_from_path(struct string path)
 
 /* TODO: Remove / move this */
 
-struct v2 poly_support(struct v2_array a, struct v2 dir)
-{
-    /* TODO: Could probably binary search for largest dot since shape is convex */
-    struct v2 furthest = a.points[0];
-    f32 furthest_dot = v2_dot(dir, furthest);
-    for (u32 i = 1; i < a.count; ++i) {
-        struct v2 p = a.points[i];
-        f32 dot = v2_dot(dir, p);
-        if (dot > furthest_dot) {
-            furthest = p;
-            furthest_dot = dot;
-        }
-    }
-    return furthest;
-}
-
-INTERNAL struct gjk_menkowski_point menkowski_point(struct v2_array poly0, struct v2_array poly1, struct v2 dir)
-{
-    struct gjk_menkowski_point res;
-    res.p0 = poly_support(poly0, dir);
-    res.p1 = poly_support(poly1, v2_neg(dir));
-    res.p = v2_sub(res.p0, res.p1);
-    return res;
-}
-
 struct v2 perp_towards_point(struct v2 start, struct v2 end, struct v2 p)
 {
     struct v2 perp = v2_perp_cw(v2_sub(end, start));
@@ -77,21 +52,11 @@ struct v2 perp_towards_point(struct v2 start, struct v2 end, struct v2 p)
     return v2_mul(perp, sign);
 }
 
-/* TODO: Remove this (debugging) */
-struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1)
+struct v2 perp_towards_dir(struct v2 start, struct v2 end, struct v2 dir)
 {
-    struct v2_array res = { .points = arena_dry_push(arena, struct v2) };
-    u64 rays = 500;
-    for (u64 i = 0; i < rays; ++i) {
-        f32 angle = ((f32)i / rays) * (2 * PI);
-        struct v2 dir = v2_from_angle(angle);
-        struct v2 p = menkowski_point(poly0, poly1, dir).p;
-        if (res.count == 0 || !v2_eq(p, res.points[res.count - 1])) {
-            *arena_push(arena, struct v2) = p;
-            ++res.count;
-        }
-    }
-    return res;
+    struct v2 perp = v2_perp_cw(v2_sub(end, start));
+    i32 sign = 1 - ((v2_dot(perp, v2_add(dir, start)) < 0) << 1);
+    return v2_mul(perp, sign);
 }
 
 /* Returns 1 if winding forward, -1 if backward */
@@ -119,40 +84,55 @@ i32 poly_get_winding_order(struct v2_array poly)
 
 
 
-b32 gjk_boolean(struct v2_array poly0, struct v2_array poly1, u32 max_steps)
+INTERNAL struct v2 poly_support(struct v2_array a, struct v2 dir)
 {
-    b32 colliding = false;
-    u32 step = 0;
-
-    /* Simplex */
-    struct { struct v2 a, b, c; } s = { 0 };
-
-    /* First point is support point towards shape centers */
-    if (step++ >= max_steps) goto abort;
-    s.a = menkowski_point(poly0, poly1, v2_sub(poly1.points[0], poly0.points[0])).p;
-
-    /* Second point is support point towards origin */
-    if (step++ >= max_steps) goto abort;
-    b32 valid_line = false;
-    {
-        struct v2 dir = v2_neg(s.a);
-        struct v2 p = menkowski_point(poly0, poly1, dir).p;
-        if (v2_dot(dir, p) >= 0) {
-            s.b = s.a;
-            s.a = p;
-            valid_line = true;
+    /* TODO: Could probably binary search for largest dot since shape is convex */
+    struct v2 furthest = a.points[0];
+    f32 furthest_dot = v2_dot(dir, furthest);
+    for (u32 i = 1; i < a.count; ++i) {
+        struct v2 p = a.points[i];
+        f32 dot = v2_dot(dir, p);
+        if (dot > furthest_dot) {
+            furthest = p;
+            furthest_dot = dot;
         }
     }
+    return furthest;
+}
 
-    if (valid_line) {
+INTERNAL struct v2 menkowski_point(struct v2_array poly0, struct v2_array poly1, struct v2 dir)
+{
+    return v2_sub(poly_support(poly0, dir), poly_support(poly1, v2_neg(dir)));
+}
+
+INTERNAL struct gjk_menkowski_point extended_menkowski_point(struct v2_array poly0, struct v2_array poly1, struct v2 dir)
+{
+    struct gjk_menkowski_point res;
+    res.p0 = poly_support(poly0, dir);
+    res.p1 = poly_support(poly1, v2_neg(dir));
+    res.p = v2_sub(res.p0, res.p1);
+    return res;
+}
+
+b32 gjk_boolean(struct v2_array poly0, struct v2_array poly1)
+{
+    struct { struct v2 a, b, c; } s = { 0 };
+
+    /* First point is support point in shape's general directions to eachother */
+    s.a = menkowski_point(poly0, poly1, v2_sub(poly1.points[0], poly0.points[0]));
+
+    /* Second point is support point towards origin */
+    struct v2 dir = v2_neg(s.a);
+    struct v2 p = menkowski_point(poly0, poly1, dir);
+    if (v2_dot(dir, p) >= 0) {
+        s.b = s.a;
+        s.a = p;
         while (true) {
-            if (step++ >= max_steps) goto abort;
-
             /* Third point is support point in direction of line normal towards origin */
-            struct v2 dir = perp_towards_point(s.a, s.b, V2(0, 0));
-            struct v2 p = menkowski_point(poly0, poly1, dir).p;
+            dir = perp_towards_point(s.a, s.b, V2(0, 0));
+            p = menkowski_point(poly0, poly1, dir);
             if (v2_dot(dir, p) < 0) {
-                colliding = false;
+                /* New point did not cross origin, collision impossible */
                 break;
             }
 
@@ -160,29 +140,39 @@ b32 gjk_boolean(struct v2_array poly0, struct v2_array poly1, u32 max_steps)
             s.b = s.a;
             s.a = p;
 
-            dir = v2_neg(perp_towards_point(s.a, s.b, s.c));  /* Normal dir of ab pointing away from c */
+            dir = v2_neg(perp_towards_point(s.a, s.b, s.c));  /* Normal of ab pointing away from c */
             struct v2 a_to_origin = v2_neg(s.a);
             if (v2_dot(dir, a_to_origin) >= 0) {
                 /* Point is in region ab, remove c from simplex (will happen automatically next iteration) */
             } else {
                 /* Point is not in region ab */
-                dir = v2_neg(perp_towards_point(s.a, s.c, s.b));  /* Normal dir of ac pointing away from b */
+                dir = v2_neg(perp_towards_point(s.a, s.c, s.b));  /* Normal of ac pointing away from b */
                 if (v2_dot(dir, a_to_origin) >= 0) {
                     /* Point is in region ac, remove b from simplex */
                     s.b = s.c;
                 } else {
                     /* Point is in simplex */
-                    colliding = true;
-                    break;
+                    return true;
                 }
             }
         }
     }
 
-    abort:
-    return colliding;
+    return false;
 }
 
+
+
+
+
+
+
+
+
+
+
+#if 0
+
 struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array poly1, u32 max_steps)
 {
     struct temp_arena scratch = scratch_begin_no_conflict();
@@ -200,7 +190,7 @@ struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array p
     /* First point is support point towards shape centers */
     if (step++ >= max_steps) goto abort;
     struct v2 dir = v2_sub(poly1.points[0], poly0.points[0]);
-    struct gjk_menkowski_point m = menkowski_point(poly0, poly1, dir);
+    struct gjk_menkowski_point m = extended_menkowski_point(poly0, poly1, dir);
     s.a = m;
     s.len = 1;
 
@@ -208,7 +198,7 @@ struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array p
         /* Second point is support point towards origin */
         if (s.len == 1) {
             if (step++ >= max_steps) goto abort;
-            m = menkowski_point(poly0, poly1, v2_neg(s.a.p));
+            m = extended_menkowski_point(poly0, poly1, v2_neg(s.a.p));
             if (v2_eq(m.p, s.a.p)) {
                 break;
             }
@@ -222,7 +212,7 @@ struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array p
         }
 
         if (step++ >= max_steps) goto abort;
-        m = menkowski_point(poly0, poly1, dir);
+        m = extended_menkowski_point(poly0, poly1, dir);
 
         if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_sub(m.p, s.a.p))) < unique_epsilon) {
             /* New point is already on the current line */
@@ -292,14 +282,12 @@ struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array p
         {
             struct gjk_menkowski_point *tmp = arena_push_array(scratch.arena, struct gjk_menkowski_point, 3);
             proto_count = s.len;
-            if (proto_count >= 1) {
-                tmp[0] = s.a;
-            }
+            tmp[0] = s.a;
             if (proto_count >= 2) {
                 tmp[1] = s.b;
-            }
-            if (proto_count >= 3) {
-                tmp[2] = s.c;
+                if (proto_count >= 3) {
+                    tmp[2] = s.c;
+                }
             }
         }
 
@@ -307,11 +295,12 @@ struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array p
             if (step++ >= max_steps) goto abort;
 
             struct v2 pen = V2(0, 0);
-            f32 pen_len = F32_INFINITY;
+            f32 pen_len_sq = F32_INFINITY;
 
             /* Find dir from origin to closest edge */
+            /* FIXME: Winding order of ps & pe index */
             u32 pen_ps_index = 0;
-            u32 pen_pe_index = 1;
+            u32 pen_pe_index = 0;
             for (u32 i = 0; i < proto_count; ++i) {
                 u32 ps_index = i;
                 u32 pe_index = (i < proto_count - 1) ? (i + 1) : 0;
@@ -324,18 +313,22 @@ struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array p
                 struct v2 vsd = v2_mul(vse, (v2_dot(vso, vse) / v2_dot(vse, vse)));
                 struct v2 pd = v2_add(ps, vsd);
 
-                /* TODO: sq cmp */
-                f32 pd_len = v2_len(pd);
-                if (pd_len < pen_len) {
+                f32 pd_len_sq = v2_len_sq(pd);
+                if (pd_len_sq < pen_len_sq) {
                     pen_ps_index = ps_index;
                     pen_pe_index = pe_index;
                     pen = pd;
-                    pen_len = pd_len;
+                    pen_len_sq = pd_len_sq;
                 }
             }
 
             /* Find new point in dir */
-            m = menkowski_point(poly0, poly1, pen);
+            m = extended_menkowski_point(poly0, poly1, pen);
+
+            /* TODO: Move to break */
+            s.a = proto[pen_ps_index];
+            s.b = proto[pen_pe_index];
+            s.len = 2;
 
             /* Check unique */
             /* TODO: Better */
@@ -350,14 +343,11 @@ struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array p
                     }
                 }
                 if (!unique) {
-                    s.a = proto[pen_ps_index];
-                    s.b = proto[pen_pe_index];
-                    s.len = 2;
                     break;
                 }
             }
 
-            /* Insert point into prototype array */
+            /* Insert point into prototype */
             /* FIXME: Preserve winding order */
             arena_push(scratch.arena, struct v2);
             ++proto_count;
@@ -403,3 +393,195 @@ abort:
     scratch_end(scratch);
     return res;
 }
+
+#else
+
+INTERNAL b32 ray_intersects_line(struct v2 p0, struct v2 p1, struct v2 ray_dir)
+{
+    return (v2_dot(ray_dir, p0) >= 0 || v2_dot(ray_dir, p1) >= 0) && math_fsign(v2_wedge(ray_dir, p0)) != math_fsign(v2_wedge(ray_dir, p1));
+}
+
+struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array poly1, u32 max_steps)
+{
+    struct v2 movedir = V2(0, 999999);
+
+
+
+    struct temp_arena scratch = scratch_begin_no_conflict();
+    b32 colliding = false;
+    struct v2 shape0_p = V2(0, 0);
+    struct v2 shape1_p = V2(0, 0);
+    u32 step = 0;
+
+    /* TODO: Verify epsilon */
+    f32 unique_epsilon = 0.00001;
+
+    /* Simplex */
+    struct gjk_extended_simplex s = { 0 };
+
+    /* First point is support point towards shape centers */
+    if (step++ >= max_steps) goto abort;
+    struct v2 dir = v2_sub(poly1.points[0], poly0.points[0]);
+    struct gjk_menkowski_point m = extended_menkowski_point(poly0, poly1, dir);
+    s.a = m;
+    s.len = 1;
+
+    while (true) {
+        /* Second point is support point towards movedir */
+        if (s.len == 1) {
+            if (step++ >= max_steps) goto abort;
+#if 0
+            m = extended_menkowski_point(poly0, poly1, movedir);
+            if (v2_eq(m.p, s.a.p)) {
+                break;
+            }
+#else
+            dir = v2_sub(v2_mul(movedir, v2_dot(movedir, s.a.p) / v2_dot(movedir, movedir)), s.a.p);
+            m = extended_menkowski_point(poly0, poly1, dir);
+            if (v2_eq(m.p, s.a.p)) {
+                break;
+            }
+#endif
+
+            s.b = s.a;
+            s.a = m;
+            s.len = 2;
+
+            /* Third point is support point in direction of line normal towards `a` projected onto movedir */
+            dir = perp_towards_dir(s.a.p, s.b.p, movedir);
+        }
+
+        if (step++ >= max_steps) goto abort;
+        m = extended_menkowski_point(poly0, poly1, dir);
+
+        if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_sub(m.p, s.a.p))) < unique_epsilon) {
+            /* New point is already on the current line */
+            break;
+        }
+
+        s.c = s.b;
+        s.b = s.a;
+        s.a = m;
+        s.len = 3;
+
+        if (step++ >= max_steps) goto abort;
+
+        /* TODO: Can group dots & wedges from intersect checks */
+        b32 abi = ray_intersects_line(s.a.p, s.b.p, movedir);
+        b32 aci = ray_intersects_line(s.a.p, s.c.p, movedir);
+        b32 bci = ray_intersects_line(s.b.p, s.c.p, movedir);
+
+
+        struct v2 ab_dir = perp_towards_dir(s.a.p, s.b.p, movedir);
+        struct v2 ac_dir = perp_towards_dir(s.a.p, s.c.p, movedir);
+        struct v2 bc_dir = perp_towards_dir(s.b.p, s.c.p, movedir);
+
+        if (abi) {
+            /* Remove c */
+            dir = ab_dir;
+            s.len = 2;
+        } else if (aci) {
+            /* Remove b */
+            dir = ac_dir;
+            s.b = s.c;
+            s.len = 2;
+        } else if (bci) {
+            /* Remove a */
+            dir = bc_dir;
+            s.a = s.b;
+            s.b = s.c;
+            s.len = 2;
+        }
+    }
+
+#if 0
+    {
+        if (s.len == 1) {
+            shape0_p = s.a.p0;
+            shape1_p = s.a.p1;
+        } else if (s.len == 2) {
+            /* Determine ratio between edge a & b that projected origin lies */
+            /* FIXME: Winding order dependent? */
+            struct v2 vab = v2_sub(s.b.p, s.a.p);
+            struct v2 vao = v2_neg(s.a.p);
+            f32 ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1);
+            /* Determine point on shape 0 */
+            {
+                shape0_p = v2_sub(s.b.p0, s.a.p0);
+                shape0_p = v2_mul(shape0_p, ratio);
+                shape0_p = v2_add(shape0_p, s.a.p0);
+            }
+            /* Determine point on shape 1 */
+            {
+                shape1_p = v2_sub(s.b.p1, s.a.p1);
+                shape1_p = v2_mul(shape1_p, ratio);
+                shape1_p = v2_add(shape1_p, s.a.p1);
+            }
+        }
+    }
+#else
+    {
+        if (s.len == 1) {
+            shape0_p = s.a.p0;
+            shape1_p = s.a.p1;
+        } else if (s.len == 2) {
+            /* Determine ratio between edge a & b that ray intersection lies */
+            /* FIXME: Winding order dependent? */
+            f32 wedgea = math_fabs(v2_wedge(movedir, s.a.p));
+            f32 wedgeb = math_fabs(v2_wedge(movedir, s.b.p));
+            f32 ratio = wedgea / (wedgea + wedgeb);
+            /* Determine point on shape 0 */
+            {
+                shape0_p = v2_sub(s.b.p0, s.a.p0);
+                shape0_p = v2_mul(shape0_p, ratio);
+                shape0_p = v2_add(shape0_p, s.a.p0);
+            }
+            /* Determine point on shape 1 */
+            {
+                shape1_p = v2_sub(s.b.p1, s.a.p1);
+                shape1_p = v2_mul(shape1_p, ratio);
+                shape1_p = v2_add(shape1_p, s.a.p1);
+            }
+        }
+    }
+#endif
+
+abort:
+    struct gjk_extended_result res = { 0 };
+    res.colliding = colliding;
+    res.p0 = shape0_p;
+    res.p1 = shape1_p;
+    res.final_simplex = s;
+    scratch_end(scratch);
+    return res;
+}
+
+#endif
+
+
+
+
+
+
+
+
+
+
+
+
+/* TODO: Remove this (debugging) */
+struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1)
+{
+    struct v2_array res = { .points = arena_dry_push(arena, struct v2) };
+    u64 rays = 500;
+    for (u64 i = 0; i < rays; ++i) {
+        f32 angle = ((f32)i / rays) * (2 * PI);
+        struct v2 dir = v2_from_angle(angle);
+        struct v2 p = extended_menkowski_point(poly0, poly1, dir).p;
+        if (res.count == 0 || !v2_eq(p, res.points[res.count - 1])) {
+            *arena_push(arena, struct v2) = p;
+            ++res.count;
+        }
+    }
+    return res;
+}
diff --git a/src/util.h b/src/util.h
index 8e3c7f72..cf6c9162 100644
--- a/src/util.h
+++ b/src/util.h
@@ -206,11 +206,11 @@ struct gjk_extended_result {
     struct gjk_extended_simplex final_simplex;
 };
 
-struct v2 poly_support(struct v2_array a, struct v2 dir);
 struct v2 perp_towards_point(struct v2 start, struct v2 end, struct v2 p);
+struct v2 perp_towards_dir(struct v2 start, struct v2 end, struct v2 dir);
 struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1);
 i32 poly_get_winding_order(struct v2_array poly);
-b32 gjk_boolean(struct v2_array poly0, struct v2_array poly1, u32 max_steps);
+b32 gjk_boolean(struct v2_array poly0, struct v2_array poly1);
 struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array poly1, u32 max_steps);
 
 #endif