From c3b96d1597167826475482945b52e178a87136bd Mon Sep 17 00:00:00 2001 From: jacob Date: Tue, 3 Sep 2024 14:30:42 -0500 Subject: [PATCH] join logic for gjk expansion into one function --- src/entity.h | 2 +- src/game.c | 17 +- src/user.c | 2 +- src/util.c | 686 +++++++++++++++++++++++---------------------------- src/util.h | 14 +- 5 files changed, 331 insertions(+), 390 deletions(-) diff --git a/src/entity.h b/src/entity.h index 7dca2462..79ef2825 100644 --- a/src/entity.h +++ b/src/entity.h @@ -94,7 +94,7 @@ struct entity { /* TODO: Remove this (testing) */ b32 colliding; struct entity_handle colliding_with; - struct gjk_extended_simplex simplex; + struct gjk_simplex simplex; struct v2 point; diff --git a/src/game.c b/src/game.c index 2f24daa8..48adc723 100644 --- a/src/game.c +++ b/src/game.c @@ -778,7 +778,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) struct v2 point0 = V2(0, 0); struct v2 point1 = V2(0, 0); struct entity *colliding_with = entity_nil(); - struct gjk_extended_simplex final_simplex = { 0 }; + struct gjk_simplex final_simplex = { 0 }; for (u64 e1_index = 0; e1_index < store->reserved; ++e1_index) { struct entity *e1 = &store->entities[e1_index]; if (e1 == e0) continue; @@ -799,12 +799,25 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) }; } - struct gjk_extended_result res = gjk_extended(e0_poly, e1_poly, G.gjk_steps); +#if 0 + struct gjk_simplex res = gjk_extended(e0_poly, e1_poly, G.gjk_steps); colliding = res.colliding; point0 = res.p0; point1 = res.p1; colliding_with = e1; final_simplex = res.final_simplex; +#else + struct v2 pendir = V2(0, 0); + //struct v2 pendir = V2(0, 99999); + + struct gjk_extended_result res = gjk_extended(e0_poly, e1_poly, pendir); + + colliding = res.colliding; + point0 = res.p0; + point1 = res.p1; + colliding_with = e1; + //final_simplex = res.simplex; +#endif if (colliding) { diff --git a/src/user.c b/src/user.c index 7fe6c5d4..3a5e6d67 100644 --- a/src/user.c +++ b/src/user.c @@ -1050,7 +1050,7 @@ INTERNAL void user_update(void) u32 color_second = RGBA_32_F(0, 1, 0, 0.75); u32 color_third = RGBA_32_F(0, 0, 1, 0.75); - struct gjk_extended_simplex simplex = ent->simplex; + struct gjk_simplex simplex = ent->simplex; 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 }; diff --git a/src/util.c b/src/util.c index e3274037..d9afaa9c 100644 --- a/src/util.c +++ b/src/util.c @@ -100,37 +100,28 @@ INTERNAL struct v2 poly_support(struct v2_array a, struct v2 dir) return furthest; } -INTERNAL struct v2 menkowski_point(struct v2_array poly0, struct v2_array poly1, struct v2 dir) +INTERNAL struct v2 menkowski_point(struct v2_array poly0, struct v2_array shape1, struct v2 dir) { - return v2_sub(poly_support(poly0, dir), poly_support(poly1, v2_neg(dir))); + return v2_sub(poly_support(poly0, dir), poly_support(shape1, v2_neg(dir))); } -INTERNAL struct gjk_menkowski_point extended_menkowski_point(struct v2_array poly0, struct v2_array poly1, struct v2 dir) -{ - struct gjk_menkowski_point res; - res.p0 = poly_support(poly0, dir); - res.p1 = poly_support(poly1, v2_neg(dir)); - res.p = v2_sub(res.p0, res.p1); - return res; -} - -b32 gjk_boolean(struct v2_array poly0, struct v2_array poly1) +b32 gjk_boolean(struct v2_array shape0, struct v2_array shape1) { struct { struct v2 a, b, c; } s = { 0 }; /* First point is support point in shape's general directions to eachother */ - s.a = menkowski_point(poly0, poly1, v2_sub(poly1.points[0], poly0.points[0])); + s.a = menkowski_point(shape0, shape1, v2_sub(shape1.points[0], shape0.points[0])); /* Second point is support point towards origin */ struct v2 dir = v2_neg(s.a); - struct v2 p = menkowski_point(poly0, poly1, dir); + struct v2 p = 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 = perp_towards_point(s.a, s.b, V2(0, 0)); - p = menkowski_point(poly0, poly1, dir); + p = menkowski_point(shape0, shape1, dir); if (v2_dot(dir, p) < 0) { /* New point did not cross origin, collision impossible */ break; @@ -171,400 +162,343 @@ b32 gjk_boolean(struct v2_array poly0, struct v2_array poly1) -#if 0 -struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array poly1, u32 max_steps) + + + + +INTERNAL struct gjk_menkowski_point menkowski_point_extended(struct v2_array poly0, struct v2_array poly1, struct v2 dir) { - struct temp_arena scratch = scratch_begin_no_conflict(); - b32 colliding = false; - struct v2 shape0_p = V2(0, 0); - struct v2 shape1_p = V2(0, 0); - u32 step = 0; + struct gjk_menkowski_point res; + res.p0 = poly_support(poly0, dir); + res.p1 = poly_support(poly1, v2_neg(dir)); + res.p = v2_sub(res.p0, res.p1); + return res; +} + +struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1, struct v2 penetration_dir) +{ + struct gjk_extended_result res = { 0 }; /* TODO: Verify epsilon */ f32 unique_epsilon = 0.00001; + b32 use_penetration_dir = false; + struct gjk_simplex s = { 0 }; - /* Simplex */ - struct gjk_extended_simplex s = { 0 }; + /* ========================== * + * Collision check + * ========================== */ + struct v2 dir = { 0 }; + struct gjk_menkowski_point m = { 0 }; + { + /* 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; - /* First point is support point towards shape centers */ - if (step++ >= max_steps) goto abort; - struct v2 dir = v2_sub(poly1.points[0], poly0.points[0]); - struct gjk_menkowski_point m = extended_menkowski_point(poly0, poly1, dir); - s.a = m; - s.len = 1; - - while (true) { /* Second point is support point towards origin */ - if (s.len == 1) { - if (step++ >= max_steps) goto abort; - m = extended_menkowski_point(poly0, poly1, v2_neg(s.a.p)); - if (v2_eq(m.p, s.a.p)) { - break; - } - + dir = v2_neg(s.a.p); + m = menkowski_point_extended(shape0, shape1, dir); + if (v2_dot(dir, m.p) >= 0) { s.b = s.a; s.a = m; s.len = 2; - /* Third point is support point in direction of line normal towards origin */ - dir = perp_towards_point(s.a.p, s.b.p, V2(0, 0)); - } - - if (step++ >= max_steps) goto abort; - m = extended_menkowski_point(poly0, poly1, dir); - - if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_sub(m.p, s.a.p))) < unique_epsilon) { - /* New point is already on the current line */ - break; - } - - s.c = s.b; - s.b = s.a; - s.a = m; - s.len = 3; - - if (step++ >= max_steps) goto abort; - - struct v2 rab_dir = v2_neg(perp_towards_point(s.a.p, s.b.p, s.c.p)); - struct v2 rac_dir = v2_neg(perp_towards_point(s.a.p, s.c.p, s.b.p)); - struct v2 rbc_dir = v2_neg(perp_towards_point(s.b.p, s.c.p, s.a.p)); - - b32 rab = v2_dot(rab_dir, v2_neg(s.a.p)) >= 0; - b32 rac = v2_dot(rac_dir, v2_neg(s.a.p)) >= 0; - b32 rbc = v2_dot(rbc_dir, v2_neg(s.b.p)) >= 0; - - if (!rab && !rac && !rbc) { - colliding = true; - break; - } - - /* Remove point or edge and determine next direction based on vornoi regions */ - b32 ra = rab && rac; - b32 rb = rab && rbc; - b32 rc = rac && rbc; - rab = rab && !ra && !rb; - rac = rac && !ra && !rc; - rbc = rbc && !rb && !rc; - if (rab) { - /* Remove c */ - dir = rab_dir; - s.len = 2; - } else if (rac) { - /* Remove b */ - dir = rac_dir; - s.b = s.c; - s.len = 2; - } else if (rbc) { - /* Remove a */ - dir = rbc_dir; - s.a = s.b; - s.b = s.c; - s.len = 2; - } else if (ra) { - /* Remove bc */ - s.len = 1; - } else if (rb) { - /* Remove ac */ - s.a = s.b; - s.len = 1; - } else if (rc) { - /* Remove ab */ - s.a = s.c; - s.len = 1; - } - } - - if (colliding) { - struct gjk_menkowski_point *proto = arena_dry_push(scratch.arena, struct gjk_menkowski_point); - u32 proto_count = 0; - - { - struct gjk_menkowski_point *tmp = arena_push_array(scratch.arena, struct gjk_menkowski_point, 3); - proto_count = s.len; - tmp[0] = s.a; - if (proto_count >= 2) { - tmp[1] = s.b; - if (proto_count >= 3) { - tmp[2] = s.c; + while (true) { + /* Third point is support point in direction of line normal towards origin */ + dir = perp_towards_point(s.a.p, s.b.p, V2(0, 0)); + m = menkowski_point_extended(shape0, shape1, dir); + if (v2_dot(dir, m.p) < 0) { + /* New point did not cross origin, collision impossible */ + break; } - } - } - while (true) { - if (step++ >= max_steps) goto abort; + s.c = s.b; + s.b = s.a; + s.a = m; + s.len = 3; - struct v2 pen = V2(0, 0); - 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); - - struct v2 vsd = v2_mul(vse, (v2_dot(vso, vse) / v2_dot(vse, vse))); - 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; - } - } - - /* Find new point in dir */ - m = extended_menkowski_point(poly0, poly1, pen); - - /* TODO: Move to break */ - s.a = proto[pen_ps_index]; - s.b = proto[pen_pe_index]; - s.len = 2; - - /* 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))) < unique_epsilon) { - unique = false; + dir = v2_neg(perp_towards_point(s.a.p, s.b.p, s.c.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) { + /* Point is in region ab, remove c from simplex */ + s.len = 2; + } else { + /* Point is not in region ab */ + dir = v2_neg(perp_towards_point(s.a.p, s.c.p, s.b.p)); /* Normal 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; + s.len = 2; + } else { + res.colliding = true; break; } } - if (!unique) { - break; + } + } + } + + if (res.colliding) { + use_penetration_dir = !v2_eq(penetration_dir, V2(0, 0)); + if (use_penetration_dir) { + /* ========================== * + * Penetration dir expansion + * ========================== */ + + while (true) { + /* Second point is support point towards penetration_dir */ + if (s.len == 1) { + 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; + } + + 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 = perp_towards_dir(s.a.p, s.b.p, penetration_dir); + } + + if (s.len == 2) { + 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))) < unique_epsilon) { + /* New point is already on the current line */ + break; + } + + s.c = s.b; + s.b = s.a; + s.a = m; + s.len = 3; + } + + 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 = perp_towards_dir(s.a.p, s.b.p, penetration_dir); + s.len = 2; + } else if (b_wedgesign != c_wedgesign) { + /* Remove b */ + dir = perp_towards_dir(s.a.p, s.c.p, penetration_dir); + s.b = s.c; + s.len = 2; + } else { + /* Remove a */ + dir = perp_towards_dir(s.b.p, s.c.p, penetration_dir); + s.a = s.b; + s.b = s.c; + s.len = 2; + } + } + } else { + /* ========================== * + * Epa expansion + * ========================== */ + + ASSERT(s.len == 3); + struct temp_arena scratch = scratch_begin_no_conflict(); + + struct gjk_menkowski_point *proto = arena_dry_push(scratch.arena, struct gjk_menkowski_point); + u32 proto_count = 0; + + { + struct gjk_menkowski_point *tmp = arena_push_array(scratch.arena, struct gjk_menkowski_point, 3); + proto_count = s.len; + tmp[0] = s.a; + if (proto_count >= 2) { + tmp[1] = s.b; + if (proto_count >= 3) { + tmp[2] = s.c; + } } } - /* 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]; + while (true) { + struct v2 pen = V2(0, 0); + 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); + + struct v2 vsd = v2_mul(vse, (v2_dot(vso, vse) / v2_dot(vse, vse))); + 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; + } + } + + /* Find new point in dir */ + m = menkowski_point_extended(shape0, shape1, pen); + + /* 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))) < unique_epsilon) { + unique = false; + break; + } + } + if (!unique) { + s.a = proto[pen_ps_index]; + s.b = proto[pen_pe_index]; + s.len = 2; + 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; + } + + scratch_end(scratch); + } + } else { + /* ========================== * + * Closest point expansion + * ========================== */ + + if (s.len == 2) { + /* Third point is support point in direction of line normal towards `a` projected onto penetration_dir */ + dir = perp_towards_dir(s.a.p, s.b.p, penetration_dir); + } + + while (true) { + /* Second point is support point towards origin */ + if (s.len == 1) { + m = menkowski_point_extended(shape0, shape1, v2_neg(s.a.p)); + if (v2_eq(m.p, s.a.p)) { + break; + } + + s.b = s.a; + s.a = m; + s.len = 2; + + /* Third point is support point in direction of line normal towards origin */ + dir = perp_towards_point(s.a.p, s.b.p, V2(0, 0)); + } + + if (s.len == 2) { + 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))) < unique_epsilon) { + /* New point is already on the current line */ + break; + } + + s.c = s.b; + s.b = s.a; + s.a = m; + s.len = 3; + } + + /* Remove point or edge and determine next direction based on vornoi regions */ + i32 code = 0; + struct v2 rab_dir = v2_neg(perp_towards_point(s.a.p, s.b.p, s.c.p)); + struct v2 rac_dir = v2_neg(perp_towards_point(s.a.p, s.c.p, s.b.p)); + struct v2 rbc_dir = v2_neg(perp_towards_point(s.b.p, s.c.p, s.a.p)); + code |= ((v2_dot(rab_dir, v2_neg(s.a.p)) >= 0) << 0); /* Regions ab, a, and b*/ + code |= ((v2_dot(rac_dir, v2_neg(s.a.p)) >= 0) << 1); /* Regions ac, a, and c */ + code |= ((v2_dot(rbc_dir, v2_neg(s.b.p)) >= 0) << 2); /* Regions bc, b, and c */ + switch (code) { + case 1: { /* Region ab, remove c */ + dir = rab_dir; + s.len = 2; + } break; + case 2: { /* Region ac, remove b */ + dir = rac_dir; + s.b = s.c; + s.len = 2; + } break; + case 4: { /* Region bc, remove a */ + dir = rbc_dir; + 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; } - proto[pen_pe_index] = m; } } - { - if (s.len == 1) { - shape0_p = s.a.p0; - shape1_p = s.a.p1; - } else if (s.len == 2) { + /* Resolve points */ + if (s.len == 1) { + res.p0 = s.a.p0; + res.p1 = s.a.p1; + } else if (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); - f32 ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1); - /* Determine point on 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); - } - /* Determine point on 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); - } + ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1); + } + /* Determine point on shape 0 */ + { + res.p0 = v2_sub(s.b.p0, s.a.p0); + res.p0 = v2_mul(res.p0, ratio); + res.p0 = v2_add(res.p0, s.a.p0); + } + /* Determine point on shape 1 */ + { + res.p1 = v2_sub(s.b.p1, s.a.p1); + res.p1 = v2_mul(res.p1, ratio); + res.p1 = v2_add(res.p1, s.a.p1); } } -abort: - struct gjk_extended_result res = { 0 }; - res.colliding = colliding; - res.p0 = shape0_p; - res.p1 = shape1_p; - res.final_simplex = s; - scratch_end(scratch); return res; } -#else - -INTERNAL b32 ray_intersects_line(struct v2 p0, struct v2 p1, struct v2 ray_dir) -{ - return (v2_dot(ray_dir, p0) >= 0 || v2_dot(ray_dir, p1) >= 0) && math_fsign(v2_wedge(ray_dir, p0)) != math_fsign(v2_wedge(ray_dir, p1)); -} - -struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array poly1, u32 max_steps) -{ - struct v2 movedir = V2(0, 999999); - - - - struct temp_arena scratch = scratch_begin_no_conflict(); - b32 colliding = false; - struct v2 shape0_p = V2(0, 0); - struct v2 shape1_p = V2(0, 0); - u32 step = 0; - - /* TODO: Verify epsilon */ - f32 unique_epsilon = 0.00001; - - /* Simplex */ - struct gjk_extended_simplex s = { 0 }; - - /* First point is support point towards shape centers */ - if (step++ >= max_steps) goto abort; - struct v2 dir = v2_sub(poly1.points[0], poly0.points[0]); - struct gjk_menkowski_point m = extended_menkowski_point(poly0, poly1, dir); - s.a = m; - s.len = 1; - - while (true) { - /* Second point is support point towards movedir */ - if (s.len == 1) { - if (step++ >= max_steps) goto abort; -#if 0 - m = extended_menkowski_point(poly0, poly1, movedir); - if (v2_eq(m.p, s.a.p)) { - break; - } -#else - dir = v2_sub(v2_mul(movedir, v2_dot(movedir, s.a.p) / v2_dot(movedir, movedir)), s.a.p); - m = extended_menkowski_point(poly0, poly1, dir); - if (v2_eq(m.p, s.a.p)) { - break; - } -#endif - - s.b = s.a; - s.a = m; - s.len = 2; - - /* Third point is support point in direction of line normal towards `a` projected onto movedir */ - dir = perp_towards_dir(s.a.p, s.b.p, movedir); - } - - if (step++ >= max_steps) goto abort; - m = extended_menkowski_point(poly0, poly1, dir); - - if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_sub(m.p, s.a.p))) < unique_epsilon) { - /* New point is already on the current line */ - break; - } - - s.c = s.b; - s.b = s.a; - s.a = m; - s.len = 3; - - if (step++ >= max_steps) goto abort; - - /* TODO: Can group dots & wedges from intersect checks */ - b32 abi = ray_intersects_line(s.a.p, s.b.p, movedir); - b32 aci = ray_intersects_line(s.a.p, s.c.p, movedir); - b32 bci = ray_intersects_line(s.b.p, s.c.p, movedir); - - - struct v2 ab_dir = perp_towards_dir(s.a.p, s.b.p, movedir); - struct v2 ac_dir = perp_towards_dir(s.a.p, s.c.p, movedir); - struct v2 bc_dir = perp_towards_dir(s.b.p, s.c.p, movedir); - - if (abi) { - /* Remove c */ - dir = ab_dir; - s.len = 2; - } else if (aci) { - /* Remove b */ - dir = ac_dir; - s.b = s.c; - s.len = 2; - } else if (bci) { - /* Remove a */ - dir = bc_dir; - s.a = s.b; - s.b = s.c; - s.len = 2; - } - } - -#if 0 - { - if (s.len == 1) { - shape0_p = s.a.p0; - shape1_p = s.a.p1; - } else if (s.len == 2) { - /* Determine ratio between edge a & b that projected origin lies */ - /* FIXME: Winding order dependent? */ - 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); - /* Determine point on 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); - } - /* Determine point on 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 - { - if (s.len == 1) { - shape0_p = s.a.p0; - shape1_p = s.a.p1; - } else if (s.len == 2) { - /* Determine ratio between edge a & b that ray intersection lies */ - /* FIXME: Winding order dependent? */ - f32 wedgea = math_fabs(v2_wedge(movedir, s.a.p)); - f32 wedgeb = math_fabs(v2_wedge(movedir, s.b.p)); - f32 ratio = wedgea / (wedgea + wedgeb); - /* Determine point on 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); - } - /* Determine point on 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: - struct gjk_extended_result res = { 0 }; - res.colliding = colliding; - res.p0 = shape0_p; - res.p1 = shape1_p; - res.final_simplex = s; - scratch_end(scratch); - return res; -} - -#endif - - - - - - - - @@ -577,7 +511,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 = extended_menkowski_point(poly0, poly1, dir).p; + struct v2 p = menkowski_point_extended(poly0, poly1, dir).p; if (res.count == 0 || !v2_eq(p, res.points[res.count - 1])) { *arena_push(arena, struct v2) = p; ++res.count; diff --git a/src/util.h b/src/util.h index cf6c9162..f8f7f067 100644 --- a/src/util.h +++ b/src/util.h @@ -184,33 +184,27 @@ INLINE void sleep_frame(sys_timestamp_t last_frame_time, f64 target_dt) * Collision testing * ========================== */ -/* TODO: Remove this */ - struct gjk_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 gjk_extended_simplex { +struct gjk_simplex { u32 len; struct gjk_menkowski_point a, b, c; }; struct gjk_extended_result { b32 colliding; - struct v2 p0; /* Closest / deepest point on first shape's edge */ - struct v2 p1; /* Closest / deepest point on second shape's edge */ - - /* For debugging */ - struct gjk_extended_simplex final_simplex; + struct v2 p0, p1; }; struct v2 perp_towards_point(struct v2 start, struct v2 end, struct v2 p); struct v2 perp_towards_dir(struct v2 start, struct v2 end, struct v2 dir); struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1); i32 poly_get_winding_order(struct v2_array poly); -b32 gjk_boolean(struct v2_array poly0, struct v2_array poly1); -struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array poly1, u32 max_steps); +b32 gjk_boolean(struct v2_array shape0, struct v2_array shape1); +struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1, struct v2 penetration_dir); #endif