From 2150d0e3c42ec23353b07ecec6ee48243a7b4574 Mon Sep 17 00:00:00 2001
From: jacob <jacob@cagori.com>
Date: Thu, 29 Aug 2024 11:31:57 -0500
Subject: [PATCH] working epa test

---
 src/entity.h |   6 +-
 src/game.c   | 184 ++--------------------------------------
 src/user.c   |  63 +++++++++++++-
 src/util.c   | 234 +++++++++++++++++++++++++++++++++++++++++++++++++++
 src/util.h   |  22 +++++
 5 files changed, 323 insertions(+), 186 deletions(-)

diff --git a/src/entity.h b/src/entity.h
index c2ae9e31..8cd84b55 100644
--- a/src/entity.h
+++ b/src/entity.h
@@ -55,11 +55,6 @@ struct entity_store {
 
 
 
-/* TODO: Remove this */
-struct simplex {
-    struct v2 a, b, c;
-};
-
 
 
 
@@ -98,6 +93,7 @@ struct entity {
 
     /* TODO: Remove this (testing) */
     b32 colliding;
+    struct entity_handle colliding_with;
     struct simplex simplex;
     struct v2 pen;
 
diff --git a/src/game.c b/src/game.c
index 3e69b64c..329b33b7 100644
--- a/src/game.c
+++ b/src/game.c
@@ -204,182 +204,6 @@ INTERNAL void spawn_test_entities(void)
     }
 }
 
-/* ========================== *
- * Collision test
- * ========================== */
-
-INTERNAL struct v2 poly_furthest_point(struct v2_array a, struct v2 dir)
-{
-    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 v2 poly_support_point(struct v2_array poly0, struct v2_array poly1, struct v2 dir)
-{
-    return v2_sub(poly_furthest_point(poly0, dir), poly_furthest_point(poly1, v2_neg(dir)));
-}
-
-INTERNAL struct v2 normal_towards_point(struct v2 start, struct v2 end, struct v2 p)
-{
-    struct v2 dir = v2_norm(v2_perp_cw(v2_sub(end, start)));
-    i32 sign = 1 - ((v2_dot(dir, v2_sub(p, start)) < 0) << 1);
-    return v2_mul(dir, sign);
-}
-
-struct gjk_result {
-    b32 colliding;
-    struct simplex final_simplex;
-};
-
-INTERNAL struct gjk_result gjk(struct v2_array poly0, struct v2_array poly1)
-{
-    struct v2 poly0_center = math_poly_center(poly0);
-    struct v2 poly1_center = math_poly_center(poly1);
-
-    /* Simplex */
-    struct simplex s = { 0 };
-    u32 s_len = 0;
-
-    /* Append first point to simplex */
-    struct v2 dir = v2_norm(v2_sub(poly1_center, poly0_center));
-    s.c = poly_support_point(poly0, poly1, dir);
-    s_len = 1;
-
-    dir = v2_norm(v2_neg(s.c));  /* Next point is towards origin */
-
-    b32 colliding = false;
-    while (true) {
-        /* Determine support point */
-        struct v2 p = poly_support_point(poly0, poly1, dir);
-        if (v2_dot(dir, p) < 0) {
-            /* Point did not cross origin */
-            colliding = false;
-            break;
-        }
-
-        if (s_len < 3) {
-            /* Line case */
-            /* Next dir is line normal towards origin */
-            if (s_len == 1) {
-                s.b = p;
-            } else if (s_len == 2) {
-                s.a = p;
-            }
-            ++s_len;
-            dir = normal_towards_point(s.a, s.b, V2(0, 0));
-        } else {
-            /* Triangle case */
-            s.c = s.b;
-            s.b = s.a;
-            s.a = p;
-
-            /* Ensure point is unique */
-            if (v2_eq(s.a, s.b)
-             || v2_eq(s.b, s.c)
-             || v2_eq(s.a, s.c)) {
-                colliding = false;
-                break;
-            }
-
-            struct v2 a_to_origin_rel = v2_neg(s.a);
-
-            dir = v2_neg(normal_towards_point(s.a, s.b, s.c));  /* Normal dir of ab pointing away from c */
-            if (v2_dot(dir, a_to_origin_rel) >= 0) {
-                /* Point is in region ab, remove c from simplex */
-            } else {
-                dir = v2_neg(normal_towards_point(s.a, s.c, s.b));  /* Normal dir of ac pointing away from b */
-                if (v2_dot(dir, a_to_origin_rel) >= 0) {
-                    /* Point is in region ac, remove b from simplex */
-                    s.b = s.c;
-                } else {
-                    /* Point must be in simplex */
-                    colliding = true;
-                    break;
-                }
-            }
-        }
-    }
-
-    return (struct gjk_result) {
-        .colliding = colliding,
-        .final_simplex = s
-    };
-}
-
-INTERNAL struct v2 epa(struct v2_array poly0, struct v2_array poly1, struct simplex simplex)
-{
-    struct temp_arena scratch = scratch_begin_no_conflict();
-
-    struct v2 *poly = arena_dry_push(scratch.arena, struct v2);
-    {
-        struct v2 *tmp = arena_push_array(scratch.arena, struct v2, 3);
-        tmp[0] = simplex.a;
-        tmp[1] = simplex.b;
-        tmp[2] = simplex.c;
-    }
-    u32 poly_count = 3;
-
-    struct v2 pen = V2(0, 0);
-    f32 pen_len = F32_INFINITY;
-    while (true) {
-        /* Find dir from origin to closest edge */
-        struct v2 ps = V2(0, 0);
-        struct v2 pe = V2(0, 0);
-        for (u32 i = 0; i < poly_count; ++i) {
-            ps = poly[i];
-            pe = poly[(i < poly_count - 1) ? i : 0];
-
-            struct v2 vso = v2_neg(ps);
-            struct v2 vse = v2_norm(v2_sub(pe, ps));
-
-            f32 dot = v2_dot(vso, vse);
-
-            struct v2 vsd = v2_mul(vse, dot);
-            struct v2 pd = v2_add(ps, vsd);
-
-            /* TODO: sq cmp */
-            f32 pd_len = v2_len(pd);
-            if (pd_len < pen_len) {
-                pen = pd;
-                pen_len = pd_len;
-            }
-        }
-
-        /* Find new point in dir */
-#if 0
-        /* FIXME: Maintain convexity */
-        struct v2 p = poly_support_point(poly0, poly1, pen);
-        if (v2_eq(p, ps) || v2_eq(p, pe)) {
-            break;
-        } else {
-            *arena_push(scratch.arena, struct v2) = p;
-            ++poly_count;
-        }
-#else
-        /* FIXME: Maintain convexity */
-        struct v2 p = poly_support_point(poly0, poly1, pen);
-        if (v2_eq(p, ps) || v2_eq(p, pe)) {
-            break;
-        } else {
-            break;
-        }
-#endif
-    }
-    pen = v2_mul(v2_norm(pen), pen_len);
-
-    scratch_end(scratch);
-    return pen;
-}
-
 /* ========================== *
  * Update
  * ========================== */
@@ -905,6 +729,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
         b32 colliding = false;
         struct simplex simplex = { 0 };
         struct v2 pen = V2(0, 0);
+        struct entity_handle colliding_with = { 0 };
         for (u64 e1_index = 0; e1_index < store->reserved; ++e1_index) {
             struct entity *e1 = &store->entities[e1_index];
             if (e1 == e0) continue;
@@ -927,13 +752,13 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
 
             struct gjk_result res = gjk(e0_poly, e1_poly);
             colliding = res.colliding;
+            colliding_with = e1->handle;
             simplex = res.final_simplex;
+            pen = epa(e0_poly, e1_poly, simplex);
 
             if (colliding) {
                 /* Pen movement test */
-                pen = epa(e0_poly, e1_poly, simplex);
-
-#if 0
+#if 1
                 {
                     struct xform xf = e1_xf;
 
@@ -950,6 +775,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
         }
 
         e0->colliding = colliding;
+        e0->colliding_with = colliding_with;
         e0->simplex = simplex;
         e0->pen = pen;
     }
diff --git a/src/user.c b/src/user.c
index f9ab73b4..68c8e4f4 100644
--- a/src/user.c
+++ b/src/user.c
@@ -391,6 +391,25 @@ INTERNAL void debug_draw_movement(struct entity *ent)
     draw_solid_arrow_ray(G.viewport_canvas, pos, vel_ray, thickness, arrow_len, color_vel);
 }
 
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
 INTERNAL void user_update(void)
 {
     struct temp_arena scratch = scratch_begin_no_conflict();
@@ -957,9 +976,49 @@ INTERNAL void user_update(void)
             /* Draw player collision */
             if (entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED)) {
                 b32 colliding = ent->colliding;
+                struct entity *e1 = entity_from_handle(store, ent->colliding_with);
+
+                /* Draw menkowski */
+                {
+                    struct quad ent_quad;
+                    struct v2_array ent_poly;
+                    struct quad e1_quad;
+                    struct v2_array e1_poly;
+                    {
+                        {
+                            struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, ent->sprite);
+                            struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("shape"), ent->animation_frame);
+                            ent_quad = xform_mul_quad(ent->sprite_local_xform, quad_from_rect(slice.rect));
+                            ent_quad = xform_mul_quad(entity_get_xform(ent), ent_quad);
+                            ent_poly = (struct v2_array) {
+                                .count = ARRAY_COUNT(ent_quad.e),
+                                .points = ent_quad.e
+                            };
+                        }
+                        {
+                            struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, e1->sprite);
+                            struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("shape"), e1->animation_frame);
+                            e1_quad = xform_mul_quad(e1->sprite_local_xform, quad_from_rect(slice.rect));
+                            e1_quad = xform_mul_quad(entity_get_xform(e1), e1_quad);
+                            e1_poly = (struct v2_array) {
+                                .count = ARRAY_COUNT(e1_quad.e),
+                                .points = e1_quad.e
+                            };
+                        }
+                    }
+
+                    u32 color = RGBA_32_F(0, 0, 0.75, 1);
+                    f32 thickness = 2;
+                    (UNUSED)thickness;
+
+                    struct v2_array m = menkowski(temp.arena, ent_poly, e1_poly);
+                    for (u64 i = 0; i < m.count; ++i) m.points[i] = xform_mul_v2(G.world_view, m.points[i]);
+                    draw_solid_poly_line(G.viewport_canvas, m, true, thickness, color);
+                    //draw_solid_poly(G.viewport_canvas, m, color);
+                }
 
                 /* Draw simplex */
-                {
+                if (colliding) {
                     f32 thickness = 2;
                     u32 color = colliding ? COLOR_WHITE: COLOR_YELLOW;
                     struct simplex simplex = ent->simplex;
@@ -970,7 +1029,7 @@ INTERNAL void user_update(void)
                 }
 
                 /* Draw pen */
-                {
+                if (colliding) {
                     f32 thickness = 2;
                     u32 color = COLOR_RED;
 
diff --git a/src/util.c b/src/util.c
index e5be98fd..0283f2b6 100644
--- a/src/util.c
+++ b/src/util.c
@@ -34,3 +34,237 @@ struct string util_file_name_from_path(struct string path)
     };
 }
 #endif
+
+/* ========================== *
+ * Collision test
+ * ========================== */
+
+#include "math.h"
+#include "arena.h"
+#include "scratch.h"
+
+/* TODO: Remove / move this */
+
+struct v2 poly_furthest_point(struct v2_array a, struct v2 dir)
+{
+    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;
+}
+
+struct v2 poly_support_point(struct v2_array poly0, struct v2_array poly1, struct v2 dir)
+{
+    return v2_sub(poly_furthest_point(poly0, dir), poly_furthest_point(poly1, v2_neg(dir)));
+}
+
+struct v2 normal_towards_point(struct v2 start, struct v2 end, struct v2 p)
+{
+    struct v2 dir = v2_norm(v2_perp_cw(v2_sub(end, start)));
+    i32 sign = 1 - ((v2_dot(dir, v2_sub(p, start)) < 0) << 1);
+    return v2_mul(dir, sign);
+}
+
+struct gjk_result gjk(struct v2_array poly0, struct v2_array poly1)
+{
+    struct v2 poly0_center = math_poly_center(poly0);
+    struct v2 poly1_center = math_poly_center(poly1);
+
+    /* Simplex */
+    struct simplex s = { 0 };
+    u32 s_len = 0;
+
+    /* Append first point to simplex */
+    struct v2 dir = v2_norm(v2_sub(poly1_center, poly0_center));
+    s.c = poly_support_point(poly0, poly1, dir);
+    s_len = 1;
+
+    dir = v2_norm(v2_neg(s.c));  /* Next point is towards origin */
+
+    b32 colliding = false;
+    while (true) {
+        /* Determine support point */
+        struct v2 p = poly_support_point(poly0, poly1, dir);
+        if (v2_dot(dir, p) < 0) {
+            /* Point did not cross origin */
+            colliding = false;
+            break;
+        }
+
+        if (s_len < 3) {
+            /* Line case */
+            /* Next dir is line normal towards origin */
+            if (s_len == 1) {
+                s.b = p;
+            } else if (s_len == 2) {
+                s.a = p;
+            }
+            ++s_len;
+            dir = normal_towards_point(s.a, s.b, V2(0, 0));
+        } else {
+            /* Triangle case */
+            s.c = s.b;
+            s.b = s.a;
+            s.a = p;
+
+            /* Ensure point is unique */
+            /* TODO: Is this necessary? */
+            if (v2_eq(s.a, s.b)
+                || v2_eq(s.b, s.c)
+                || v2_eq(s.a, s.c)) {
+                colliding = false;
+                break;
+            }
+
+            struct v2 a_to_origin_rel = v2_neg(s.a);
+
+            dir = v2_neg(normal_towards_point(s.a, s.b, s.c));  /* Normal dir of ab pointing away from c */
+            if (v2_dot(dir, a_to_origin_rel) >= 0) {
+                /* Point is in region ab, remove c from simplex */
+            } else {
+                dir = v2_neg(normal_towards_point(s.a, s.c, s.b));  /* Normal dir of ac pointing away from b */
+                if (v2_dot(dir, a_to_origin_rel) >= 0) {
+                    /* Point is in region ac, remove b from simplex */
+                    s.b = s.c;
+                } else {
+                    /* Point must be in simplex */
+                    colliding = true;
+                    break;
+                }
+            }
+        }
+    }
+
+    return (struct gjk_result)
+    {
+        .colliding = colliding,
+            .final_simplex = s
+    };
+}
+
+struct v2 epa(struct v2_array poly0, struct v2_array poly1, struct simplex simplex)
+{
+    struct temp_arena scratch = scratch_begin_no_conflict();
+
+    struct v2 *proto = arena_dry_push(scratch.arena, struct v2);
+    {
+        struct v2 *tmp = arena_push_array(scratch.arena, struct v2, 3);
+        tmp[0] = simplex.a;
+        tmp[1] = simplex.b;
+        tmp[2] = simplex.c;
+    }
+    u32 proto_count = 3;
+
+    struct v2 pen = V2(0, 0);
+    f32 pen_len = F32_INFINITY;
+    while (true) {
+        pen = V2(0, 0);
+        pen_len = F32_INFINITY;
+
+        /* Find dir from origin to closest edge */
+        u32 pen_pe_index = 1;
+        for (u32 i = 0; i < proto_count; ++i) {
+            u32 ps_index = i;
+            u32 pe_index = (i < proto_count - 1) ? (i + 1) : 0;
+            struct v2 ps = proto[ps_index];
+            struct v2 pe = proto[pe_index];
+
+            struct v2 vse = v2_sub(pe, ps);
+            struct v2 vso = v2_neg(ps);
+
+            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) {
+                pen_pe_index = pe_index;
+                pen = pd;
+                pen_len = pd_len;
+            }
+        }
+
+        /* Find new point in dir */
+#if 1
+        /* FIXME: Ensure convexity */
+
+        struct v2 p = poly_support_point(poly0, poly1, pen);
+
+        /* Check unique */
+        /* TODO: Better */
+        /* TODO: Epsilon or iteration limit */
+        {
+            b32 unique = true;
+            for (u32 i = 0; i < proto_count; ++i) {
+                if (v2_eq(p, proto[i])) {
+                    unique = false;
+                    break;
+                }
+            }
+            if (!unique) {
+                break;
+            }
+        }
+
+        /* Insert point into prototype array */
+        arena_push(scratch.arena, struct v2);
+        ++proto_count;
+        for (u32 i = proto_count - 1; i > pen_pe_index; --i) {
+            u32 shift_from = (i > 0) ? i - 1 : proto_count - 1;
+            u32 shift_to = i;
+            proto[shift_to] = proto[shift_from];
+        }
+        proto[pen_pe_index] = p;
+#else
+        /* FIXME: Maintain convexity */
+        struct v2 p = poly_support_point(poly0, poly1, pen);
+        if (v2_eq(p, ps) || v2_eq(p, pe)) {
+            break;
+        } else {
+            break;
+        }
+#endif
+    }
+    pen = v2_mul(v2_norm(pen), pen_len);
+
+    scratch_end(scratch);
+    return pen;
+}
+
+/* TODO: Remove this (debugging) */
+struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1)
+{
+    DEBUGBREAKABLE;
+#if 0
+    struct v2_array res = { .points = arena_dry_push(arena, struct v2) };
+    for (u64 i = 0; i < poly0.count; ++i) {
+        struct v2 a = poly0.points[i];
+        for (u64 j = 0; j < poly1.count; ++j) {
+            struct v2 b = poly1.points[j];
+            *arena_push(arena, struct v2) = v2_sub(a, b);
+            ++res.count;
+        }
+    }
+    return res;
+#else
+    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 = poly_support_point(poly0, poly1, dir);
+        if (res.count == 0 || !v2_eq(p, res.points[res.count - 1])) {
+            *arena_push(arena, struct v2) = p;
+            ++res.count;
+        }
+    }
+    return res;
+#endif
+}
diff --git a/src/util.h b/src/util.h
index ddfdd47f..b64aa4bf 100644
--- a/src/util.h
+++ b/src/util.h
@@ -180,4 +180,26 @@ INLINE void sleep_frame(sys_timestamp_t last_frame_time, f64 target_dt)
     }
 }
 
+/* ========================== *
+ * Collision testing
+ * ========================== */
+
+/* TODO: Remove this */
+
+struct simplex {
+    struct v2 a, b, c;
+};
+
+struct gjk_result {
+    b32 colliding;
+    struct simplex final_simplex;
+};
+
+struct v2 poly_furthest_point(struct v2_array a, struct v2 dir);
+struct v2 poly_support_point(struct v2_array poly0, struct v2_array poly1, struct v2 dir);
+struct v2 normal_towards_point(struct v2 start, struct v2 end, struct v2 p);
+struct gjk_result gjk(struct v2_array poly0, struct v2_array poly1);
+struct v2 epa(struct v2_array poly0, struct v2_array poly1, struct simplex simplex);
+struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1);
+
 #endif