gjk contact pair testing

src/entity.h

@@ -93,12 +93,15 @@ struct entity {
     /* TODO: Remove this (testing) */
     b32 colliding;
     struct entity_handle colliding_with;
-    struct v2 point;
     struct gjk_simplex simplex;
     struct gjk_prototype prototype;
     struct v2 pendir;
     b32 solved;
 
+    struct v2 point0;
+    struct v2 point1;
+    b32 has_2nd_point;
+
     b32 test_torque_applied;
     b32 test_collided;
 

src/game.c

@@ -128,8 +128,8 @@ INTERNAL void spawn_test_entities(void)
         struct v2 size = V2(1, 1);
         //f32 r = PI / 4;
         //f32 r = PI / 3;
-        //f32 r = PI / 2;
+        f32 r = PI / 2;
-        f32 r = 0;
+        //f32 r = 0;
         f32 skew = 0;
 
         struct entity *e = entity_alloc(root);
@@ -910,8 +910,11 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
         }
 
         b32 colliding = false;
-        struct v2 p0 = V2(0, 0);
+        struct v2 pair0p0 = V2(0, 0);
-        struct v2 p1 = V2(0, 0);
+        struct v2 pair0p1 = V2(0, 0);
+        struct v2 pair1p0 = V2(0, 0);
+        struct v2 pair1p1 = V2(0, 0);
+        b32 has_2nd_pair = false;
         struct entity *colliding_with = entity_nil();
         struct gjk_simplex simplex = { 0 };
         struct gjk_prototype prototype = { 0 };
@@ -938,11 +941,14 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
                 };
             }
 
-            struct gjk_extended_result res = gjk_extended(e0_poly, e1_poly);
+            struct gjk_contact_points_result res = gjk_contact_points(e0_poly, e1_poly);
 
             colliding = res.colliding;
-            p0 = res.p0;
+            pair0p0 = res.pair0p0;
-            p1 = res.p1;
+            pair0p1 = res.pair0p1;
+            pair1p0 = res.pair1p0;
+            pair1p1 = res.pair1p1;
+            has_2nd_pair = res.has_2nd_pair;
             colliding_with = e1;
             simplex = res.simplex;
             prototype = res.prototype;
@@ -951,7 +957,9 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
 
         e0->colliding = colliding;
         e0->colliding_with = colliding_with->handle;
-        e0->point = p0;
+        e0->point0 = pair0p0;
+        e0->point1 = pair1p0;
+        e0->has_2nd_point = has_2nd_pair;
         e0->simplex = simplex;
         e0->prototype = prototype;
         e0->pendir = velocity;
@@ -960,11 +968,13 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
         if (colliding_with->valid) {
             colliding_with->colliding = colliding;
             colliding_with->colliding_with = e0->handle;
-            colliding_with->point = p1;
+            colliding_with->point0 = pair0p1;
+            colliding_with->point1 = pair1p1;
+            colliding_with->has_2nd_point = has_2nd_pair;
         }
 
         {
-#if 1
+#if 0
             if (colliding) {
                 struct entity *e1 = colliding_with;
                 struct xform e1_xf = entity_get_xform(e1);

src/gjk.c

@@ -86,13 +86,423 @@ INTERNAL struct gjk_menkowski_point menkowski_point_extended(struct v2_array pol
     return res;
 }
 
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+/* ========================== *
+ * Debug functions
+ * TODO: Remove these
+ * ========================== */
+
+/* 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 = menkowski_point_extended(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;
+}
+
+/* TODO: Remove this (debugging) */
+struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_array poly1)
+{
+    struct v2_array res = { .points = arena_dry_push(arena, struct v2) };
+    for (u64 i = 0; i < poly0.count; ++i) {
+        struct v2 p0 = poly0.points[i];
+        for (u64 j = 0; j < poly1.count; ++j) {
+            struct v2 p1 = poly1.points[j];
+            *arena_push(arena, struct v2) = v2_sub(p0, p1);
+            ++res.count;
+        }
+    }
+
+    return res;
+}
+
+
+
+
+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 */
+    const f32 epsilon = 0.0000100;
+    struct gjk_simplex s = { 0 };
+    b32 colliding = false;
+    struct gjk_menkowski_point *proto = NULL;
+    u32 proto_count = 0;
+
+    struct v2 pair0p0 = { 0 };
+    struct v2 pair0p1 = { 0 };
+    struct v2 pair1p0 = { 0 };
+    struct v2 pair1p1 = { 0 };
+    b32 has_2nd_pair = false;
+
+#if GJK_DEBUG
+    u32 dbg_step = 0;
+#endif
+
+    /* ========================== *
+     * GJK collision check
+     * Construct encapsulating simplex OR closest feature if not colliding
+     * ========================== */
+
+    struct v2 dir = { 0 };
+    struct gjk_menkowski_point m = { 0 };
+    /* Determine encapsulating simplex if colliding, or closest edge / point to origin on simplex */
+    {
+        /* 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;
+
+        while (!colliding) {
+            if (s.len == 1) {
+                /* Second point is support point towards origin */
+                dir = v2_neg(s.a.p);
+
+                DBGSTEP;
+                m = menkowski_point_extended(shape0, shape1, dir);
+                if (v2_eq(m.p, s.a.p)) {
+                    /* Point is the same */
+                    break;
+                }
+                s.b = s.a;
+                s.a = m;
+                s.len = 2;
+                if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) < epsilon) {
+                    /* New ab lies on origin */
+                    break;
+                }
+
+                /* 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));
+            }
+
+            {
+                DBGSTEP;
+                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))) < epsilon) {
+                    /* New point is on existing line ab */
+                    break;
+                }
+                s.c = s.b;
+                s.b = s.a;
+                s.a = m;
+                s.len = 3;
+                if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) < epsilon) {
+                    /* New ab lies on origin */
+                    s.len = 2;
+                    break;
+                } else if (math_fabs(v2_wedge(v2_sub(s.c.p, s.a.p), v2_neg(s.a.p))) < epsilon) {
+                    /* New ac lies on origin */
+                    s.b = s.c;
+                    s.len = 2;
+                    break;
+                }
+            }
+
+            /* Determine voronoi region of the simplex in which the origin lies */
+            i32 voronoi_mask = 0;
+            struct v2 vab = v2_sub(s.b.p, s.a.p);
+            struct v2 vac = v2_sub(s.c.p, s.a.p);
+            struct v2 vbc = v2_sub(s.c.p, s.b.p);
+            struct v2 rab_dir = v2_perp_towards_dir(vab, v2_neg(vac));
+            struct v2 rac_dir = v2_perp_towards_dir(vac, v2_neg(vab));
+            struct v2 rbc_dir = v2_perp_towards_dir(vbc, vab);
+            voronoi_mask |= (v2_dot(rab_dir, v2_neg(s.a.p)) >= 0) << 0;  /* Regions ab, a, and b*/
+            voronoi_mask |= (v2_dot(rac_dir, v2_neg(s.a.p)) >= 0) << 1;  /* Regions ac, a, and c */
+            voronoi_mask |= (v2_dot(rbc_dir, v2_neg(s.b.p)) >= 0) << 2;  /* Regions bc, b, and c */
+            /* Remove point or edge and determine next direction based on voronoi region */
+            switch (voronoi_mask) {
+                default:
+                {  /* No region, must be in simplex */
+                    colliding = true;
+                } break;
+                case 1:
+                {  /* Region ab, remove c */
+                    dir = rab_dir;  /* Next third point is in direction of region ab */
+                    s.len = 2;
+                } break;
+                case 2:
+                {  /* Region ac, remove b */
+                    dir = rac_dir;  /* Next third point is in direction of region ac */
+                    s.b = s.c;
+                    s.len = 2;
+                } break;
+                case 4:
+                {  /* Region bc, remove a */
+                    dir = rbc_dir;  /* Next third point is in direction of region bc */
+                    s.a = s.b;
+                    s.b = s.c;
+                    s.len = 2;
+                } break;
+                case 3:
+                {  /* Region a, remove bc */
+                    s.len = 1;
+                } break;
+                case 5:
+                {  /* Region b, remove ac */
+                    s.a = s.b;
+                    s.len = 1;
+                } break;
+                case 6:
+                {  /* Region c, remove ab */
+                    s.a = s.c;
+                    s.len = 1;
+                } 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;
+        }
+
+        /* ========================== *
+         * Expand simplex towards closest edge to origin inside menkowski
+         * ========================== */
+
+        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) {
+        pair0p0 = s.a.p0;
+        pair0p1 = s.a.p1;
+    } else if (s.len == 2) {
+
+        /* TODO: Epsilon */
+        if (v2_eq(s.a.p0, s.b.p0) || v2_eq(s.a.p1, s.b.p1)) {
+            /* FIXME: Winding order dependent? */
+            ASSERT(s.len == 2);
+            f32 ratio;
+            {
+                /* Determine ratio between edge a & b that projected origin lies */
+                struct v2 vab = v2_sub(s.b.p, s.a.p);
+                struct v2 vao = v2_neg(s.a.p);
+                ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1);
+            }
+            /* Shape 0 */
+            pair0p0 = v2_sub(s.b.p0, s.a.p0);
+            pair0p0 = v2_mul(pair0p0, ratio);
+            pair0p0 = v2_add(pair0p0, s.a.p0);
+            /* Shape 1 */
+            pair0p1 = v2_sub(s.b.p1, s.a.p1);
+            pair0p1 = v2_mul(pair0p1, ratio);
+            pair0p1 = v2_add(pair0p1, s.a.p1);
+        } else {
+            /* Face -> face, need 2nd pair */
+
+            has_2nd_pair = true;
+
+            /* TODO: Rename abcd variables */
+
+            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);
+
+            pair0p0 = v2_add(a, v2_mul(vab, clamp_f32(v2_dot(vab, vac) * inv_vab_len_sq, 0, 1)));
+            pair1p0 = v2_add(a, v2_mul(vab, clamp_f32(v2_dot(vab, vad) * inv_vab_len_sq, 0, 1)));
+            pair0p1 = v2_add(c, v2_mul(vcd, clamp_f32(v2_dot(vcd, vca) * inv_vcd_len_sq, 0, 1)));
+            pair1p1 = v2_add(c, v2_mul(vcd, clamp_f32(v2_dot(vcd, vcb) * inv_vcd_len_sq, 0, 1)));
+        }
+    }
+
+    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.colliding = colliding;
+    res.pair0p0 = pair0p0;
+    res.pair0p1 = pair0p1;
+    res.pair1p0 = pair1p0;
+    res.pair1p1 = pair1p1;
+    res.has_2nd_pair = has_2nd_pair;
+    res.simplex = s;
+    scratch_end(scratch);
+    return res;
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+/* ========================== *
+ * GJK closest / deepest point (unused)
+ * ========================== */
+
+#if 0
 struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1)
 {
     struct temp_arena scratch = scratch_begin_no_conflict();
     struct gjk_extended_result res = { 0 };
 
     /* TODO: Verify epsilon */
-    f32 epsilon = 0.0000100;
+    const f32 epsilon = 0.0000100;
     struct gjk_simplex s = { 0 };
     b32 colliding = false;
     struct v2 shape0_p = { 0 };
@@ -347,39 +757,7 @@ abort:
     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 = menkowski_point_extended(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;
-}
-
-/* TODO: Remove this (debugging) */
-struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_array poly1)
-{
-    struct v2_array res = { .points = arena_dry_push(arena, struct v2) };
-    for (u64 i = 0; i < poly0.count; ++i) {
-        struct v2 p0 = poly0.points[i];
-        for (u64 j = 0; j < poly1.count; ++j) {
-            struct v2 p1 = poly1.points[j];
-            *arena_push(arena, struct v2) = v2_sub(p0, p1);
-            ++res.count;
-        }
-    }
-
-    return res;
-}
 
 
 

src/gjk.h

@@ -22,6 +22,27 @@ struct gjk_simplex {
 b32 gjk_boolean(struct v2_array shape0, struct v2_array shape1);
 
 struct gjk_prototype { struct v2 points[64]; u32 len; };
+struct gjk_contact_points_result {
+    b32 colliding;
+
+
+    struct v2 pair0p0, pair0p1;
+    struct v2 pair1p0, pair1p1;
+    b32 has_2nd_pair;
+
+    /* For debugging */
+    b32 solved;
+    struct gjk_simplex simplex;
+    struct gjk_prototype prototype;
+};
+
+struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, struct v2_array shape1);
+
+struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1);
+
+struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_array poly1);
+
+#if 0
 struct gjk_extended_result {
     b32 colliding;
     struct v2 p0, p1;  /* Closest points (or penetrating points if colliding) on each shape */
@@ -32,18 +53,9 @@ struct gjk_extended_result {
     struct gjk_prototype prototype;
 };
 
-/* Returns shape whether shapes are colliding well as closest / penetrating points on each shape.
+/* Returns whether shapes are colliding well as closest / penetrating points on each shape.  */
- *
- * If shapes are colliding and `penetration_dir` is non-zero, the shortest
- * direction to resolve the collision will be used to calculate penetrating
- * points. Otherwise, the penetrating points will be calculated using the
- * supplied direction.
- */
 struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1);
-
+#endif
-struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1);
-
-struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_array poly1);
 
 #if 0
 

src/user.c

@@ -1113,13 +1113,20 @@ INTERNAL void user_update(void)
                 }
 #endif
 
-                /* Draw point */
+                /* Draw points */
+                {
+                    f32 radius = 5;
                     {
                         u32 color = entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED) ? RGBA_32_F(1, 0, 0, 0.75) : RGBA_32_F(0, 1, 1, 0.75);
-                    f32 radius = 5;
+                        struct v2 point = xform_mul_v2(G.world_view, ent->point0);
-                    struct v2 point = xform_mul_v2(G.world_view, ent->point);
                         draw_solid_circle(G.viewport_canvas, point, radius, color, 10);
                     }
+                    if (ent->has_2nd_point) {
+                        u32 color = entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED) ? RGBA_32_F(1, 1, 0, 0.75) : RGBA_32_F(1, 0, 1, 0.75);
+                        struct v2 point = xform_mul_v2(G.world_view, ent->point1);
+                        draw_solid_circle(G.viewport_canvas, point, radius, color, 10);
+                    }
+                }
             }
 
             /* Draw hierarchy */