better direction gjk testing (still bugged since edge is furthest in velocity rather than closest projected, even though point is now correctly projected from end of velocity ray)

src/entity.h

@@ -97,7 +97,8 @@ struct entity {
     struct entity_handle colliding_with;
     struct v2 point;
     struct gjk_simplex simplex;
-
+    struct v2 pendir;
+    b32 velocity_intersects;
 
 
 

src/game.c

@@ -122,9 +122,12 @@ INTERNAL void spawn_test_entities(void)
     /* Player */
     struct entity *player_ent;
     {
-        struct v2 pos = V2(-1, -1);
+        //struct v2 pos = V2(-1, -1);
+        //struct v2 pos = V2(1.1230469346046448864129274625156, -1);  /* Touching right side of box */
+        struct v2 pos = V2(1.1230469346046448864129274625156 - 0.0001, -1);  /* Touching right side of box */
         struct v2 size = V2(1, 1);
         f32 r = PI / 4;
+        //f32 r = 0;
         f32 skew = 0;
 
         struct entity *e = entity_alloc(root);
@@ -644,8 +647,9 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
 
         /* Verlet integration */
         struct v2 tick_velocity = v2_sub(xf.og, ent->verlet_xform.og);
+        tick_velocity = v2_add(tick_velocity, v2_mul(acceleration, dt));
         ent->verlet_xform = xf;
-        xf.og = v2_add(xf.og, v2_add(tick_velocity, v2_mul(acceleration, dt)));
+        xf.og = v2_add(xf.og, tick_velocity);
         entity_set_xform(ent, xf);
     }
 
@@ -764,6 +768,8 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
         struct v2 point1 = V2(0, 0);
         struct entity *colliding_with = entity_nil();
         struct gjk_simplex simplex = { 0 };
+        struct v2 pendir = { 0 };
+        b32 velocity_intersects = false;
         for (u64 e1_index = 0; e1_index < store->reserved; ++e1_index) {
             struct entity *e1 = &store->entities[e1_index];
             if (e1 == e0) continue;
@@ -785,13 +791,19 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
             }
 
 #if 0
-            struct v2 e0_velocity = v2_div(v2_sub(e0_xf.og, e0->verlet_xform.og), dt);
+            struct v2 e0_velocity = v2_sub(e0_xf.og, e0->verlet_xform.og);
-            struct v2 e1_velocity = v2_div(v2_sub(e1_xf.og, e1->verlet_xform.og), dt);
+            struct v2 e1_velocity = v2_sub(e1_xf.og, e1->verlet_xform.og);
-            struct v2 pendir = v2_sub(e0_velocity, e1_velocity);  /* What if both point in opposite directions? */
+            pendir = v2_sub(e0_velocity, e1_velocity);  /* TODO: What if both point in opposite directions? */
-            pendir = v2_norm(pendir);
+            //pendir = v2_norm(pendir);
 #else
-            struct v2 pendir = V2(0, 0);
+            //pendir = V2(0, 0);
-            //struct v2 pendir = V2(0, 99999);
+            //pendir = V2(-0.25, -1);
+            pendir = v2_mul(v2_norm(V2(-0.25, -1)), 0.2);
+            //pendir = V2(0, 99999);
+            //pendir = V2(-99999999999, -99999999999);
+            //pendir = V2(-0.0000794729349, -1.00000);
+            //pendir = V2(-0.0001794729349, -1.00000);
+            //pendir = V2(-0.1794729349, -1.00000);
 #endif
 
             struct gjk_extended_result res = gjk_extended(e0_poly, e1_poly, pendir);
@@ -801,20 +813,21 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
             point1 = res.p1;
             colliding_with = e1;
             simplex = res.simplex;
+            velocity_intersects = res.velocity_intersects;
 
             if (colliding) {
 
-#if 1
+#if 0
                 struct v2 pen = v2_sub(point1, point0);
                 /* Pen movement test */
#if 0
-                f32 epsilon = 0.000;
                pen = v2_add(pen, v2_mul(v2_norm(pen), epsilon));
+                f32 epsilon = 0.000100;
                pen = v2_add(pen, v2_mul(v2_norm(pen), epsilon));
 #endif
 
                 struct xform xf = e0_xf;
                 xf.og = v2_add(xf.og, pen);
                 entity_set_xform(e0, xf);
-                //e0->verlet_xform.og = v2_add(e0->verlet_xform.og, pen);
+                e0->verlet_xform.og = v2_add(e0->verlet_xform.og, pen);
-                e0->verlet_xform.og = xf.og;
+                //e0->verlet_xform.og = xf.og;
 #endif
             }
         }
@@ -823,6 +836,8 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
         e0->colliding_with = colliding_with->handle;
         e0->point = point0;
         e0->simplex = simplex;
+        e0->pendir = pendir;
+        e0->velocity_intersects = velocity_intersects;
 
         if (colliding_with->valid) {
             colliding_with->colliding = colliding;

src/gjk.c

@@ -86,6 +86,7 @@ b32 gjk_boolean(struct v2_array shape0, struct v2_array shape1)
     return false;
 }
 
+#if 0
 struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1, struct v2 penetration_dir)
 {
     struct temp_arena scratch = scratch_begin_no_conflict();
@@ -129,7 +130,8 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array
                 /* 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) {
+                f32 dot = v2_dot(dir, m.p);
+                if (dot <= 0) {
                     /* New point did not cross origin, collision impossible */
                     break;
                 }
@@ -142,13 +144,16 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array
                 DBGSTEP;
                 dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), v2_neg(v2_sub(s.c.p, s.a.p)));  /* Normal of ab pointing away from c */
                 struct v2 a_to_origin = v2_neg(s.a.p);
-                if (v2_dot(dir, a_to_origin) >= 0) {
+
+                dot = v2_dot(dir, a_to_origin);
+                if (dot >= 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(v2_sub(s.c.p, s.a.p), v2_neg(v2_sub(s.b.p, s.a.p)));  /* Normal of ac pointing away from b */
-                    if (v2_dot(dir, a_to_origin) >= 0) {
+                    dot = v2_dot(dir, a_to_origin);
+                    if (dot >= 0) {
                         /* Point is in region ac, remove b from simplex */
                         s.b = s.c;
                         s.len = 2;
@@ -237,6 +242,7 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array
                 proto_count = 3;
             }
 
+            const f32 pen_epsilon = 0.00001;
             while (true) {
                 DBGSTEP;
                 struct v2 pen = V2(0, 0);
@@ -255,15 +261,28 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array
                     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)));
+                    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 = pd;
                         pen_len_sq = pd_len_sq;
+
+#if 0
+                        pen = pd;
+#else
+                        if (pd_len_sq > pen_epsilon) {
+                            pen = pd;
+                        } else {
+                            u32 p_next_index = pe_index < proto_count - 1 ? pe_index + 1 : 0;
+                            struct v2 p_next = proto[p_next_index].p;
+                            pen = v2_neg(v2_perp_towards_dir(vse, v2_sub(p_next, ps)));
+                        }
+#endif
                     }
                 }
 
@@ -423,6 +442,473 @@ abort:
     return res;
 }
 
+#else
+
+struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1, struct v2 penetration_dir)
+{
+    struct temp_arena scratch = scratch_begin_no_conflict();
+    struct gjk_extended_result res = { 0 };
+
+    /* TODO: Verify epsilon */
+    f32 epsilon = 0.0000100;
+    b32 use_penetration_dir = false;
+    struct gjk_simplex s = { 0 };
+    b32 colliding = false;
+    struct v2 shape0_p = { 0 };
+    struct v2 shape1_p = { 0 };
+    b32 velocity_intersects = 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) {
+            /* Second point is support point towards origin */
+            if (s.len == 1) {
+                DBGSTEP;
+                m = menkowski_point_extended(shape0, shape1, v2_neg(s.a.p));
+                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) {
+                    /* 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;
+
+                DBGSTEP;
+                if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) < epsilon) {
+                    /* 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) {
+                    /* ac lies on origin */
+                    s.b = s.c;
+                    s.len = 2;
+                    break;
+                }
+            }
+
+            DBGSTEP;
+            /* 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) {
+                case 0: {  /* 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 ab */
+                    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 ab */
+                    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) {
+        use_penetration_dir = !v2_eq(penetration_dir, V2(0, 0));
+        if (use_penetration_dir) {
+
+            /* ========================== *
+             * Move simplex towards penetration dir
+             * ========================== */
+
+            while (true) {
+                /* Second point is support point towards penetration_dir */
+                if (s.len == 1) {
+                    DBGSTEP;
+                    dir = v2_sub(v2_mul(penetration_dir, v2_dot(penetration_dir, s.a.p) / v2_dot(penetration_dir, penetration_dir)), s.a.p);
+                    m = menkowski_point_extended(shape0, shape1, dir);
+                    if (v2_eq(m.p, s.a.p)) {
+                        break;
+                    }
+
+                    if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) < epsilon) {
+                        /* ab lies on origin */
+                        break;
+                    }
+
+                    s.b = s.a;
+                    s.a = m;
+                    s.len = 2;
+                    /* Third point is support point in direction of line normal towards `a` projected onto penetration_dir */
+                    dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), penetration_dir);
+                }
+
+                if (s.len == 2) {
+                    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 already on the current line */
+                        break;
+                    }
+                    s.c = s.b;
+                    s.b = s.a;
+                    s.a = m;
+                    s.len = 3;
+
+                    DBGSTEP;
+                    if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) < epsilon) {
+                        /* 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) {
+                        /* ac lies on origin */
+                        s.b = s.c;
+                        s.len = 2;
+                        break;
+                    }
+                }
+
+                DBGSTEP;
+#if 0
+                i32 a_wedgesign = math_fsign(v2_wedge(penetration_dir, s.a.p));
+                i32 b_wedgesign = math_fsign(v2_wedge(penetration_dir, s.b.p));
+                i32 c_wedgesign = math_fsign(v2_wedge(penetration_dir, s.c.p));
+                if (a_wedgesign != b_wedgesign) {
+                    /* Remove c */
+                    dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), penetration_dir);
+                    s.len = 2;
+                } else if (a_wedgesign != c_wedgesign) {
+                    /* Remove b */
+                    dir = v2_perp_towards_dir(v2_sub(s.c.p, s.a.p), penetration_dir);
+                    s.b = s.c;
+                    s.len = 2;
+                } else {
+                    /* Remove a */
+                    dir = v2_perp_towards_dir(v2_sub(s.c.p, s.b.p), penetration_dir);
+                    s.a = s.b;
+                    s.b = s.c;
+                    s.len = 2;
+                }
+#else
+
+                //struct v2 vab = v2_sub(s.b.p, s.a.p);
+
+
+                f32 a_dot = v2_dot(penetration_dir, s.a.p);
+                f32 a_wedge = v2_wedge(penetration_dir, s.a.p);
+
+                f32 b_dot = v2_dot(penetration_dir, s.b.p);
+                f32 b_wedge = v2_wedge(penetration_dir, s.b.p);
+
+                f32 c_dot = v2_dot(penetration_dir, s.c.p);
+                f32 c_wedge = v2_wedge(penetration_dir, s.c.p);
+
+
+                i32 code = 0;  /* 0 = ab, 1 = ac, 2 = bc */
+
+#if 0
+                f32 highest_intercept;
+                if (math_fsign(a_wedge) != math_fsign(b_wedge)) {
+                    highest_intercept = ((y2 - y1) / (x2 - x1)) * (-x1) + y1;
+                }
+                if (math_fsign(a_wedge) != math_fsign(c_wedge)) {
+                    f32 intercept = ((y2 - y1) / (x2 - x1)) * (-x1) + y1;
+                    if (intercept > highest_intercept) {
+                        code = 1;
+                        highest_intercept = intercept;
+                    }
+                }
+                if (math_fsign(b_wedge) != math_fsign(c_wedge)) {
+                    f32 intercept = ((y2 - y1) / (x2 - x1)) * (-x1) + y1;
+                    if (intercept > highest_intercept) {
+                        code = 2;
+                        highest_intercept = intercept;
+                    }
+                }
+#else
+                f32 highest_intercept = -F32_INFINITY;
+                if (math_fsign(a_wedge) != math_fsign(b_wedge)) {
+                    highest_intercept = ((b_dot - a_dot) / (b_wedge - a_wedge)) * (-a_wedge) + a_dot;
+                }
+                if (math_fsign(a_wedge) != math_fsign(c_wedge)) {
+                    f32 intercept = ((c_dot - a_dot) / (c_wedge - a_wedge)) * (-a_wedge) + a_dot;
+                    if (intercept > highest_intercept) {
+                        code = 1;
+                        highest_intercept = intercept;
+                    }
+                }
+                if (math_fsign(b_wedge) != math_fsign(c_wedge)) {
+                    f32 intercept = ((c_dot - b_dot) / (c_wedge - b_wedge)) * (-b_wedge) + b_dot;
+                    if (intercept > highest_intercept) {
+                        code = 2;
+                        highest_intercept = intercept;
+                    }
+                }
+#endif
+
+                switch (code) {
+                    case 0: {  /* ab is furthest, remove c */
+                        dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), penetration_dir);
+                        s.len = 2;
+                    } break;
+
+                    case 1: {  /* ac is furthest, remove b */
+                        dir = v2_perp_towards_dir(v2_sub(s.c.p, s.a.p), penetration_dir);
+                        s.b = s.c;
+                        s.len = 2;
+                    } break;
+
+                    case 2: {  /* bc is furthest, remove a */
+                        dir = v2_perp_towards_dir(v2_sub(s.c.p, s.b.p), penetration_dir);
+                        s.a = s.b;
+                        s.b = s.c;
+                        s.len = 2;
+                    } break;
+                }
+#endif
+            }
+        } else {
+
+            /* ========================== *
+             * Expand simplex towards closest edge to origin inside menkowski (EPA)
+             * ========================== */
+
+            struct gjk_menkowski_point *proto = arena_dry_push(scratch.arena, struct gjk_menkowski_point);
+            u32 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) {
+                DBGSTEP;
+                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 */
+                {
+                    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 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] = m;
+            }
+        }
+    }
+
+    /* Resolve points */
+#if 0
+    DBGSTEP;
+    if (s.len == 1) {
+        shape0_p = s.a.p0;
+        shape1_p = s.a.p1;
+    } else {
+        ASSERT(s.len == 2);
+        /* FIXME: Winding order dependent? */
+        f32 ratio;
+        if (use_penetration_dir) {
+            /* Determine ratio between edge a & b that penetration dir intersection lies */
+            f32 wedgea = math_fabs(v2_wedge(penetration_dir, s.a.p));
+            f32 wedgeb = math_fabs(v2_wedge(penetration_dir, s.b.p));
+            ratio = wedgea / (wedgea + wedgeb);
+        } else {
+            /* 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 */
+        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);
+        /* 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
+    DBGSTEP;
+    if (s.len == 1) {
+        shape0_p = s.a.p0;
+        shape1_p = s.a.p1;
+    } else {
+        ASSERT(s.len == 2);
+        /* FIXME: Winding order dependent? */
+        f32 ratio;
+        if (use_penetration_dir) {
+            struct v2 vao = v2_neg(s.a.p);
+            struct v2 vab = v2_sub(s.b.p, s.a.p);
+
+            f32 t1 = v2_wedge(penetration_dir, vao) / v2_wedge(penetration_dir, vab);
+            f32 t2 = v2_wedge(vab, vao) / v2_wedge(penetration_dir, vab);
+
+            velocity_intersects = 0 <= t1 && 0 <= t2 && t2 <= 1;
+
+            if (velocity_intersects) {
+                /* Ratio between edge a & b that velocity intersection lies */
+                ratio = t1;
+            } else {
+                /* Ratio between edge a & b that projected velocity lies */
+                struct v2 vap = v2_sub(penetration_dir, s.a.p);
+                ratio = clamp_f32(v2_dot(vab, vap) / v2_dot(vab, vab), 0, 1);
+            }
+        } else {
+            /* 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 */
+        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);
+        /* 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:
+    res.colliding = colliding;
+    res.p0 = shape0_p;
+    res.p1 = shape1_p;
+    res.simplex = s;
+    res.velocity_intersects = velocity_intersects;
+    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)
 {

src/gjk.h

@@ -24,6 +24,7 @@ struct gjk_extended_result {
 
     /* For debugging */
     struct gjk_simplex simplex;
+    b32 velocity_intersects;
 };
 
 /* Returns simple true or false indicating shape collision */
@@ -34,7 +35,8 @@ b32 gjk_boolean(struct v2_array shape0, struct v2_array shape1);
  * 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. */
+ * supplied direction.
+ */
 struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1, struct v2 penetration_dir);
 
 struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1);

src/user.c

@@ -1029,19 +1029,20 @@ INTERNAL void user_update(void)
                     draw_solid_poly_line(G.viewport_canvas, m, true, thickness, color);
                     //draw_solid_poly(G.viewport_canvas, m, color);
                 }
-#endif
 
-                /* Draw point */
+                /* Draw pendir */
-                {
+                if (entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED)) {
-                    u32 color = COLOR_PURPLE;
+                    f32 thickness = 2;
-                    f32 thickness = 5;
+                    u32 color = ent->velocity_intersects ? COLOR_RED : COLOR_YELLOW;
-                    struct v2 point = xform_mul_v2(G.world_view, ent->point);
+
-                    draw_solid_circle(G.viewport_canvas, point, thickness, color, 10);
+                    struct v2 start = G.world_view.og;
+                    struct v2 ray = xform_basis_mul_v2(G.world_view, ent->pendir);
+
+                    draw_solid_arrow_ray(G.viewport_canvas, start, ray, thickness, thickness * 4, color);
                 }
 
-#if 1
                 /* Draw simplex */
-                {
+                if (entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED)) {
                     f32 thickness = 2;
                     u32 line_color = colliding ? COLOR_WHITE: COLOR_YELLOW;
                     u32 color_first = RGBA_32_F(1, 0, 0, 0.75);
@@ -1071,18 +1072,13 @@ INTERNAL void user_update(void)
                 }
 #endif
 
-#if 0
+                /* Draw point */
-                /* Draw pen */
+                {
-                if (colliding) {
+                    u32 color = entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED) ? COLOR_PURPLE : COLOR_TURQOISE;
-                    f32 thickness = 2;
+                    f32 thickness = 5;
-                    u32 color = COLOR_RED;
+                    struct v2 point = xform_mul_v2(G.world_view, ent->point);
-
+                    draw_solid_circle(G.viewport_canvas, point, thickness, color, 10);
-                    struct v2 start = G.world_view.og;
-                    struct v2 ray = xform_basis_mul_v2(G.world_view, ent->pen);
-
-                    draw_solid_arrow_ray(G.viewport_canvas, start, ray, thickness, thickness * 4, color);
                 }
-#endif
             }
 
             /* Draw hierarchy */