gjk step tweaking in debug mode

src/common.h

@@ -278,6 +278,8 @@ void __asan_unpoison_memory_region(void const volatile *add, size_t);
 #define COLOR_GREEN     RGB_32(0   , 0xFF, 0   )
 #define COLOR_BLUE      RGB_32(0   , 0   , 0xFF)
 #define COLOR_YELLOW    RGB_32(0xFF, 0xFF, 0   )
+#define COLOR_TURQOISE  RGB_32(0,    0xFF, 0XFF)
+#define COLOR_PURPLE    RGB_32(0xFF, 0,    0XFF)
 
 /* Barrier */
 #if COMPILER_MSVC

src/entity.h

@@ -96,6 +96,7 @@ struct entity {
     struct entity_handle colliding_with;
     struct simplex simplex;
     struct v2 pen;
+    struct v2 spot;
 
 
 

src/game.c

@@ -26,6 +26,11 @@ GLOBAL struct {
     struct atomic_u64 prev_tick_id;
     struct world prev_tick;
     struct world tick;
+
+#if RTC
+    u32 gjk_steps;
+#endif
+
 } G = { 0 }, DEBUG_ALIAS(G, G_game);
 
 /* ========================== *
@@ -43,6 +48,10 @@ struct game_startup_receipt game_startup(struct mixer_startup_receipt *mixer_sr,
     (UNUSED)sheet_sr;
     (UNUSED)sound_sr;
 
+#if RTC
+    G.gjk_steps = U32_MAX;
+#endif
+
     /* Initialize game cmd storage */
     G.game_cmds_mutex = sys_mutex_alloc();
     G.game_cmds_arena = arena_alloc(GIGABYTE(64));
@@ -277,6 +286,13 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
                 spawn_test_entities();
             } break;
 
+#if RTC
+            /* Debug */
+            case GAME_CMD_KIND_SET_GJK_STEPS: {
+                G.gjk_steps = cmd.gjk_steps;
+            } break;
+#endif
+
             default: break;
         };
     }
@@ -729,6 +745,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 v2 spot = 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];
@@ -750,13 +767,13 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
                 };
             }
 
-            struct gjk_result res = gjk(e0_poly, e1_poly);
+            struct gjk_result res = gjk(e0_poly, e1_poly, G.gjk_steps);
             colliding = res.colliding;
             colliding_with = e1->handle;
             simplex = res.final_simplex;
-            pen = epa(e0_poly, e1_poly, simplex);
 
             if (colliding) {
+                pen = epa(e0_poly, e1_poly, simplex);
 #if 0
             /* Pen movement test */
                 {
@@ -766,6 +783,8 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
                     //e1->verlet_xform.og = v2_add(e1->verlet_xform.og, pen);
                 }
 #endif
+            } else {
+                spot = e0->spot;
             }
         }
 
@@ -773,6 +792,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
         e0->colliding_with = colliding_with;
         e0->simplex = simplex;
         e0->pen = pen;
+        e0->spot = spot;
     }
 #endif
 

src/game.h

@@ -24,6 +24,11 @@ enum game_cmd_kind {
     GAME_CMD_KIND_PAUSE,
     GAME_CMD_KIND_STEP,
 
+#if RTC
+    /* Debug */
+    GAME_CMD_KIND_SET_GJK_STEPS,
+#endif
+
     GAME_CMD_KIND_COUNT
 };
 
@@ -34,6 +39,10 @@ struct game_cmd {
     /* GAME_CMD_KIND_PLAYER_MOVE */
     struct v2 move_dir;
     struct v2 aim_pos;
+
+#if RTC
+    u32 gjk_steps;
+#endif
 };
 
 struct game_cmd_array {

src/user.c

@@ -68,6 +68,10 @@ GLOBAL struct {
     struct v2 viewport_center;
     struct v2 viewport_cursor;
     struct v2 world_cursor;
+
+#if RTC
+    u32 gjk_steps;
+#endif
 } G = { 0 }, DEBUG_ALIAS(G, G_user);
 
 /* ========================== *
@@ -95,7 +99,15 @@ GLOBAL READONLY enum user_bind_kind g_binds[SYS_BTN_COUNT] = {
     [SYS_BTN_MWHEELUP]          = USER_BIND_KIND_ZOOM_IN,
     [SYS_BTN_MWHEELDOWN]        = USER_BIND_KIND_ZOOM_OUT,
     [SYS_BTN_M3]                = USER_BIND_KIND_PAN,
-    [SYS_BTN_CTRL]          = USER_BIND_KIND_CTRL_TEST
+    [SYS_BTN_CTRL]              = USER_BIND_KIND_CTRL_TEST,
+
+#if RTC
+    /* Debug */
+
+    [SYS_BTN_FORWARD_SLASH]     = USER_BIND_KIND_RESET_GJK_STEPS,
+    [SYS_BTN_COMMA]             = USER_BIND_KIND_DECR_GJK_STEPS,
+    [SYS_BTN_PERIOD]            = USER_BIND_KIND_INCR_GJK_STEPS
+#endif
 };
 
 /* ========================== *
@@ -134,6 +146,9 @@ struct user_startup_receipt user_startup(struct work_startup_receipt *work_sr,
     G.viewport_bg_canvas = renderer_canvas_alloc();
     G.viewport_canvas = renderer_canvas_alloc();
     G.window = window;
+#if RTC
+    G.gjk_steps = U32_MAX;
+#endif
     sys_window_register_event_callback(G.window, &window_event_callback);
 
     G.user_thread = sys_thread_alloc(&user_thread_entry_point, NULL, STR("[P1] User thread"));
@@ -1007,7 +1022,7 @@ INTERNAL void user_update(void)
                         }
                     }
 
-                    u32 color = RGBA_32_F(0, 0, 0.75, 1);
+                    u32 color = RGBA_32_F(0, 0, 0.25, 1);
                     f32 thickness = 2;
                     (UNUSED)thickness;
 
@@ -1018,14 +1033,31 @@ INTERNAL void user_update(void)
                 }
 
                 /* Draw simplex */
-                if (colliding) {
+                {
                     f32 thickness = 2;
-                    u32 color = colliding ? COLOR_WHITE: COLOR_YELLOW;
+                    u32 line_color = colliding ? COLOR_WHITE: COLOR_YELLOW;
+                    u32 color_first = COLOR_RED;
+                    u32 color_second = COLOR_GREEN;
+                    u32 color_third = COLOR_TURQOISE;
+
                     struct simplex simplex = ent->simplex;
                     struct v2 simplex_points[] = { simplex.a, simplex.b, simplex.c };
                     for (u64 i = 0; i < ARRAY_COUNT(simplex_points); ++i) simplex_points[i] = xform_mul_v2(G.world_view, simplex_points[i]);
-                    struct v2_array simplex_array = { .count = ARRAY_COUNT(simplex_points), .points = simplex_points };
+                    struct v2_array simplex_array = { .count = simplex.len, .points = simplex_points };
-                    draw_solid_poly_line(G.viewport_canvas, simplex_array, true, thickness, color);
+
+                    if (simplex.len >= 1) {
+                        u32 color = simplex.len == 1 ? color_first : (simplex.len == 2 ? color_second : color_third);
+                        draw_solid_circle(G.viewport_canvas, simplex_array.points[0], thickness * 2, color, 10);
+                    }
+                    if (simplex.len >= 2) {
+                        u32 color = simplex.len == 2 ? color_first : color_second;
+                        draw_solid_poly_line(G.viewport_canvas, simplex_array, simplex.len > 2, thickness, line_color);
+                        draw_solid_circle(G.viewport_canvas, simplex_array.points[1], thickness * 2, color, 10);
+                    }
+                    if (simplex.len >= 3) {
+                        u32 color = color_first;
+                        draw_solid_circle(G.viewport_canvas, simplex_array.points[2], thickness * 2, color, 10);
+                    }
                 }
 
                 /* Draw pen */
@@ -1146,6 +1178,24 @@ INTERNAL void user_update(void)
                 .state = (G.bind_states[USER_BIND_KIND_FIRE].num_presses > 0 || G.bind_states[USER_BIND_KIND_FIRE].is_held) ? GAME_CMD_STATE_START : GAME_CMD_STATE_STOP
             });
         }
+
+#if RTC
+        /* Queue debug cmds */
+
+        /* Gjk steps */
+        {
+            i64 new_steps = (i64)G.gjk_steps;
+            new_steps += G.bind_states[USER_BIND_KIND_INCR_GJK_STEPS].num_presses_and_repeats;
+            new_steps -= G.bind_states[USER_BIND_KIND_DECR_GJK_STEPS].num_presses_and_repeats;
+            if (G.bind_states[USER_BIND_KIND_RESET_GJK_STEPS].num_presses_and_repeats > 0) new_steps = 0;
+            G.gjk_steps = (u32)clamp_i64(new_steps, 0, U32_MAX);
+            queue_game_cmd(scratch.arena, &cmd_list, (struct game_cmd) {
+                .kind = GAME_CMD_KIND_SET_GJK_STEPS,
+                .state = 1,
+                .gjk_steps = G.gjk_steps
+            });
+        }
+#endif
     }
 
     /* ---------------------------------------------------------------------- */
@@ -1202,6 +1252,11 @@ INTERNAL void user_update(void)
             draw_text(G.viewport_canvas, font, pos, string_format(temp.arena, STR("debug_camera: %F"), FMT_STR(G.debug_camera ? STR("true") : STR("false"))));
             pos.y += spacing;
 
+#if RTC
+            draw_text(G.viewport_canvas, font, pos, string_format(temp.arena, STR("gjk steps: %F"), FMT_UINT(G.gjk_steps)));
+            pos.y += spacing;
+#endif
+
             arena_temp_end(temp);
         }
 

src/user.h

@@ -34,6 +34,14 @@ enum user_bind_kind {
     USER_BIND_KIND_PAN,
     USER_BIND_KIND_CTRL_TEST,
 
+#if RTC
+    /* Debug */
+
+    USER_BIND_KIND_RESET_GJK_STEPS,
+    USER_BIND_KIND_INCR_GJK_STEPS,
+    USER_BIND_KIND_DECR_GJK_STEPS,
+#endif
+
     USER_BIND_KIND_COUNT
 };
 

src/util.c

@@ -45,7 +45,7 @@ struct string util_file_name_from_path(struct string path)
 
 /* TODO: Remove / move this */
 
-struct v2 poly_furthest_point(struct v2_array a, struct v2 dir)
+struct v2 poly_support(struct v2_array a, struct v2 dir)
 {
     struct v2 furthest = a.points[0];
     f32 furthest_dot = v2_dot(dir, furthest);
@@ -60,9 +60,9 @@ struct v2 poly_furthest_point(struct v2_array a, struct v2 dir)
     return furthest;
 }
 
-struct v2 poly_support_point(struct v2_array poly0, struct v2_array poly1, struct v2 dir)
+struct v2 menkowski_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)));
+    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)
@@ -72,6 +72,7 @@ struct v2 normal_towards_point(struct v2 start, struct v2 end, struct v2 p)
     return v2_mul(dir, sign);
 }
 
+#if 0
 struct gjk_result gjk(struct v2_array poly0, struct v2_array poly1)
 {
     struct v2 poly0_center = math_poly_center(poly0);
@@ -79,34 +80,36 @@ struct gjk_result gjk(struct v2_array poly0, struct v2_array 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.a = menkowski_point(poly0, poly1, dir);
-    s_len = 1;
+    s.len = 1;
 
-    dir = v2_norm(v2_neg(s.c));  /* Next point is towards origin */
+    dir = v2_norm(v2_neg(s.a));  /* Next point is towards origin */
 
     b32 colliding = false;
     while (true) {
         /* Determine support point */
-        struct v2 p = poly_support_point(poly0, poly1, dir);
+        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) {
+        if (s.len < 3) {
             /* Line case */
             /* Next dir is line normal towards origin */
-            if (s_len == 1) {
+            if (s.len == 1) {
-                s.b = p;
+                s.b = s.a;
-            } else if (s_len == 2) {
+                s.a = p;
+            } else if (s.len == 2) {
+                s.c = s.b;
+                s.b = s.a;
                 s.a = p;
             }
-            ++s_len;
+            ++s.len;
             dir = normal_towards_point(s.a, s.b, V2(0, 0));
         } else {
             /* Triangle case */
@@ -114,11 +117,8 @@ struct gjk_result gjk(struct v2_array poly0, struct v2_array poly1)
             s.b = s.a;
             s.a = p;
 
-            /* Ensure point is unique */
+            /* Ensure new 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)) {
-            if (v2_eq(s.a, s.b)
-                || v2_eq(s.b, s.c)
-                || v2_eq(s.a, s.c)) {
                 colliding = false;
                 break;
             }
@@ -127,14 +127,14 @@ struct gjk_result gjk(struct v2_array poly0, struct v2_array poly1)
 
             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 */
+                /* 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 must be in simplex */
+                    /* Point is in simplex */
                     colliding = true;
                     break;
                 }
@@ -142,16 +142,174 @@ struct gjk_result gjk(struct v2_array poly0, struct v2_array poly1)
         }
     }
 
-    return (struct gjk_result)
+    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;
+    u32 step = 0;
+
+    /* Simplex */
+    struct simplex s = { 0 };
+
+    /* First point is support point towards shape centers */
+    if (step++ >= max_steps) goto abort;
+    {
+        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 */
+    if (step++ >= max_steps) goto abort;
+    {
+        struct v2 dir = v2_norm(v2_neg(s.a));
+        struct v2 p = menkowski_point(poly0, poly1, dir);
+#if 0
+        if (!v2_eq(p, s.a)) {
+            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) {
+        while (true) {
+            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 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;
+
+            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;
+                }
+            }
+        }
+    }
+
+    abort:
+    return (struct gjk_result) {
+        .colliding = colliding,
+            .final_simplex = s
+    };
+}
+
+#endif
+
 struct v2 epa(struct v2_array poly0, struct v2_array poly1, struct simplex simplex)
 {
     struct temp_arena scratch = scratch_begin_no_conflict();
+    ASSERT(simplex.len == 3);
 
     struct v2 *proto = arena_dry_push(scratch.arena, struct v2);
     {
@@ -195,7 +353,7 @@ struct v2 epa(struct v2_array poly0, struct v2_array poly1, struct simplex simpl
 #if 1
         /* FIXME: Ensure convexity */
 
-        struct v2 p = poly_support_point(poly0, poly1, pen);
+        struct v2 p = menkowski_point(poly0, poly1, pen);
 
         /* Check unique */
         /* TODO: Better */
@@ -225,7 +383,7 @@ struct v2 epa(struct v2_array poly0, struct v2_array poly1, struct simplex simpl
         proto[pen_pe_index] = p;
 #else
         /* FIXME: Maintain convexity */
-        struct v2 p = poly_support_point(poly0, poly1, pen);
+        struct v2 p = menkowski_point(poly0, poly1, pen);
         if (v2_eq(p, ps) || v2_eq(p, pe)) {
             break;
         } else {
@@ -260,7 +418,7 @@ struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_
     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);
+        struct v2 p = menkowski_point(poly0, poly1, dir);
         if (res.count == 0 || !v2_eq(p, res.points[res.count - 1])) {
             *arena_push(arena, struct v2) = p;
             ++res.count;

src/util.h

@@ -187,6 +187,7 @@ INLINE void sleep_frame(sys_timestamp_t last_frame_time, f64 target_dt)
 /* TODO: Remove this */
 
 struct simplex {
+    u32 len;
     struct v2 a, b, c;
 };
 
@@ -195,10 +196,10 @@ struct gjk_result {
     struct simplex final_simplex;
 };
 
-struct v2 poly_furthest_point(struct v2_array a, struct v2 dir);
+struct v2 poly_support(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 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 gjk_result gjk(struct v2_array poly0, struct v2_array poly1);
+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);
 i32 poly_get_winding_order(struct v2_array poly);