From 1e81a7ea415341f45a41ce367a8779a73766809e Mon Sep 17 00:00:00 2001 From: jacob Date: Mon, 6 Jan 2025 11:41:59 -0600 Subject: [PATCH] create collider_closest_points function --- src/collider.c | 491 +++++++++++++++++++++++++++++++++++++++++-------- src/collider.h | 29 ++- src/entity.h | 3 + src/game.c | 65 ++++++- src/user.c | 21 ++- 5 files changed, 519 insertions(+), 90 deletions(-) diff --git a/src/collider.c b/src/collider.c index 94de590c..a9c2cdc2 100644 --- a/src/collider.c +++ b/src/collider.c @@ -76,9 +76,13 @@ INTERNAL u32 collider_support_point_index(struct collider_shape *a, struct xform return furthest; } -INTERNAL struct v2 menkowski_point(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1, struct v2 dir) +INTERNAL struct collider_menkowski_point get_menkowski_point(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1, struct v2 dir) { - return v2_sub(collider_support_point(shape0, xf0, dir), collider_support_point(shape1, xf1, v2_neg(dir))); + struct collider_menkowski_point res; + res.s0 = collider_support_point(shape0, xf0, dir); + res.s1 = collider_support_point(shape1, xf1, v2_neg(dir)); + res.p = v2_sub(res.s0, res.s1); + return res; } struct collider_collision_points_result collider_collision_points(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1) @@ -112,7 +116,7 @@ struct collider_collision_points_result collider_collision_points(struct collide b32 simplex_is_closest_edge = false; struct collider_simplex s = ZI; - struct v2 *proto = NULL; + struct collider_menkowski_point *proto = NULL; u32 proto_count = 0; struct v2 normal = ZI; @@ -120,7 +124,7 @@ struct collider_collision_points_result collider_collision_points(struct collide u32 num_points = 0; struct v2 dir = ZI; - struct v2 m = ZI; + struct collider_menkowski_point m = ZI; #if COLLIDER_DEBUG u32 dbg_step = 0; @@ -136,7 +140,7 @@ struct collider_collision_points_result collider_collision_points(struct collide /* 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 = menkowski_point(shape0, shape1, xf0, xf1, dir); + s.a = get_menkowski_point(shape0, shape1, xf0, xf1, dir); s.len = 1; struct v2 removed_a = ZI; @@ -145,12 +149,12 @@ struct collider_collision_points_result collider_collision_points(struct collide while (true) { if (s.len == 1) { /* Second point is support point towards origin */ - dir = v2_neg(s.a); + dir = v2_neg(s.a.p); DBGSTEP; - m = menkowski_point(shape0, shape1, xf0, xf1, dir); + 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, s.a)) < min_unique_pt_dist_sq) { + if (v2_len_sq(v2_sub(m.p, s.a.p)) < min_unique_pt_dist_sq) { simplex_is_closest_edge = true; break; } @@ -159,21 +163,21 @@ struct collider_collision_points_result collider_collision_points(struct collide s.len = 2; /* Third point is support point in direction of line normal towards origin */ - dir = v2_perp_towards_dir(v2_sub(s.b, s.a), v2_neg(s.a)); + dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p)); } { DBGSTEP; - m = menkowski_point(shape0, shape1, xf0, xf1, dir); + 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, s.a)) < min_unique_pt_dist_sq || - v2_len_sq(v2_sub(m, s.b)) < min_unique_pt_dist_sq || + 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, removed_a)) < min_unique_pt_dist_sq) || - (num_removed >= 2 && v2_len_sq(v2_sub(m, removed_b)) < min_unique_pt_dist_sq)) + (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, s.a), v2_sub(m, s.a))) < 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; @@ -184,9 +188,9 @@ struct collider_collision_points_result collider_collision_points(struct collide s.a = m; s.len = 3; - if ((math_fabs(v2_wedge(v2_sub(s.b, s.a), v2_neg(s.a))) <= min_unique_pt_dist_sq) || - (math_fabs(v2_wedge(v2_sub(s.c, s.b), v2_neg(s.b))) <= min_unique_pt_dist_sq) || - (math_fabs(v2_wedge(v2_sub(s.c, s.a), v2_neg(s.a))) <= min_unique_pt_dist_sq)) { + 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; @@ -195,39 +199,39 @@ struct collider_collision_points_result collider_collision_points(struct collide /* Determine region of the simplex in which the origin lies */ DBGSTEP; - struct v2 vab = v2_sub(s.b, s.a); - struct v2 vac = v2_sub(s.c, s.a); - struct v2 vbc = v2_sub(s.c, s.b); + 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)); - f32 rac_dot = v2_dot(rac_dir, v2_neg(s.a)); - f32 rbc_dot = v2_dot(rbc_dir, v2_neg(s.b)); + 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)) / v2_len_sq(vab); - f32 vac_dot = v2_dot(vac, v2_neg(s.a)) / v2_len_sq(vac); - f32 vbc_dot = v2_dot(vbc, v2_neg(s.b)) / v2_len_sq(vbc); + 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; + 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; + 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; + removed_a = s.a.p; s.len = 2; s.a = s.b; s.b = s.c; @@ -235,21 +239,21 @@ struct collider_collision_points_result collider_collision_points(struct collide } else if (vab_dot <= 0 && vac_dot <= 0) { /* Region a, remove bc */ num_removed = 2; - removed_a = s.b; - removed_b = s.c; + 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; - removed_b = s.c; + 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; - removed_b = s.b; + removed_a = s.a.p; + removed_b = s.b.p; s.len = 1; s.a = s.c; } else { @@ -265,18 +269,18 @@ struct collider_collision_points_result collider_collision_points(struct collide * Epa (to find collision normal from inside shape) * ========================== */ - proto = arena_dry_push(scratch.arena, struct v2); + proto = arena_dry_push(scratch.arena, struct collider_menkowski_point); proto_count = 0; { ASSERT(s.len == 3); - struct v2 *tmp = arena_push_array(scratch.arena, struct v2, 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, s.a), v2_sub(s.b, s.a)); + 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) { @@ -285,20 +289,20 @@ struct collider_collision_points_result collider_collision_points(struct collide /* Find dir from origin to closest edge */ /* FIXME: Winding order of ps & pe index */ f32 closest_len_sq = F32_INFINITY; - struct v2 closest_a = ZI; - struct v2 closest_b = ZI; + 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 v2 a = proto[a_index]; - struct v2 b = proto[b_index]; + struct collider_menkowski_point a = proto[a_index]; + struct collider_menkowski_point b = proto[b_index]; - struct v2 vab = v2_sub(b, a); - struct v2 vao = v2_neg(a); + 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, v2_mul(vab, proj_ratio)); + 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) { @@ -308,11 +312,11 @@ struct collider_collision_points_result collider_collision_points(struct collide closest_len_sq = proj_len_sq; } } - struct v2 vab = v2_sub(closest_b, closest_a); + struct v2 vab = v2_sub(closest_b.p, closest_a.p); /* Find new point in dir */ dir = v2_mul(v2_perp(vab), winding); - m = menkowski_point(shape0, shape1, xf0, xf1, dir); + m = get_menkowski_point(shape0, shape1, xf0, xf1, dir); /* TODO: Remove this (debugging) */ { @@ -332,8 +336,8 @@ struct collider_collision_points_result collider_collision_points(struct collide //const f32 validity_epsilon = min_unique_pt_dist_sq; /* Arbitrary */ const f32 validity_epsilon = 0.0000000001f; /* Arbitrary */ - struct v2 vam = v2_sub(m, closest_a); - struct v2 vbm = v2_sub(closest_b, closest_a); + 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); @@ -365,7 +369,7 @@ struct collider_collision_points_result collider_collision_points(struct collide } } else if (simplex_is_closest_edge) { if (s.len == 1) { - struct v2 p = v2_neg(s.a); + struct v2 p = v2_neg(s.a.p); if (v2_len_sq(p) <= (tolerance * tolerance)) { res.path = 2; normal = v2_norm(dir); @@ -375,10 +379,10 @@ struct collider_collision_points_result collider_collision_points(struct collide /* 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, s.a); - struct v2 vao = v2_neg(s.a); + 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, v2_mul(vab, ratio)); + 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); @@ -647,7 +651,7 @@ struct collider_collision_points_result collider_collision_points(struct collide #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]; + res.prototype.points[i] = proto[i].p; } res.prototype.len = len; res.normal = normal; @@ -659,6 +663,358 @@ struct collider_collision_points_result collider_collision_points(struct collide return res; } +/* ========================== * + * Closest 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; + + 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; + + 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; + } + } + } + + /* ========================== * + * Resolve points + * ========================== */ + + if (s.len == 1) { + p0 = s.a.s0; + p1 = s.a.s1; + } else { + ASSERT(s.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); + 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_mul(p0, ratio); + p0 = v2_add(p0, s.a.s0); + /* Shape 1 */ + p1 = v2_sub(s.b.s1, s.a.s1); + p1 = v2_mul(p1, ratio); + p1 = v2_add(p1, s.a.s1); + } + + res.solved = true; +#if COLLIDER_DEBUG + abort : +#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; +} + /* ========================== * * Debug functions * TODO: Remove these @@ -671,9 +1027,9 @@ struct v2_array menkowski(struct arena *arena, struct collider_shape *shape0, st for (u64 i = 0; i < detail; ++i) { f32 angle = ((f32)i / detail) * (2 * PI); struct v2 dir = v2_from_angle(angle); - struct v2 p = menkowski_point(shape0, shape1, xf0, xf1, dir); - if (res.count == 0 || !v2_eq(p, res.points[res.count - 1])) { - *arena_push(arena, struct v2) = p; + struct collider_menkowski_point m = get_menkowski_point(shape0, shape1, xf0, xf1, dir); + if (res.count == 0 || !v2_eq(m.p, res.points[res.count - 1])) { + *arena_push(arena, struct v2) = m.p; ++res.count; } } @@ -700,19 +1056,6 @@ struct v2_array cloud(struct arena *arena, struct collider_shape *shape0, struct return res; } - - - - - - - - - - - - - /* ========================== * * Boolean GJK (unused) * ========================== */ @@ -728,18 +1071,18 @@ b32 collider_collision_boolean(struct collider_shape *shape0, struct collider_sh /* First point is support point in shape's general directions to eachother */ dir = v2_sub(starting_point(shape1), starting_point(shape0)); if (v2_is_zero(dir)) dir = V2(1, 0); - s.a = menkowski_point(shape0, shape1, dir); + s.a = get_menkowski_point(shape0, shape1, dir); /* Second point is support point towards origin */ dir = v2_neg(s.a); - p = menkowski_point(shape0, shape1, dir); + p = get_menkowski_point(shape0, shape1, dir); if (v2_dot(dir, p) >= 0) { s.b = s.a; s.a = p; while (true) { /* Third point is support point in direction of line normal towards origin */ dir = v2_perp_towards_dir(v2_sub(s.b, s.a), v2_neg(s.a)); - p = menkowski_point(shape0, shape1, dir); + p = get_menkowski_point(shape0, shape1, dir); if (v2_dot(dir, p) < 0) { /* New point did not cross origin, collision impossible */ break; diff --git a/src/collider.h b/src/collider.h index 4814513c..b4ea7a4e 100644 --- a/src/collider.h +++ b/src/collider.h @@ -12,15 +12,15 @@ struct v2 collider_support_point(struct collider_shape *a, struct xform xf, stru b32 collider_collision_boolean(struct collider_shape *shape0, struct collider_shape *shape1); #endif -struct collider_simplex { - u32 len; - struct v2 a, b, c; +struct collider_menkowski_point { + struct v2 p; /* Menkowski difference point */ + struct v2 s0; /* Support point of first shape in dir */ + struct v2 s1; /* Support point of second shape in -dir */ }; -struct collider_menkowski_point { - struct v2 p0; /* Support point of first shape in dir */ - struct v2 p1; /* Support point of second shape in -dir */ - struct v2 p; /* Menkowski difference point */ +struct collider_simplex { + u32 len; + struct collider_menkowski_point a, b, c; }; struct collider_collision_point { @@ -31,7 +31,6 @@ struct collider_collision_point { struct collider_prototype { struct v2 points[64]; u32 len; }; struct collider_collision_points_result { - struct v2 normal; struct collider_collision_point points[2]; u32 num_points; @@ -46,6 +45,20 @@ 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_prototype prototype; +}; + +struct collider_closest_points_result collider_closest_points(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1); + struct v2_array menkowski(struct arena *arena, struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1, u32 detail); struct v2_array cloud(struct arena *arena, struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1); diff --git a/src/entity.h b/src/entity.h index a7fed20d..8a00578c 100644 --- a/src/entity.h +++ b/src/entity.h @@ -113,6 +113,9 @@ struct collision_debug { struct contact_point points[2]; u32 num_points; + struct v2 closest0; + struct v2 closest1; + struct xform xf0; struct xform xf1; }; diff --git a/src/game.c b/src/game.c index e8dcb400..f325044c 100644 --- a/src/game.c +++ b/src/game.c @@ -650,6 +650,7 @@ INTERNAL void create_contacts(void) struct string fdkey = STRING_FROM_BUFFER(BUFFER_FROM_STRUCT(&lookup_hash)); struct entity_handle *dbg_ent_handle = fixed_dict_get(&G.collision_debug_lookup.dict, fdkey); if (!dbg_ent_handle) { + /* FIXME: Handle never released */ dbg_ent_handle = arena_push_zero(&G.collision_debug_lookup.arena, struct entity_handle); } @@ -689,6 +690,13 @@ INTERNAL void create_contacts(void) dbg->xf0 = e0_xf; dbg->xf1 = e1_xf; + + /* Update closest points */ + { + struct collider_closest_points_result closest_points_res = collider_closest_points(&e0_collider, &e1_collider, e0_xf, e1_xf); + dbg->closest0 = closest_points_res.p0; + dbg->closest1 = closest_points_res.p1; + } } #endif } @@ -1450,17 +1458,70 @@ INTERNAL void integrate_positions_from_velocities(f32 dt) +/* ========================== * + * TESTING TOI + * ========================== */ +#if 0 +/* Takes 2 shapes and their xforms at t=0 and t=1. + * Returns time of impact in range [0, 1]. */ +INTERNAL f32 toi(struct collider_shape c0, struct collider_shape c1, + struct xform xf0_t0, struct xform xf1_t0, + struct xform xf0_t1, struct xform xf1_t1, + f32 tolerance) +{ + /* Find direction p0 -> p1 at t=0 */ + struct v2 dir; + struct v2 dir_neg; + { + struct collider_closest_points_result closest_points_res = collider_closest_points(&c0, &c1, xf0_t0, xf1_t0); + if (closest_points_res.colliding) { + /* Shapes are penetrating at t=0 */ + return 0; + } + dir = v2_norm(v2_sub(closest_points_res.p1, closest_points_res.p0)); + dir_neg = v2_neg(dir); + } + /* Safety check that shapes penetrate at t=1 */ + f32 sep; + { + struct v2 p0 = collider_support_point(&c0, xf0_t1, dir); + struct v2 p1 = collider_support_point(&c1, xf1_t1, dir_neg); + sep = v2_dot(dir, v2_sub(p1, p0)); + if (sep > tolerance) { + /* Shapes are not penetrating at t=1 */ + return 1; + } + } + /* Root finding (bisection) */ + f32 t0 = 0.0; + f32 t1 = 1.0; + f32 t = 0.5f; + while (math_fabs(sep) > tolerance) { + struct xform xf0 = xform_lerp(xf0_t0, xf0_t1, t); + struct xform xf1 = xform_lerp(xf1_t0, xf1_t1, t); + struct v2 p0 = collider_support_point(&c0, xf0, dir); + struct v2 p1 = collider_support_point(&c1, xf1, dir_neg); + sep = v2_dot(dir, v2_sub(p1, p0)); + /* Update bracket */ + if (sep > 0) { + t0 = t; + } else { + t1 = t; + } + t = (t1 - t0) / 2.0; + } - - + return t; +} +#endif diff --git a/src/user.c b/src/user.c index ce6f62fc..e0b4765b 100644 --- a/src/user.c +++ b/src/user.c @@ -1188,7 +1188,7 @@ INTERNAL void user_update(void) u32 color_third = RGBA_32_F(0, 0, 1, 0.75); struct collider_simplex simplex = collider_res.simplex; - struct v2 simplex_points[] = { simplex.a, simplex.b, simplex.c }; + struct v2 simplex_points[] = { simplex.a.p, simplex.b.p, simplex.c.p }; 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 = simplex.len, .points = simplex_points }; @@ -1299,22 +1299,31 @@ INTERNAL void user_update(void) u32 color_line = RGBA_32_F(1, 0, 1, 0.5); u32 color_a = RGBA_32_F(1, 0, 0, 0.5); u32 color_b = RGBA_32_F(0, 1, 0, 0.5); - struct collider_collision_points_result res = collider_res; { - struct v2 a = xform_mul_v2(G.world_view, res.a0); - struct v2 b = xform_mul_v2(G.world_view, res.b0); + struct v2 a = xform_mul_v2(G.world_view, collider_res.a0); + struct v2 b = xform_mul_v2(G.world_view, collider_res.b0); draw_solid_line(G.viewport_canvas, a, b, thickness, color_line); draw_solid_circle(G.viewport_canvas, a, radius, color_a, 10); draw_solid_circle(G.viewport_canvas, b, radius, color_b, 10); } { - struct v2 a = xform_mul_v2(G.world_view, res.a1); - struct v2 b = xform_mul_v2(G.world_view, res.b1); + struct v2 a = xform_mul_v2(G.world_view, collider_res.a1); + struct v2 b = xform_mul_v2(G.world_view, collider_res.b1); draw_solid_line(G.viewport_canvas, a, b, thickness, color_line); draw_solid_circle(G.viewport_canvas, a, radius, color_a, 10); draw_solid_circle(G.viewport_canvas, b, radius, color_b, 10); } } + + /* Draw closest points */ + { + f32 radius = 4; + u32 color = RGBA_32_F(1, 1, 0, 0.5); + struct v2 a = xform_mul_v2(G.world_view, data->closest0); + struct v2 b = xform_mul_v2(G.world_view, data->closest1); + draw_solid_circle(G.viewport_canvas, a, radius, color, 10); + draw_solid_circle(G.viewport_canvas, b, radius, color, 10); + } #endif } #endif