From 1849f1143ac13b4dc0ade913b86d585a1916e017 Mon Sep 17 00:00:00 2001 From: jacob Date: Fri, 10 Jan 2025 11:37:27 -0600 Subject: [PATCH] de-duplicate collider.c gjk & epa logic --- src/collider.c | 849 ++++++++++++++++++++----------------------------- src/collider.h | 14 +- src/entity.h | 3 +- src/game.c | 96 +++--- 4 files changed, 409 insertions(+), 553 deletions(-) diff --git a/src/collider.c b/src/collider.c index 10897dc4..0f9911b1 100644 --- a/src/collider.c +++ b/src/collider.c @@ -3,6 +3,15 @@ #include "arena.h" #include "scratch.h" +/* How close can non-overlapping shapes be before collision is considered */ +#define COLLISION_TOLERANCE 0.005f + +/* NOTE: Should always be less than tolerance, since colliding = true if origin is within this distance. */ +#define MIN_UNIQUE_PT_DIST_SQ (0.001f * 0.001f) + +/* To prevent extremely large prototypes when origin is in exact center of rounded feature */ +#define MAX_EPA_ITERATIONS 64 + #if COLLIDER_DEBUG u32 collider_debug_steps = U32_MAX; //u32 collider_debug_steps = 1000000; @@ -87,192 +96,211 @@ INTERNAL struct collider_menkowski_point get_menkowski_point(struct collider_sha return res; } -struct collider_collision_points_result collider_collision_points(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1) +/* ========================== * + * GJK + * + * Determine simplex in menkowksi difference that encapsulates origin if shapes + * overlap, or closest edge / point to origin on menkowski difference if they + * do not. + * ========================== */ + +struct gjk_result { + struct collider_menkowski_simplex simplex; + struct v2 final_dir; + + /* If true, simplex represents triangle inside of menkowski difference + * encapsulating the origin. If false, simplex represents the closest + * feature on menkowski difference to the origin. */ + b32 overlapping; + +#if COLLIDER_DEBUG + u32 dbg_step; +#endif +}; + +#if COLLIDER_DEBUG +INTERNAL struct gjk_result gjk_get_simplex(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1, f32 min_unique_pt_dist_sq, u32 dbg_step) +#else +INTERNAL struct gjk_result gjk_get_simplex(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1, f32 min_unique_pt_dist_sq) +#endif { - struct temp_arena scratch = scratch_begin_no_conflict(); /* TODO: Only begin scratch for EPA */ - struct collider_collision_points_result res = ZI; - - struct v2 *points0 = shape0->points; - struct v2 *points1 = shape1->points; - u32 count0 = shape0->count; - u32 count1 = shape1->count; - f32 radius0 = shape0->radius; - f32 radius1 = shape1->radius; - (UNUSED)radius0; - (UNUSED)radius1; - - /* TODO: Parameterize */ - /* How close can non-overlapping shapes be before collision is considered */ - //const f32 tolerance = 0.f; - //const f32 tolerance = 0.05f; - const f32 tolerance = 0.005f; - - /* NOTE: Should always be less than tolerance, since colliding=true if origin is within this distance. */ - //const f32 min_unique_pt_dist_sq = 0.0001f * 0.0001f; - const f32 min_unique_pt_dist_sq = 0.001f * 0.001f; - - /* To prevent extremely large prototypes when origin is in exact center of rounded feature */ - const u32 max_epa_iterations = 64; - - b32 colliding = false; - b32 simplex_is_closest_edge = false; - - struct collider_simplex s = ZI; - struct collider_menkowski_point *proto = NULL; - u32 proto_count = 0; - - struct v2 normal = ZI; - struct collider_collision_point points[2] = ZI; - u32 num_points = 0; - + b32 overlapping = false; + struct collider_menkowski_simplex s = ZI; struct v2 dir = ZI; struct collider_menkowski_point m = ZI; -#if COLLIDER_DEBUG - u32 dbg_step = 0; -#endif + /* First point is support point in shape's general directions to eachother */ + dir = v2_sub(xf1.og, xf0.og); + if (v2_is_zero(dir)) dir = V2(1, 0); + s.a = get_menkowski_point(shape0, shape1, xf0, xf1, dir); + s.len = 1; - /* ========================== * - * GJK - * - * Determine encapsulating simplex if colliding, or closest edge / point to - * origin on simplex (for check if shape distances are within tolerance) - * ========================== */ - { - /* First point is support point in shape's general directions to eachother */ - dir = v2_sub(xf1.og, xf0.og); - if (v2_is_zero(dir)) dir = V2(1, 0); - s.a = get_menkowski_point(shape0, shape1, xf0, xf1, dir); - s.len = 1; + struct v2 removed_a = ZI; + struct v2 removed_b = ZI; + u32 num_removed = 0; + while (true) { + if (s.len == 1) { + /* Second point is support point towards origin */ + dir = v2_neg(s.a.p); - struct v2 removed_a = ZI; - struct v2 removed_b = ZI; - u32 num_removed = 0; - while (true) { - if (s.len == 1) { - /* Second point is support point towards origin */ - dir = v2_neg(s.a.p); - - DBGSTEP; - m = get_menkowski_point(shape0, shape1, xf0, xf1, dir); - /* Check that new point is far enough away from existing point */ - if (v2_len_sq(v2_sub(m.p, s.a.p)) < min_unique_pt_dist_sq) { - simplex_is_closest_edge = true; - break; - } - s.b = s.a; - s.a = m; - s.len = 2; - - /* 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 = get_menkowski_point(shape0, shape1, xf0, xf1, dir); + /* Check that new point is far enough away from existing point */ + if (v2_len_sq(v2_sub(m.p, s.a.p)) < min_unique_pt_dist_sq) { + overlapping = false; + break; } + s.b = s.a; + s.a = m; + s.len = 2; - { - DBGSTEP; - m = get_menkowski_point(shape0, shape1, xf0, xf1, dir); - /* Check that new point is far enough away from existing points */ - if (v2_len_sq(v2_sub(m.p, s.a.p)) < min_unique_pt_dist_sq || - v2_len_sq(v2_sub(m.p, s.b.p)) < min_unique_pt_dist_sq || - ( - (num_removed >= 1) && ( + /* 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 = get_menkowski_point(shape0, shape1, xf0, xf1, dir); + /* Check that new point is far enough away from existing points */ + if (v2_len_sq(v2_sub(m.p, s.a.p)) < min_unique_pt_dist_sq || + v2_len_sq(v2_sub(m.p, s.b.p)) < min_unique_pt_dist_sq || + ( + (num_removed >= 1) && ( (v2_len_sq(v2_sub(m.p, removed_a)) < min_unique_pt_dist_sq) || (num_removed >= 2 && v2_len_sq(v2_sub(m.p, removed_b)) < min_unique_pt_dist_sq)) ) || - math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_sub(m.p, s.a.p))) < min_unique_pt_dist_sq) - { - colliding = false; - simplex_is_closest_edge = true; - 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))) <= min_unique_pt_dist_sq) || - (math_fabs(v2_wedge(v2_sub(s.c.p, s.b.p), v2_neg(s.b.p))) <= min_unique_pt_dist_sq) || - (math_fabs(v2_wedge(v2_sub(s.c.p, s.a.p), v2_neg(s.a.p))) <= min_unique_pt_dist_sq)) { - /* Simplex lies on origin */ - colliding = true; - break; - } + math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_sub(m.p, s.a.p))) < min_unique_pt_dist_sq) { + overlapping = false; + break; } + s.c = s.b; + s.b = s.a; + s.a = m; + s.len = 3; - /* Determine region of the simplex in which the origin lies */ - DBGSTEP; - 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); - - f32 rab_dot = v2_dot(rab_dir, v2_neg(s.a.p)); - f32 rac_dot = v2_dot(rac_dir, v2_neg(s.a.p)); - f32 rbc_dot = v2_dot(rbc_dir, v2_neg(s.b.p)); - - f32 vab_dot = v2_dot(vab, v2_neg(s.a.p)) / v2_len_sq(vab); - f32 vac_dot = v2_dot(vac, v2_neg(s.a.p)) / v2_len_sq(vac); - f32 vbc_dot = v2_dot(vbc, v2_neg(s.b.p)) / v2_len_sq(vbc); - - if (rab_dot >= 0 && vab_dot >= 0 && vab_dot <= 1) { - /* Region ab, remove c */ - num_removed = 1; - removed_a = s.c.p; - s.len = 2; - dir = rab_dir; /* Next third point is in direction of region ab */ - } else if (rac_dot >= 0 && vac_dot >= 0 && vac_dot <= 1) { - /* Region ac, remove b */ - num_removed = 1; - removed_a = s.b.p; - s.len = 2; - s.b = s.c; - dir = rac_dir; /* Next third point is in direction of region ac */ - } else if (rbc_dot >= 0 && vbc_dot >= 0 && vbc_dot <= 1) { - /* Region bc, remove a */ - num_removed = 1; - removed_a = s.a.p; - s.len = 2; - s.a = s.b; - s.b = s.c; - dir = rbc_dir; /* Next third point is in direction of region bc */ - } else if (vab_dot <= 0 && vac_dot <= 0) { - /* Region a, remove bc */ - num_removed = 2; - removed_a = s.b.p; - removed_b = s.c.p; - s.len = 1; - } else if (vab_dot >= 1 && vbc_dot <= 0) { - /* Region b, remove ac */ - num_removed = 2; - removed_a = s.a.p; - removed_b = s.c.p; - s.len = 1; - s.a = s.b; - } else if (vac_dot >= 1 && vbc_dot >= 1) { - /* Region c, remove ab */ - num_removed = 2; - removed_a = s.a.p; - removed_b = s.b.p; - s.len = 1; - s.a = s.c; - } else { - /* No region, must be in simplex */ - colliding = true; + if ((math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) <= min_unique_pt_dist_sq) || + (math_fabs(v2_wedge(v2_sub(s.c.p, s.b.p), v2_neg(s.b.p))) <= min_unique_pt_dist_sq) || + (math_fabs(v2_wedge(v2_sub(s.c.p, s.a.p), v2_neg(s.a.p))) <= min_unique_pt_dist_sq)) { + /* Simplex lies on origin */ + overlapping = true; break; } } + + /* Determine region of the simplex in which the origin lies */ + DBGSTEP; + 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); + + f32 rab_dot = v2_dot(rab_dir, v2_neg(s.a.p)); + f32 rac_dot = v2_dot(rac_dir, v2_neg(s.a.p)); + f32 rbc_dot = v2_dot(rbc_dir, v2_neg(s.b.p)); + + f32 vab_dot = v2_dot(vab, v2_neg(s.a.p)) / v2_len_sq(vab); + f32 vac_dot = v2_dot(vac, v2_neg(s.a.p)) / v2_len_sq(vac); + f32 vbc_dot = v2_dot(vbc, v2_neg(s.b.p)) / v2_len_sq(vbc); + + if (rab_dot >= 0 && vab_dot >= 0 && vab_dot <= 1) { + /* Region ab, remove c */ + num_removed = 1; + removed_a = s.c.p; + s.len = 2; + dir = rab_dir; /* Next third point is in direction of region ab */ + } else if (rac_dot >= 0 && vac_dot >= 0 && vac_dot <= 1) { + /* Region ac, remove b */ + num_removed = 1; + removed_a = s.b.p; + s.len = 2; + s.b = s.c; + dir = rac_dir; /* Next third point is in direction of region ac */ + } else if (rbc_dot >= 0 && vbc_dot >= 0 && vbc_dot <= 1) { + /* Region bc, remove a */ + num_removed = 1; + removed_a = s.a.p; + s.len = 2; + s.a = s.b; + s.b = s.c; + dir = rbc_dir; /* Next third point is in direction of region bc */ + } else if (vab_dot <= 0 && vac_dot <= 0) { + /* Region a, remove bc */ + num_removed = 2; + removed_a = s.b.p; + removed_b = s.c.p; + s.len = 1; + } else if (vab_dot >= 1 && vbc_dot <= 0) { + /* Region b, remove ac */ + num_removed = 2; + removed_a = s.a.p; + removed_b = s.c.p; + s.len = 1; + s.a = s.b; + } else if (vac_dot >= 1 && vbc_dot >= 1) { + /* Region c, remove ab */ + num_removed = 2; + removed_a = s.a.p; + removed_b = s.b.p; + s.len = 1; + s.a = s.c; + } else { + /* No region, must be in simplex */ + overlapping = true; + break; + } } - if (colliding) { - /* ========================== * - * Epa (to find collision normal from inside shape) - * ========================== */ +#if COLLIDER_DEBUG + abort: +#endif + struct gjk_result res = { + .simplex = s, + .overlapping = overlapping, + .final_dir = dir, +#if COLLIDER_DEBUG + .dbg_step = dbg_step +#endif + }; + + return res; +} + +/* ========================== * + * EPA + * + * Expands upon result of GJK calculation to determine collision normal & closest edge when shapes are overlapping + * ========================== */ + +struct epa_result { + struct v2 normal; + struct collider_menkowski_feature closest_feature; /* Represents closest feature (edge or point) to origin on menkowski difference */ + +#if COLLIDER_DEBUG + struct collider_prototype prototype; + u32 dbg_step; +#endif +}; + +#if COLLIDER_DEBUG +INTERNAL struct epa_result epa_get_normal_from_gjk(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1, struct gjk_result gjk_res, f32 min_unique_pt_dist_sq, u32 max_iterations, u32 dbg_step) +#else +INTERNAL struct epa_result epa_get_normal_from_gjk(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1, struct gjk_result gjk_res, f32 min_unique_pt_dist_sq, u32 max_iterations) +#endif +{ + struct temp_arena scratch = scratch_begin_no_conflict(); + + struct collider_menkowski_feature closest_feature = ZI; + struct v2 normal = ZI; + + struct collider_menkowski_point *proto = NULL; + u32 proto_count = 0; + if (gjk_res.overlapping) { + struct collider_menkowski_simplex s = gjk_res.simplex; proto = arena_dry_push(scratch.arena, struct collider_menkowski_point); - proto_count = 0; { ASSERT(s.len == 3); struct collider_menkowski_point *tmp = arena_push_array(scratch.arena, struct collider_menkowski_point, 3); @@ -285,7 +313,7 @@ struct collider_collision_points_result collider_collision_points(struct collide i32 winding = v2_winding(v2_sub(s.c.p, s.a.p), v2_sub(s.b.p, s.a.p)); u32 epa_iterations = 0; - while (colliding) { + while (true) { ++epa_iterations; /* Find dir from origin to closest edge */ @@ -317,16 +345,18 @@ struct collider_collision_points_result collider_collision_points(struct collide struct v2 vab = v2_sub(closest_b.p, closest_a.p); /* Find new point in dir */ - dir = v2_mul(v2_perp(vab), winding); - m = get_menkowski_point(shape0, shape1, xf0, xf1, dir); + struct v2 dir = v2_mul(v2_perp(vab), winding); + struct collider_menkowski_point m = get_menkowski_point(shape0, shape1, xf0, xf1, dir); /* TODO: Remove this (debugging) */ +#if COLLIDER_DEBUG { normal = v2_norm(dir); - s.a = closest_a; - s.b = closest_b; - s.len = 2; + closest_feature.a = closest_a; + closest_feature.b = closest_b; + closest_feature.len = 2; } +#endif /* Check validity of new point */ DBGSTEP; @@ -352,9 +382,11 @@ struct collider_collision_points_result collider_collision_points(struct collide } } - if (!valid || epa_iterations >= max_epa_iterations) { - res.path = 1; + if (!valid || epa_iterations >= max_iterations) { normal = v2_norm(dir); + closest_feature.a = closest_a; + closest_feature.b = closest_b; + closest_feature.len = 2; break; } } @@ -369,35 +401,109 @@ struct collider_collision_points_result collider_collision_points(struct collide } proto[closest_b_index] = m; } - } else if (simplex_is_closest_edge) { - if (s.len == 1) { - struct v2 p = v2_neg(s.a.p); + } else { + normal = v2_norm(gjk_res.final_dir); + closest_feature.len = gjk_res.simplex.len; + closest_feature.a = gjk_res.simplex.a; + closest_feature.b = gjk_res.simplex.b; + } + +#if COLLIDER_DEBUG + abort: +#endif + + struct epa_result res = { + .normal = normal, + .closest_feature = closest_feature + }; + +#if COLLIDER_DEBUG + res.dbg_step = dbg_step; + u32 len = min_u32(proto_count, ARRAY_COUNT(res.prototype.points)); + for (u32 i = 0; i < len; ++i) { + res.prototype.points[i] = proto[i].p; + } + res.prototype.len = len; +#endif + + scratch_end(scratch); + return res; +} + +/* ========================== * + * Collision points + * ========================== */ + +struct collider_collision_points_result collider_collision_points(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1) +{ + struct collider_collision_points_result res = ZI; + + const f32 tolerance = COLLISION_TOLERANCE; + const f32 min_unique_pt_dist_sq = MIN_UNIQUE_PT_DIST_SQ; + const u32 max_epa_iterations = MAX_EPA_ITERATIONS; + + struct v2 *points0 = shape0->points; + struct v2 *points1 = shape1->points; + u32 count0 = shape0->count; + u32 count1 = shape1->count; + f32 radius0 = shape0->radius; + f32 radius1 = shape1->radius; + + struct collider_collision_point points[2] = ZI; + u32 num_points = 0; + b32 colliding = false; + struct v2 normal = ZI; + +#if COLLIDER_DEBUG + u32 dbg_step = 0; +#endif + + + /* Run GJK */ +#if COLLIDER_DEBUG + struct gjk_result gjk_res = gjk_get_simplex(shape0, shape1, xf0, xf1, min_unique_pt_dist_sq, dbg_step); + dbg_step = gjk_res.dbg_step; +#else + struct gjk_result gjk_res = gjk_get_simplex(shape0, shape1, xf0, xf1, min_unique_pt_dist_sq); +#endif + DBGSTEP; + + /* Run EPA */ +#if COLLIDER_DEBUG + struct epa_result epa_res = epa_get_normal_from_gjk(shape0, shape1, xf0, xf1, gjk_res, min_unique_pt_dist_sq, max_epa_iterations, dbg_step); + dbg_step = epa_res.dbg_step; +#else + struct epa_result epa_res = epa_get_normal_from_gjk(shape0, shape1, xf0, xf1, gjk_res, min_unique_pt_dist_sq, max_epa_iterations); +#endif + normal = epa_res.normal; + DBGSTEP; + + /* Determine collision */ + if (gjk_res.overlapping) { + colliding = true; + } else { + struct collider_menkowski_feature f = epa_res.closest_feature; + /* Shapes not overlapping, determine if distance between shapes within tolerance */ + if (f.len == 1) { + struct v2 p = v2_neg(f.a.p); if (v2_len_sq(p) <= (tolerance * tolerance)) { - res.path = 2; - normal = v2_norm(dir); colliding = true; } } else { - /* Shapes are not overlapping (origin is outside of simplex). Project - * origin to determine if distance is within tolerance. */ - ASSERT(s.len == 2); - struct v2 vab = v2_sub(s.b.p, s.a.p); - struct v2 vao = v2_neg(s.a.p); + /* Project origin to determine if distance is within tolerance. */ + ASSERT(f.len == 2); + struct v2 vab = v2_sub(f.b.p, f.a.p); + struct v2 vao = v2_neg(f.a.p); f32 ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1); - struct v2 p = v2_add(s.a.p, v2_mul(vab, ratio)); + struct v2 p = v2_add(f.a.p, v2_mul(vab, ratio)); if (v2_len_sq(p) <= (tolerance * tolerance)) { - res.path = 2; - normal = v2_norm(dir); colliding = true; } } } + /* Clip to determine final points */ if (colliding) { - /* ========================== * - * Clip to determine final points - * ========================== */ - /* Max vertices must be < 16 to fit in 4 bit ids */ CT_ASSERT(ARRAY_COUNT(shape0->points) <= 16); @@ -647,21 +753,17 @@ struct collider_collision_points_result collider_collision_points(struct collide } } - res.solved = true; #if COLLIDER_DEBUG + res.solved = true; abort: + res.simplex = gjk_res.simplex; + res.prototype.len = epa_res.prototype.len; + MEMCPY(res.prototype.points, epa_res.prototype.points, sizeof(res.prototype.points[0]) * res.prototype.len); #endif - u32 len = min_u32(proto_count, ARRAY_COUNT(res.prototype.points)); - for (u32 i = 0; i < len; ++i) { - res.prototype.points[i] = proto[i].p; - } - res.prototype.len = len; res.normal = normal; res.points[0] = points[0]; res.points[1] = points[1]; res.num_points = num_points; - res.simplex = s; - scratch_end(scratch); return res; } @@ -669,351 +771,84 @@ struct collider_collision_points_result collider_collision_points(struct collide * Closest points * ========================== */ -/* TODO: De-duplicate code between collider_closest_points & collider_collision_points */ + /* TODO: De-duplicate code between collider_closest_points & collider_collision_points */ struct collider_closest_points_result collider_closest_points(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1) { - struct temp_arena scratch = scratch_begin_no_conflict(); /* TODO: Only begin scratch for EPA */ struct collider_closest_points_result res = ZI; + const f32 tolerance = COLLISION_TOLERANCE; + const f32 min_unique_pt_dist_sq = MIN_UNIQUE_PT_DIST_SQ; + const u32 max_epa_iterations = MAX_EPA_ITERATIONS; + struct v2 p0 = ZI; struct v2 p1 = ZI; - f32 radius0 = shape0->radius; - f32 radius1 = shape1->radius; - (UNUSED)radius0; - (UNUSED)radius1; - - /* TODO: Parameterize */ - /* How close can non-overlapping shapes be before collision is considered */ - //const f32 tolerance = 0.f; - //const f32 tolerance = 0.05f; - const f32 tolerance = 0.005f; - - /* NOTE: Should always be less than tolerance, since colliding=true if origin is within this distance. */ - //const f32 min_unique_pt_dist_sq = 0.0001f * 0.0001f; - const f32 min_unique_pt_dist_sq = 0.001f * 0.001f; - - /* To prevent extremely large prototypes when origin is in exact center of rounded feature */ - const u32 max_epa_iterations = 64; - b32 colliding = false; - b32 simplex_is_closest_edge = false; - - struct collider_simplex s = ZI; - struct collider_menkowski_point *proto = NULL; - u32 proto_count = 0; - - struct v2 normal = ZI; - - struct v2 dir = ZI; - struct collider_menkowski_point m = ZI; #if COLLIDER_DEBUG u32 dbg_step = 0; #endif - /* ========================== * - * GJK - * - * Determine encapsulating simplex if colliding, or closest edge / point to - * origin on simplex (for check if shape distances are within tolerance) - * ========================== */ - { - /* First point is support point in shape's general directions to eachother */ - dir = v2_sub(xf1.og, xf0.og); - if (v2_is_zero(dir)) dir = V2(1, 0); - s.a = get_menkowski_point(shape0, shape1, xf0, xf1, dir); - s.len = 1; + /* Run GJK */ +#if COLLIDER_DEBUG + struct gjk_result gjk_res = gjk_get_simplex(shape0, shape1, xf0, xf1, min_unique_pt_dist_sq, dbg_step); + dbg_step = gjk_res.dbg_step; +#else + struct gjk_result gjk_res = gjk_get_simplex(shape0, shape1, xf0, xf1, min_unique_pt_dist_sq); +#endif + DBGSTEP; - struct v2 removed_a = ZI; - struct v2 removed_b = ZI; - u32 num_removed = 0; - while (true) { - if (s.len == 1) { - /* Second point is support point towards origin */ - dir = v2_neg(s.a.p); - - DBGSTEP; - m = get_menkowski_point(shape0, shape1, xf0, xf1, dir); - /* Check that new point is far enough away from existing point */ - if (v2_len_sq(v2_sub(m.p, s.a.p)) < min_unique_pt_dist_sq) { - simplex_is_closest_edge = true; - break; - } - s.b = s.a; - s.a = m; - s.len = 2; - - /* 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 = get_menkowski_point(shape0, shape1, xf0, xf1, dir); - /* Check that new point is far enough away from existing points */ - if (v2_len_sq(v2_sub(m.p, s.a.p)) < min_unique_pt_dist_sq || - v2_len_sq(v2_sub(m.p, s.b.p)) < min_unique_pt_dist_sq || - ( - (num_removed >= 1) && ( - (v2_len_sq(v2_sub(m.p, removed_a)) < min_unique_pt_dist_sq) || - (num_removed >= 2 && v2_len_sq(v2_sub(m.p, removed_b)) < min_unique_pt_dist_sq)) - ) || - math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_sub(m.p, s.a.p))) < min_unique_pt_dist_sq) { - colliding = false; - simplex_is_closest_edge = true; - 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))) <= min_unique_pt_dist_sq) || - (math_fabs(v2_wedge(v2_sub(s.c.p, s.b.p), v2_neg(s.b.p))) <= min_unique_pt_dist_sq) || - (math_fabs(v2_wedge(v2_sub(s.c.p, s.a.p), v2_neg(s.a.p))) <= min_unique_pt_dist_sq)) { - /* Simplex lies on origin */ - colliding = true; - break; - } - } - - /* Determine region of the simplex in which the origin lies */ - DBGSTEP; - 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); - - f32 rab_dot = v2_dot(rab_dir, v2_neg(s.a.p)); - f32 rac_dot = v2_dot(rac_dir, v2_neg(s.a.p)); - f32 rbc_dot = v2_dot(rbc_dir, v2_neg(s.b.p)); - - f32 vab_dot = v2_dot(vab, v2_neg(s.a.p)) / v2_len_sq(vab); - f32 vac_dot = v2_dot(vac, v2_neg(s.a.p)) / v2_len_sq(vac); - f32 vbc_dot = v2_dot(vbc, v2_neg(s.b.p)) / v2_len_sq(vbc); - - if (rab_dot >= 0 && vab_dot >= 0 && vab_dot <= 1) { - /* Region ab, remove c */ - num_removed = 1; - removed_a = s.c.p; - s.len = 2; - dir = rab_dir; /* Next third point is in direction of region ab */ - } else if (rac_dot >= 0 && vac_dot >= 0 && vac_dot <= 1) { - /* Region ac, remove b */ - num_removed = 1; - removed_a = s.b.p; - s.len = 2; - s.b = s.c; - dir = rac_dir; /* Next third point is in direction of region ac */ - } else if (rbc_dot >= 0 && vbc_dot >= 0 && vbc_dot <= 1) { - /* Region bc, remove a */ - num_removed = 1; - removed_a = s.a.p; - s.len = 2; - s.a = s.b; - s.b = s.c; - dir = rbc_dir; /* Next third point is in direction of region bc */ - } else if (vab_dot <= 0 && vac_dot <= 0) { - /* Region a, remove bc */ - num_removed = 2; - removed_a = s.b.p; - removed_b = s.c.p; - s.len = 1; - } else if (vab_dot >= 1 && vbc_dot <= 0) { - /* Region b, remove ac */ - num_removed = 2; - removed_a = s.a.p; - removed_b = s.c.p; - s.len = 1; - s.a = s.b; - } else if (vac_dot >= 1 && vbc_dot >= 1) { - /* Region c, remove ab */ - num_removed = 2; - removed_a = s.a.p; - removed_b = s.b.p; - s.len = 1; - s.a = s.c; - } else { - /* No region, must be in simplex */ - colliding = true; - break; - } - } - } - - if (colliding) { - /* ========================== * - * Epa (to find collision normal from inside shape) - * ========================== */ - - proto = arena_dry_push(scratch.arena, struct collider_menkowski_point); - proto_count = 0; - { - ASSERT(s.len == 3); - struct collider_menkowski_point *tmp = arena_push_array(scratch.arena, struct collider_menkowski_point, 3); - tmp[0] = s.a; - tmp[1] = s.b; - tmp[2] = s.c; - proto_count = 3; - } - - i32 winding = v2_winding(v2_sub(s.c.p, s.a.p), v2_sub(s.b.p, s.a.p)); - - u32 epa_iterations = 0; - while (colliding) { - ++epa_iterations; - - /* Find dir from origin to closest edge */ - /* FIXME: Winding order of ps & pe index */ - f32 closest_len_sq = F32_INFINITY; - struct collider_menkowski_point closest_a = ZI; - struct collider_menkowski_point closest_b = ZI; - u32 closest_b_index = 0; - for (u32 i = 0; i < proto_count; ++i) { - u32 a_index = i; - u32 b_index = (i < proto_count - 1) ? (i + 1) : 0; - struct collider_menkowski_point a = proto[a_index]; - struct collider_menkowski_point b = proto[b_index]; - - struct v2 vab = v2_sub(b.p, a.p); - struct v2 vao = v2_neg(a.p); - - f32 proj_ratio = clamp_f32(v2_dot(vao, vab) / v2_len_sq(vab), 0, 1); - struct v2 proj = v2_add(a.p, v2_mul(vab, proj_ratio)); - - f32 proj_len_sq = v2_len_sq(proj); - if (proj_len_sq < closest_len_sq - min_unique_pt_dist_sq) { - closest_a = a; - closest_b = b; - closest_b_index = b_index; - closest_len_sq = proj_len_sq; - } - } - struct v2 vab = v2_sub(closest_b.p, closest_a.p); - - /* Find new point in dir */ - dir = v2_mul(v2_perp(vab), winding); - m = get_menkowski_point(shape0, shape1, xf0, xf1, dir); - - /* TODO: Remove this (debugging) */ - { - normal = v2_norm(dir); - s.a = closest_a; - s.b = closest_b; - s.len = 2; - } - - /* Check validity of new point */ - DBGSTEP; - { - b32 valid = true; - - { - /* NOTE: Changing this value affects how stable normals are for circular colliders */ - //const f32 validity_epsilon = min_unique_pt_dist_sq; /* Arbitrary */ - const f32 validity_epsilon = 0.0000000001f; /* Arbitrary */ - - struct v2 vam = v2_sub(m.p, closest_a.p); - struct v2 vbm = v2_sub(closest_b.p, closest_a.p); - - f32 dot = v2_dot(vab, vam) / v2_len_sq(vab); - - if (dot >= -validity_epsilon && dot <= 1 - validity_epsilon && (v2_wedge(vab, vam) * -winding) >= -validity_epsilon) { - /* New point is not between edge */ - valid = false; - } else if (v2_len_sq(vam) < min_unique_pt_dist_sq || v2_len_sq(vbm) < min_unique_pt_dist_sq) { - /* New point is too close to existing */ - valid = false; - } - } - - if (!valid || epa_iterations >= max_epa_iterations) { - res.path = 1; - normal = v2_norm(dir); - break; - } - } - - /* Insert point into prototype */ - arena_push(scratch.arena, struct collider_menkowski_point); - ++proto_count; - for (u32 i = proto_count - 1; i > closest_b_index; --i) { - u32 shift_from = (i > 0) ? i - 1 : proto_count - 1; - u32 shift_to = i; - proto[shift_to] = proto[shift_from]; - } - proto[closest_b_index] = m; - } - } else if (simplex_is_closest_edge) { - if (s.len == 1) { - struct v2 p = v2_neg(s.a.p); - if (v2_len_sq(p) <= (tolerance * tolerance)) { - res.path = 2; - normal = v2_norm(dir); - colliding = true; - } - } else { - /* Shapes are not overlapping (origin is outside of simplex). Project - * origin to determine if distance is within tolerance. */ - ASSERT(s.len == 2); - struct v2 vab = v2_sub(s.b.p, s.a.p); - struct v2 vao = v2_neg(s.a.p); - f32 ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1); - struct v2 p = v2_add(s.a.p, v2_mul(vab, ratio)); - if (v2_len_sq(p) <= (tolerance * tolerance)) { - res.path = 2; - normal = v2_norm(dir); - colliding = true; - } - } - } + /* Run EPA */ +#if COLLIDER_DEBUG + struct epa_result epa_res = epa_get_normal_from_gjk(shape0, shape1, xf0, xf1, gjk_res, min_unique_pt_dist_sq, max_epa_iterations, dbg_step); + dbg_step = epa_res.dbg_step; +#else + struct epa_result epa_res = epa_get_normal_from_gjk(shape0, shape1, xf0, xf1, gjk_res, min_unique_pt_dist_sq, max_epa_iterations); +#endif + DBGSTEP; /* ========================== * * Resolve points * ========================== */ - if (s.len == 1) { - p0 = s.a.s0; - p1 = s.a.s1; + colliding = gjk_res.overlapping; + struct collider_menkowski_feature f = epa_res.closest_feature; + if (f.len == 1) { + p0 = f.a.s0; + p1 = f.a.s1; + colliding = gjk_res.overlapping || v2_len_sq(v2_neg(f.a.p)) <= (tolerance * tolerance); } else { - ASSERT(s.len == 2); + ASSERT(f.len == 2); /* FIXME: Winding order dependent? */ 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); + struct v2 vab = v2_sub(f.b.p, f.a.p); + struct v2 vao = v2_neg(f.a.p); ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1); } /* Shape 0 */ - p0 = v2_sub(s.b.s0, s.a.s0); + p0 = v2_sub(f.b.s0, f.a.s0); p0 = v2_mul(p0, ratio); - p0 = v2_add(p0, s.a.s0); + p0 = v2_add(p0, f.a.s0); /* Shape 1 */ - p1 = v2_sub(s.b.s1, s.a.s1); + p1 = v2_sub(f.b.s1, f.a.s1); p1 = v2_mul(p1, ratio); - p1 = v2_add(p1, s.a.s1); + p1 = v2_add(p1, f.a.s1); + colliding = gjk_res.overlapping || v2_len_sq(v2_sub(p1, p0)) <= (tolerance * tolerance); } - res.solved = true; #if COLLIDER_DEBUG - abort : + res.solved = true; + abort: + res.simplex = gjk_res.simplex; + res.prototype.len = epa_res.prototype.len; + MEMCPY(res.prototype.points, epa_res.prototype.points, sizeof(res.prototype.points[0]) *res.prototype.len); + res.simplex = gjk_res.simplex; #endif - u32 len = min_u32(proto_count, ARRAY_COUNT(res.prototype.points)); - for (u32 i = 0; i < len; ++i) { - res.prototype.points[i] = proto[i].p; - } - res.prototype.len = len; - res.normal = normal; - res.simplex = s; res.p0 = p0; res.p1 = p1; res.colliding = colliding; - scratch_end(scratch); return res; } diff --git a/src/collider.h b/src/collider.h index b4ea7a4e..c8f3a2fd 100644 --- a/src/collider.h +++ b/src/collider.h @@ -18,11 +18,16 @@ struct collider_menkowski_point { struct v2 s1; /* Support point of second shape in -dir */ }; -struct collider_simplex { +struct collider_menkowski_simplex { u32 len; struct collider_menkowski_point a, b, c; }; +struct collider_menkowski_feature { + u32 len; + struct collider_menkowski_point a, b; +}; + struct collider_collision_point { struct v2 point; f32 separation; @@ -37,8 +42,7 @@ struct collider_collision_points_result { /* For debugging */ b32 solved; - i32 path; - struct collider_simplex simplex; + struct collider_menkowski_simplex simplex; struct collider_prototype prototype; struct v2 a0, b0, a1, b1; /* Clipping faces */ }; @@ -46,14 +50,12 @@ struct collider_collision_points_result { struct collider_collision_points_result collider_collision_points(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1); struct collider_closest_points_result { - struct v2 normal; struct v2 p0, p1; b32 colliding; /* For debugging */ b32 solved; - i32 path; - struct collider_simplex simplex; + struct collider_menkowski_simplex simplex; struct collider_prototype prototype; }; diff --git a/src/entity.h b/src/entity.h index 8a00578c..02622d1d 100644 --- a/src/entity.h +++ b/src/entity.h @@ -9,7 +9,8 @@ enum entity_prop { ENTITY_PROP_ACTIVE, - ENTITY_PROP_RELEASE_AT_END_OF_FRAME, + ENTITY_PROP_RELEASE_BEFORE_PUBLISH, + ENTITY_PROP_RELEASE_AFTER_PUBLISH, ENTITY_PROP_PHYSICAL_DYNAMIC, ENTITY_PROP_PHYSICAL_KINEMATIC, diff --git a/src/game.c b/src/game.c index 774adb67..657effa3 100644 --- a/src/game.c +++ b/src/game.c @@ -543,7 +543,7 @@ INTERNAL void create_contacts(void) event->hit_event.e0 = e0->handle; event->hit_event.e1 = e1->handle; event->hit_event.normal = collider_res.normal; - entity_enable_prop(event, ENTITY_PROP_RELEASE_AT_END_OF_FRAME); + entity_enable_prop(event, ENTITY_PROP_RELEASE_BEFORE_PUBLISH); entity_enable_prop(event, ENTITY_PROP_ACTIVE); /* Calculate point */ @@ -778,7 +778,7 @@ INTERNAL void prepare_contacts(void) } else { /* Mark constraint for removal */ entity_disable_prop(constraint_ent, ENTITY_PROP_ACTIVE); - entity_enable_prop(constraint_ent, ENTITY_PROP_RELEASE_AT_END_OF_FRAME); + entity_enable_prop(constraint_ent, ENTITY_PROP_RELEASE_BEFORE_PUBLISH); /* Remove from lookup */ struct entity_lookup_key key = entity_lookup_key_from_two_handles(constraint->e0, constraint->e1); struct entity_lookup_entry *entry = entity_lookup_get(&G.contact_lookup, key); @@ -806,7 +806,7 @@ INTERNAL void prepare_contacts(void) || !(entity_has_prop(e1, ENTITY_PROP_PHYSICAL_DYNAMIC) || entity_has_prop(e1, ENTITY_PROP_PHYSICAL_KINEMATIC))) { /* Mark dbg ent for removal */ entity_disable_prop(dbg_ent, ENTITY_PROP_ACTIVE); - entity_enable_prop(dbg_ent, ENTITY_PROP_RELEASE_AT_END_OF_FRAME); + entity_enable_prop(dbg_ent, ENTITY_PROP_RELEASE_BEFORE_PUBLISH); /* Remove from lookup */ struct entity_lookup_key key = entity_lookup_key_from_two_handles(dbg->e0, dbg->e1); @@ -816,7 +816,7 @@ INTERNAL void prepare_contacts(void) --e0->colliding; } if (e1->valid) { - --e0->colliding; + --e1->colliding; } if (entry) { @@ -1064,7 +1064,7 @@ INTERNAL void prepare_motor_joints(void) } else { /* Mark joint for removal */ entity_disable_prop(joint_ent, ENTITY_PROP_ACTIVE); - entity_enable_prop(joint_ent, ENTITY_PROP_RELEASE_AT_END_OF_FRAME); + entity_enable_prop(joint_ent, ENTITY_PROP_RELEASE_BEFORE_PUBLISH); } } } @@ -1307,7 +1307,7 @@ INTERNAL void prepare_mouse_joints(void) } else { /* Mark joint for removal */ entity_disable_prop(joint_ent, ENTITY_PROP_ACTIVE); - entity_enable_prop(joint_ent, ENTITY_PROP_RELEASE_AT_END_OF_FRAME); + entity_enable_prop(joint_ent, ENTITY_PROP_RELEASE_BEFORE_PUBLISH); } } } @@ -1581,10 +1581,6 @@ INTERNAL f32 toi(struct collider_shape *c0, struct collider_shape *c1, ++iteration; } - if (iteration > 1) { - DEBUGBREAKABLE; - } - return t; } @@ -1594,7 +1590,6 @@ INTERNAL f32 determine_earliest_toi(f32 dt, f32 tolerance, u32 max_iterations) f32 smallest_t = 1; struct entity_store *store = G.tick.entity_store; - //struct entity *root = G.root; for (u64 e0_index = 0; e0_index < store->reserved; ++e0_index) { struct entity *e0 = &store->entities[e0_index]; @@ -1646,7 +1641,37 @@ INTERNAL f32 determine_earliest_toi(f32 dt, f32 tolerance, u32 max_iterations) return smallest_t; } +/* ========================== * + * Release entities + * ========================== */ +INTERNAL void release_entities_with_prop(enum entity_prop prop) +{ + struct temp_arena scratch = scratch_begin_no_conflict(); + struct entity_store *store = G.tick.entity_store; + + struct entity **ents_to_release = arena_dry_push(scratch.arena, struct entity *); + u64 ents_to_release_count = 0; + for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { + struct entity *ent = &store->entities[entity_index]; + if (!ent->valid) continue; + + if (entity_has_prop(ent, prop)) { + *arena_push(scratch.arena, struct entity *) = ent; + ++ents_to_release_count; + } + } + + for (u64 i = 0; i < ents_to_release_count; ++i) { + struct entity *ent = ents_to_release[i]; + if (ent->valid) { + /* Release */ + entity_release(store, ent); + } + } + + scratch_end(scratch); +} /* ========================== * * Update @@ -2271,7 +2296,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) #if GAME_PHYSICS_ENABLE_TOI const f32 min_toi = 0.000001f; const f32 tolerance = 0.00001f; - const u32 max_iterations = 128; + const u32 max_iterations = 16; earliest_toi = max_f32(determine_earliest_toi(remaining_dt, tolerance, max_iterations), min_toi); #else (UNUSED)toi; @@ -2335,7 +2360,18 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) (UNUSED)bullet; (UNUSED)target; - entity_enable_prop(bullet, ENTITY_PROP_RELEASE_AT_END_OF_FRAME); +#if 0 + { + /* Set bullet position to hit position */ + struct xform xf = entity_get_xform(bullet); + xf.og = event->point; + entity_set_xform(bullet, xf); + /* Release after publish so user sees bullet in final postiion */ + entity_enable_prop(bullet, ENTITY_PROP_RELEASE_AFTER_PUBLISH); + } +#else + entity_enable_prop(bullet, ENTITY_PROP_RELEASE_BEFORE_PUBLISH); +#endif /* Create test blood */ /* TODO: Remove this */ @@ -2451,37 +2487,13 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) } /* ========================== * - * Release entities + * Release entities pre-publish * ========================== */ /* TODO: Breadth first iteration to only release parent entities (since * child entities will be released along with parent anyway) */ - { - struct temp_arena temp = arena_temp_begin(scratch.arena); - - struct entity **ents_to_release = arena_dry_push(temp.arena, struct entity *); - u64 ents_to_release_count = 0; - for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { - struct entity *ent = &store->entities[entity_index]; - if (!ent->valid) continue; - - if (entity_has_prop(ent, ENTITY_PROP_RELEASE_AT_END_OF_FRAME)) { - *arena_push(temp.arena, struct entity *) = ent; - ++ents_to_release_count; - } - } - - for (u64 i = 0; i < ents_to_release_count; ++i) { - struct entity *ent = ents_to_release[i]; - if (ent->valid) { - /* Release */ - entity_release(store, ent); - } - } - - arena_temp_end(temp); - } + release_entities_with_prop(ENTITY_PROP_RELEASE_BEFORE_PUBLISH); /* ========================== * * Publish tick @@ -2491,6 +2503,12 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) publish_game_tick(); __profframe("Game"); + /* ========================== * + * Release entities post-publish + * ========================== */ + + release_entities_with_prop(ENTITY_PROP_RELEASE_AFTER_PUBLISH); + /* ========================== * * End frame cache scopes * ========================== */