use shortcut-gjk for gjk_contact_points

src/game.c

@@ -129,10 +129,10 @@ INTERNAL void spawn_test_entities(void)
         //struct v2 pos = V2(0.374142020941, -0.246118023992);  /* Touching glitch spot */
         //struct v2 size = V2(1, 1);
         struct v2 size = V2(0.5, 0.5);
-        //f32 r = PI / 4;
+        f32 r = PI / 4;
         //f32 r = PI / 3;
         //f32 r = PI / 2;
-        f32 r = 0;
+        //f32 r = 0;
         f32 skew = 0;
 
         struct entity *e = entity_alloc(root);
@@ -155,8 +155,8 @@ INTERNAL void spawn_test_entities(void)
 
         entity_enable_prop(e, ENTITY_PROP_PHYSICAL);
         e->mass_unscaled = 100;
-        e->inertia_unscaled = 1;
+        //e->inertia_unscaled = 1;
-        //e->inertia_unscaled = F32_INFINITY;
+        e->inertia_unscaled = F32_INFINITY;
         e->linear_ground_friction = 1000;
         e->angular_ground_friction = 100;
 
@@ -200,8 +200,8 @@ INTERNAL void spawn_test_entities(void)
 
         entity_enable_prop(e, ENTITY_PROP_PHYSICAL);
 #if 0
-        e->mass_unscaled = 1000;
+        e->mass_unscaled = 500;
-        e->inertia_unscaled = 1000;
+        e->inertia_unscaled = 500;
 #else
         e->mass_unscaled = F32_INFINITY;
         e->inertia_unscaled = F32_INFINITY;
@@ -566,7 +566,7 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias)
                     //f32 bias_factor = 0.2;
                     f32 bias_factor = 0.1;
 
-                    f32 bias_slop = 0.001;
+                    f32 bias_slop = 0.0005;
                     //f32 bias_slop = 0.001;
                     //f32 bias_slop = 0.00;
 

src/gjk.c

@@ -56,13 +56,16 @@ b32 gjk_boolean(struct v2_array shape0, struct v2_array shape1)
             s.b = s.a;
             s.a = p;
 
-            dir = v2_perp_towards_dir(v2_sub(s.b, s.a), v2_neg(v2_sub(s.c, s.a)));  /* Normal of ab pointing away from c */
+            struct v2 vab = v2_sub(s.b, s.a);
+            struct v2 vac = v2_sub(s.c, s.a);
             struct v2 a_to_origin = v2_neg(s.a);
+
+            dir = v2_perp_towards_dir(vab, v2_neg(vac));  /* Normal 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_perp_towards_dir(v2_sub(s.c, s.a), v2_neg(v2_sub(s.b, s.a)));  /* Normal of ac pointing away from b */
+                dir = v2_perp_towards_dir(vac, v2_neg(vab));  /* 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;
@@ -147,6 +150,281 @@ struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_arra
 
 
 
+
+
+
+
+
+/* TODO: Update this function to only calculate contact pairs if colliding. Will allow for shortcut-style GJK.
+ * `colliding` & `has_2nd_pair` become "num_pairs", where 0 = no collision. */
+struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, struct v2_array shape1)
+{
+    struct temp_arena scratch = scratch_begin_no_conflict();
+    struct gjk_contact_points_result res = { 0 };
+
+    /* TODO: Verify epsilon */
+    /* FIXME: Infinite loop when epsilon too low and shapes is too far from 0 (precision issue) */
+    const f32 epsilon = 0.0000100;
+    struct gjk_simplex s = { 0 };
+    b32 colliding = false;
+    struct gjk_menkowski_point *proto = NULL;
+    u32 proto_count = 0;
+
+    struct gjk_contact_pair pair0 = { 0 };
+    struct gjk_contact_pair pair1 = { 0 };
+    u32 num_pairs = 0;
+
+    /* Used by GJK & EPA */
+    struct v2 dir = { 0 };
+    struct gjk_menkowski_point m = { 0 };
+
+#if GJK_DEBUG
+    u32 dbg_step = 0;
+#endif
+
+    /* ========================== *
+     * GJK
+     * ========================== */
+
+    {
+        /* First point is support point in shape's general directions to eachother */
+        s.a = menkowski_point_extended(shape0, shape1, v2_sub(shape1.points[0], shape0.points[0]));
+        s.len = 1;
+
+        /* Second point is support point towards origin */
+        dir = v2_neg(s.a.p);
+        m = menkowski_point_extended(shape0, shape1, dir);
+        if (v2_dot(dir, m.p) >= 0) {
+            s.b = s.a;
+            s.a = m;
+            s.len = 2;
+            while (true) {
+                /* 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) {
+                    /* New point did not cross origin, collision impossible */
+                    break;
+                }
+
+                s.c = s.b;
+                s.b = s.a;
+                s.a = m;
+                s.len = 3;
+
+                struct v2 vab = v2_sub(s.b.p, s.a.p);
+                struct v2 vac = v2_sub(s.c.p, s.a.p);
+                struct v2 a_to_origin = v2_neg(s.a.p);
+
+                dir = v2_perp_towards_dir(vab, v2_neg(vac));  /* Normal of ab pointing away from c */
+                if (v2_dot(dir, a_to_origin) >= 0) {
+                    /* Point is in region ab, remove c from simplex */
+                    s.len = 2;
+                } else {
+                    /* Point is not in region ab */
+                    dir = v2_perp_towards_dir(vac, v2_neg(vab));  /* 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;
+                        s.len = 2;
+                    } else {
+                        /* Point is in simplex */
+                        colliding = true;
+                        break;
+                    }
+                }
+            }
+        }
+    }
+
+    if (colliding) {
+        /* ========================== *
+         * Epa
+         * ========================== */
+
+        proto = arena_dry_push(scratch.arena, struct gjk_menkowski_point);
+        proto_count = 0;
+        {
+            ASSERT(s.len == 3);
+            struct gjk_menkowski_point *tmp = arena_push_array(scratch.arena, struct gjk_menkowski_point, 3);
+            tmp[0] = s.a;
+            tmp[1] = s.b;
+            tmp[2] = s.c;
+            proto_count = 3;
+        }
+
+        while (true) {
+            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 = 0;
+            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].p;
+                struct v2 pe = proto[pe_index].p;
+
+                struct v2 vse = v2_sub(pe, ps);
+                struct v2 vso = v2_neg(ps);
+
+                f32 d1 = v2_dot(vso, vse);
+                f32 d2 = v2_dot(vse, vse);
+                struct v2 vsd = v2_mul(vse, (d1 / d2));
+                struct v2 pd = v2_add(ps, vsd);
+
+                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_len_sq = pd_len_sq;
+                    dir = pd;
+                }
+            }
+
+            /* TODO: Move to break (debugging) */
+            s.a = proto[pen_ps_index];
+            s.b = proto[pen_pe_index];
+            s.len = 2;
+
+            /* Find new point in dir */
+            m = menkowski_point_extended(shape0, shape1, dir);
+
+            /* Check unique */
+            /* TODO: Better */
+            DBGSTEP;
+            {
+                b32 unique = true;
+                for (u32 i = 0; i < proto_count; ++i) {
+                    struct v2 edge_start = proto[i].p;
+                    struct v2 edge_end = i < proto_count - 1 ? proto[i + 1].p : proto[0].p;
+                    if (math_fabs(v2_wedge(v2_sub(edge_end, edge_start), v2_sub(m.p, edge_start))) < epsilon) {
+                        unique = false;
+                        break;
+                    }
+                }
+                if (!unique) {
+                    break;
+                }
+            }
+
+            /* Insert point into prototype */
+            /* FIXME: Preserve winding order */
+            arena_push(scratch.arena, struct gjk_menkowski_point);
+            ++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] = m;
+        }
+
+        /* ========================== *
+         * Resolve points
+         * ========================== */
+
+        if (s.len == 1) {
+            num_pairs = 1;
+            pair0.p0 = s.a.p0;
+            pair0.p1 = s.a.p1;
+        } else {
+            ASSERT(s.len == 2);
+            /* TODO: Epsilon */
+            if (!v2_eq(s.a.p0, s.b.p0) && !v2_eq(s.a.p1, s.b.p1)) {
+                /* Closest features are both faces, return clipped edge points of each face */
+                num_pairs = 2;
+                struct v2 a = s.a.p0;
+                struct v2 b = s.b.p0;
+                struct v2 c = s.a.p1;
+                struct v2 d = s.b.p1;
+
+                struct v2 vab = v2_sub(b, a);
+                struct v2 vac = v2_sub(c, a);
+                struct v2 vad = v2_sub(d, a);
+
+                struct v2 vcd = v2_sub(d, c);
+                struct v2 vca = v2_sub(a, c);
+                struct v2 vcb = v2_sub(b, c);
+
+                f32 inv_vab_len_sq = 1.f / v2_len_sq(vab);
+                f32 inv_vcd_len_sq = 1.f / v2_len_sq(vcd);
+
+                pair0.p0 = v2_add(a, v2_mul(vab, clamp_f32(v2_dot(vab, vac) * inv_vab_len_sq, 0, 1)));
+                pair0.p1 = v2_add(c, v2_mul(vcd, clamp_f32(v2_dot(vcd, vcb) * inv_vcd_len_sq, 0, 1)));
+                pair1.p0 = v2_add(a, v2_mul(vab, clamp_f32(v2_dot(vab, vad) * inv_vab_len_sq, 0, 1)));
+                pair1.p1 = v2_add(c, v2_mul(vcd, clamp_f32(v2_dot(vcd, vca) * inv_vcd_len_sq, 0, 1)));
+            } else {
+                /* Determine ratio between edge a & b that projected origin lies */
+                num_pairs = 1;
+                struct v2 vab = v2_sub(s.b.p, s.a.p);
+                f32 ratio = clamp_f32(v2_dot(vab, v2_neg(s.a.p)) / v2_dot(vab, vab), 0, 1);
+                pair0.p0 = v2_add(s.a.p0, v2_mul(v2_sub(s.b.p0, s.a.p0), ratio));
+                pair0.p1 = v2_add(s.a.p1, v2_mul(v2_sub(s.b.p1, s.a.p1), ratio));
+            }
+        }
+    }
+
+    res.solved = true;
+abort:
+
+    if (proto_count > 0) {
+        for (u32 i = 0; i < min_u32(proto_count, ARRAY_COUNT(res.prototype.points)); ++i) {
+            res.prototype.points[i] = proto[i].p;
+        }
+        res.prototype.len = proto_count;
+    } else {
+        if (s.len >= 1) {
+            res.prototype.points[0] = s.a.p;
+            if (s.len >= 2) {
+                res.prototype.points[1] = s.b.p;
+                if (s.len >= 3) {
+                    res.prototype.points[2] = s.c.p;
+                }
+            }
+        }
+        res.prototype.len = s.len;
+    }
+    res.pairs[0] = pair0;
+    res.pairs[1] = pair1;
+    res.num_pairs = num_pairs;
+    res.simplex = s;
+    scratch_end(scratch);
+    return res;
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+/* ========================== *
+ * GJK closest / deepest contact pairs (unused)
+ * ========================== */
+
+#if 0
 /* TODO: Update this function to only calculate contact pairs if colliding. Will allow for shortcut-style GJK.
  * `colliding` & `has_2nd_pair` become "num_pairs", where 0 = no collision. */
 struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, struct v2_array shape1)
@@ -442,44 +720,7 @@ abort:
     scratch_end(scratch);
     return res;
 }
-
+#endif
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
 
 /* ========================== *
  * GJK closest / deepest point (unused)

src/user.c

@@ -1001,15 +1001,15 @@ INTERNAL void user_update(void)
                     struct contact contact = ent->contacts[i];
                     {
                         u32 color = entity_has_prop(e0, ENTITY_PROP_PLAYER_CONTROLLED) ? RGBA_32_F(1, 0, 0, 0.50) : RGBA_32_F(0, 1, 1, 0.50);
-                        //struct v2 point = xform_mul_v2(e0_xf, contact.p0_local);
+                        struct v2 point = xform_mul_v2(e0_xf, contact.p0_local);
-                        struct v2 point = contact.p0_initial_world;
+                        //struct v2 point = contact.p0_initial_world;
                         point = xform_mul_v2(G.world_view, point);
                         draw_solid_circle(G.viewport_canvas, point, radius, color, 10);
                     }
                     {
                         u32 color = entity_has_prop(e1, ENTITY_PROP_PLAYER_CONTROLLED) ? RGBA_32_F(1, 0, 0, 0.50) : RGBA_32_F(0, 1, 1, 0.50);
-                        //struct v2 point = xform_mul_v2(e1_xf, contact.p1_local);
+                        struct v2 point = xform_mul_v2(e1_xf, contact.p1_local);
-                        struct v2 point = contact.p1_initial_world;
+                        //struct v2 point = contact.p1_initial_world;
                         point = xform_mul_v2(G.world_view, point);
                         draw_solid_circle(G.viewport_canvas, point, radius, color, 10);
                     }