From 99381f127462e80782c4de78b19f402ce77acc4f Mon Sep 17 00:00:00 2001
From: jacob <jacob@cagori.com>
Date: Fri, 30 Aug 2024 09:00:18 -0500
Subject: [PATCH] gjk tweaks

---
 src/math.h |   6 +-
 src/user.c |   8 +--
 src/util.c | 181 +++--------------------------------------------------
 src/util.h |   2 +-
 4 files changed, 17 insertions(+), 180 deletions(-)

diff --git a/src/math.h b/src/math.h
index 673ca2e2..175edb75 100644
--- a/src/math.h
+++ b/src/math.h
@@ -651,9 +651,9 @@ INLINE struct v2 v2_norm(struct v2 a)
 {
     f32 l = v2_len_squared(a);
     if (l != 0) {
-        l = math_sqrt(l);
-        a.x /= l;
-        a.y /= l;
+        f32 denom = 1.f / math_sqrt(l);
+        a.x *= denom;
+        a.y *= denom;
     }
     return a;
 }
diff --git a/src/user.c b/src/user.c
index c4ff5526..fbd2176f 100644
--- a/src/user.c
+++ b/src/user.c
@@ -1034,11 +1034,11 @@ INTERNAL void user_update(void)
 
                 /* Draw simplex */
                 {
-                    f32 thickness = 2;
+                    f32 thickness = 5;
                     u32 line_color = colliding ? COLOR_WHITE: COLOR_YELLOW;
-                    u32 color_first = COLOR_RED;
-                    u32 color_second = COLOR_GREEN;
-                    u32 color_third = COLOR_TURQOISE;
+                    u32 color_first = RGBA_32_F(1, 0, 0, 0.5);
+                    u32 color_second = RGBA_32_F(0, 1, 0, 0.5);
+                    u32 color_third = RGBA_32_F(0, 0, 1, 0.5);
 
                     struct simplex simplex = ent->simplex;
                     struct v2 simplex_points[] = { simplex.a, simplex.b, simplex.c };
diff --git a/src/util.c b/src/util.c
index c6a9a9fe..5879b5a2 100644
--- a/src/util.c
+++ b/src/util.c
@@ -65,158 +65,13 @@ struct v2 menkowski_point(struct v2_array poly0, struct v2_array poly1, struct v
     return v2_sub(poly_support(poly0, dir), poly_support(poly1, v2_neg(dir)));
 }
 
-struct v2 normal_towards_point(struct v2 start, struct v2 end, struct v2 p)
+struct v2 perp_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 v2 perp = v2_perp_cw(v2_sub(end, start));
+    i32 sign = 1 - ((v2_dot(perp, v2_sub(p, start)) < 0) << 1);
+    return v2_mul(perp, sign);
 }
 
-#if 0
-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 };
-
-    /* Append first point to simplex */
-    struct v2 dir = v2_norm(v2_sub(poly1_center, poly0_center));
-    s.a = menkowski_point(poly0, poly1, dir);
-    s.len = 1;
-
-    dir = v2_norm(v2_neg(s.a));  /* Next point is towards origin */
-
-    b32 colliding = false;
-    while (true) {
-        /* Determine support point */
-        struct v2 p = menkowski_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 = s.a;
-                s.a = p;
-            } else if (s.len == 2) {
-                s.c = s.b;
-                s.b = s.a;
-                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 new 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 (will happen automatically next iteration) */
-            } 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 is in simplex */
-                    colliding = true;
-                    break;
-                }
-            }
-        }
-    }
-
-    return (struct gjk_result) {
-        .colliding = colliding,
-        .final_simplex = s
-    };
-}
-
-#elif 0
-
-struct gjk_result gjk(struct v2_array poly0, struct v2_array poly1)
-{
-    b32 colliding = false;
-
-    /* Simplex */
-    struct simplex s = { 0 };
-
-    /* First point is support point towards shape centers */
-    {
-        s.a = menkowski_point(poly0, poly1, v2_norm(v2_sub(poly1.points[0], poly0.points[0])));
-        s.len = 1;
-    }
-
-    /* Second point is support point towards origin */
-    {
-        struct v2 dir = v2_norm(v2_neg(s.a));
-        struct v2 p = menkowski_point(poly0, poly1, dir);
-        if (!v2_eq(p, s.a)) {
-            s.b = s.a;
-            s.a = p;
-            s.len = 2;
-        }
-    }
-
-    if (s.len == 2) {
-        while (true) {
-            /* Third point is support point in direction of line normal towards origin */
-            struct v2 dir = normal_towards_point(s.a, s.b, V2(0, 0));
-            struct v2 p = menkowski_point(poly0, poly1, dir);
-            if (v2_eq(p, s.a) || v2_eq(p, s.b)) {
-                colliding = false;
-                break;
-            }
-            s.c = s.b;
-            s.b = s.a;
-            s.a = p;
-
-            struct v2 a_to_origin = 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) >= 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(normal_towards_point(s.a, s.c, s.b));  /* Normal dir 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 */
-                    s.len = 3;
-                    colliding = true;
-                    break;
-                }
-            }
-        }
-    }
-
-    return (struct gjk_result) {
-        .colliding = colliding,
-        .final_simplex = s
-    };
-}
-
-#else
-
 struct gjk_result gjk(struct v2_array poly0, struct v2_array poly1, u32 max_steps)
 {
     b32 colliding = false;
@@ -235,21 +90,13 @@ struct gjk_result gjk(struct v2_array poly0, struct v2_array poly1, u32 max_step
     /* Second point is support point towards origin */
     if (step++ >= max_steps) goto abort;
     {
-        struct v2 dir = v2_norm(v2_neg(s.a));
+        struct v2 dir = v2_neg(s.a);
         struct v2 p = menkowski_point(poly0, poly1, dir);
-#if 0
-        if (!v2_eq(p, s.a)) {
+        if (v2_dot(dir, p) >= 0) {
             s.b = s.a;
             s.a = p;
             s.len = 2;
         }
-#else
-        if (v2_dot(s.a, p) < 0) {
-            s.b = s.a;
-            s.a = p;
-            s.len = 2;
-        }
-#endif
     }
 
     if (s.len == 2) {
@@ -257,33 +104,24 @@ struct gjk_result gjk(struct v2_array poly0, struct v2_array poly1, u32 max_step
             if (step++ >= max_steps) goto abort;
 
             /* Third point is support point in direction of line normal towards origin */
-            struct v2 dir = normal_towards_point(s.a, s.b, V2(0, 0));
+            struct v2 dir = perp_towards_point(s.a, s.b, V2(0, 0));
             struct v2 p = menkowski_point(poly0, poly1, dir);
-#if 0
-            if (v2_eq(p, s.a) || v2_eq(p, s.b)) {
-                colliding = false;
-                break;
-            }
-#else
             if (v2_dot(dir, p) < 0) {
                 colliding = false;
                 break;
             }
-#endif
 
             s.c = s.b;
             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 */
             struct v2 a_to_origin = 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) >= 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(normal_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 dir 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;
@@ -304,7 +142,6 @@ struct gjk_result gjk(struct v2_array poly0, struct v2_array poly1, u32 max_step
     };
 }
 
-#endif
 
 struct v2 epa(struct v2_array poly0, struct v2_array poly1, struct simplex simplex)
 {
diff --git a/src/util.h b/src/util.h
index eada1840..2dba5ce2 100644
--- a/src/util.h
+++ b/src/util.h
@@ -198,7 +198,7 @@ struct gjk_result {
 
 struct v2 poly_support(struct v2_array a, struct v2 dir);
 struct v2 menkowski_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 v2 perp_towards_point(struct v2 start, struct v2 end, struct v2 p);
 struct gjk_result gjk(struct v2_array poly0, struct v2_array poly1, u32 max_steps);
 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);