From 8fd92c55a1e9c06aad023fa5ffc68e6d9b2dd093 Mon Sep 17 00:00:00 2001 From: jacob Date: Fri, 30 Aug 2024 13:39:28 -0500 Subject: [PATCH] gjk expanded testing non-colliding closest face --- src/entity.h | 2 +- src/game.c | 119 +++++++---- src/user.c | 10 +- src/util.c | 555 +++++++++++++++++++++++++++++++++++++++------------ src/util.h | 22 +- 5 files changed, 523 insertions(+), 185 deletions(-) diff --git a/src/entity.h b/src/entity.h index cb3bf393..1cd6b007 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 simplex simplex; + struct gjk_extended_simplex simplex; struct v2 pen; struct v2 spot; diff --git a/src/game.c b/src/game.c index 67fb68d7..fbf15283 100644 --- a/src/game.c +++ b/src/game.c @@ -682,46 +682,6 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) * ========================== */ #if 0 - for (u64 e0_index = 0; e0_index < store->reserved; ++e0_index) { - struct entity *e0 = &store->entities[e0_index]; - if (!(e0->valid && entity_has_prop(e0, ENTITY_PROP_ACTIVE))) continue; - if (!entity_has_prop(e0, ENTITY_PROP_PHYSICAL)) continue; - - struct xform e0_xf = entity_get_xform(e0); - struct quad e0_quad; - { - struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, e0->sprite); - struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("shape"), e0->animation_frame); - e0_quad = xform_mul_quad(e0->sprite_local_xform, quad_from_rect(slice.rect)); - e0_quad = xform_mul_quad(e0_xf, e0_quad); - } - - b32 colliding = false; - for (u64 e1_index = 0; e1_index < store->reserved; ++e1_index) { - struct entity *e1 = &store->entities[e1_index]; - if (e1 == e0) continue; - if (!(e1->valid && entity_has_prop(e1, ENTITY_PROP_ACTIVE))) continue; - if (!entity_has_prop(e1, ENTITY_PROP_PHYSICAL)) continue; - - struct xform e1_xf = entity_get_xform(e1); - struct quad e1_quad; - { - struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, e1->sprite); - struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("shape"), e1->animation_frame); - e1_quad = xform_mul_quad(e1->sprite_local_xform, quad_from_rect(slice.rect)); - e1_quad = xform_mul_quad(e1_xf, e1_quad); - } - - struct gjk_result res = gjk(e0_quad, e1_quad); - if (res.colliding) { - colliding = true; - break; - } - } - - e0->colliding = colliding; - } -#else for (u64 e0_index = 0; e0_index < store->reserved; ++e0_index) { struct entity *e0 = &store->entities[e0_index]; if (!(e0->valid && entity_has_prop(e0, ENTITY_PROP_ACTIVE))) continue; @@ -743,7 +703,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) } b32 colliding = false; - struct simplex simplex = { 0 }; + struct gjk_extended_simplex simplex = { 0 }; struct v2 pen = V2(0, 0); struct v2 spot = V2(0, 0); struct entity_handle colliding_with = { 0 }; @@ -767,13 +727,86 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) }; } - struct gjk_result res = gjk(e0_poly, e1_poly, G.gjk_steps); + colliding = gjk_boolean(e0_poly, e1_poly, G.gjk_steps); + colliding_with = e1->handle; + //simplex = res.final_simplex; + + if (colliding) { + //pen = epa_colliding_pen(e0_poly, e1_poly, simplex); +#if 0 + /* Pen movement test */ + { + struct xform xf = e1_xf; + xf.og = v2_add(xf.og, pen); + entity_set_xform(e1, xf); + //e1->verlet_xform.og = v2_add(e1->verlet_xform.og, pen); + } +#endif + } else { + spot = e0->spot; + } + } + + e0->colliding = colliding; + e0->colliding_with = colliding_with; + e0->simplex = simplex; + e0->pen = pen; + e0->spot = spot; + } +#else + for (u64 e0_index = 0; e0_index < store->reserved; ++e0_index) { + struct entity *e0 = &store->entities[e0_index]; + if (!(e0->valid && entity_has_prop(e0, ENTITY_PROP_ACTIVE))) continue; + if (!entity_has_prop(e0, ENTITY_PROP_PHYSICAL)) continue; + if (!entity_has_prop(e0, ENTITY_PROP_PLAYER_CONTROLLED)) continue; + + struct xform e0_xf = entity_get_xform(e0); + struct quad e0_quad; + struct v2_array e0_poly; + { + struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, e0->sprite); + struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("shape"), e0->animation_frame); + e0_quad = xform_mul_quad(e0->sprite_local_xform, quad_from_rect(slice.rect)); + e0_quad = xform_mul_quad(e0_xf, e0_quad); + e0_poly = (struct v2_array) { + .count = ARRAY_COUNT(e0_quad.e), + .points = e0_quad.e + }; + } + + b32 colliding = false; + struct gjk_extended_simplex simplex = { 0 }; + struct v2 pen = V2(0, 0); + struct v2 spot = V2(0, 0); + struct entity_handle colliding_with = { 0 }; + for (u64 e1_index = 0; e1_index < store->reserved; ++e1_index) { + struct entity *e1 = &store->entities[e1_index]; + if (e1 == e0) continue; + if (!(e1->valid && entity_has_prop(e1, ENTITY_PROP_ACTIVE))) continue; + if (!entity_has_prop(e1, ENTITY_PROP_PHYSICAL)) continue; + + struct xform e1_xf = entity_get_xform(e1); + struct quad e1_quad; + struct v2_array e1_poly; + { + struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, e1->sprite); + struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("shape"), e1->animation_frame); + e1_quad = xform_mul_quad(e1->sprite_local_xform, quad_from_rect(slice.rect)); + e1_quad = xform_mul_quad(e1_xf, e1_quad); + e1_poly = (struct v2_array) { + .count = ARRAY_COUNT(e1_quad.e), + .points = e1_quad.e + }; + } + + struct gjk_extended_result res = gjk_extended(e0_poly, e1_poly, G.gjk_steps); colliding = res.colliding; colliding_with = e1->handle; simplex = res.final_simplex; + pen = res.colliding_pen; if (colliding) { - pen = epa(e0_poly, e1_poly, simplex); + //pen = epa_colliding_pen(e0_poly, e1_poly, simplex); #if 0 /* Pen movement test */ { diff --git a/src/user.c b/src/user.c index fbd2176f..352949cc 100644 --- a/src/user.c +++ b/src/user.c @@ -1034,14 +1034,14 @@ INTERNAL void user_update(void) /* Draw simplex */ { - f32 thickness = 5; + f32 thickness = 2; u32 line_color = colliding ? COLOR_WHITE: COLOR_YELLOW; u32 color_first = RGBA_32_F(1, 0, 0, 0.5); u32 color_second = RGBA_32_F(0, 1, 0, 0.5); u32 color_third = RGBA_32_F(0, 0, 1, 0.5); - struct simplex simplex = ent->simplex; - struct v2 simplex_points[] = { simplex.a, simplex.b, simplex.c }; + struct gjk_extended_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 }; @@ -1051,13 +1051,15 @@ INTERNAL void user_update(void) } if (simplex.len >= 2) { u32 color = simplex.len == 2 ? color_first : color_second; - draw_solid_poly_line(G.viewport_canvas, simplex_array, simplex.len > 2, thickness, line_color); draw_solid_circle(G.viewport_canvas, simplex_array.points[1], thickness * 2, color, 10); } if (simplex.len >= 3) { u32 color = color_first; draw_solid_circle(G.viewport_canvas, simplex_array.points[2], thickness * 2, color, 10); } + if (simplex.len >= 2) { + draw_solid_poly_line(G.viewport_canvas, simplex_array, simplex.len > 2, thickness, line_color); + } } /* Draw pen */ diff --git a/src/util.c b/src/util.c index 5879b5a2..eb59afad 100644 --- a/src/util.c +++ b/src/util.c @@ -47,6 +47,7 @@ struct string util_file_name_from_path(struct string path) struct v2 poly_support(struct v2_array a, struct v2 dir) { + /* TODO: Could probably binary search for largest dot since shape is convex */ struct v2 furthest = a.points[0]; f32 furthest_dot = v2_dot(dir, furthest); for (u32 i = 1; i < a.count; ++i) { @@ -60,9 +61,13 @@ struct v2 poly_support(struct v2_array a, struct v2 dir) return furthest; } -struct v2 menkowski_point(struct v2_array poly0, struct v2_array poly1, struct v2 dir) +INTERNAL struct gjk_menkowski_point menkowski_point(struct v2_array poly0, struct v2_array poly1, struct v2 dir) { - return v2_sub(poly_support(poly0, dir), poly_support(poly1, v2_neg(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; } struct v2 perp_towards_point(struct v2 start, struct v2 end, struct v2 p) @@ -72,40 +77,80 @@ struct v2 perp_towards_point(struct v2 start, struct v2 end, struct v2 p) return v2_mul(perp, sign); } -struct gjk_result gjk(struct v2_array poly0, struct v2_array poly1, u32 max_steps) +/* TODO: Remove this (debugging) */ +struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1) +{ + struct v2_array res = { .points = arena_dry_push(arena, struct v2) }; + u64 rays = 500; + for (u64 i = 0; i < rays; ++i) { + f32 angle = ((f32)i / rays) * (2 * PI); + struct v2 dir = v2_from_angle(angle); + struct v2 p = menkowski_point(poly0, poly1, dir).p; + if (res.count == 0 || !v2_eq(p, res.points[res.count - 1])) { + *arena_push(arena, struct v2) = p; + ++res.count; + } + } + return res; +} + +/* Returns 1 if winding forward, -1 if backward */ +i32 poly_get_winding_order(struct v2_array poly) +{ + i32 res; + if (poly.count >= 3) { + struct v2 a = poly.points[0]; + struct v2 b = poly.points[1]; + struct v2 c = poly.points[2]; + if (v2_wedge(v2_sub(b, a), v2_sub(c, b)) > 0) { + res = 1; + } else { + res = -1; + } + } else { + res = -1; + } + return res; +} + + + + + + + +b32 gjk_boolean(struct v2_array poly0, struct v2_array poly1, u32 max_steps) { b32 colliding = false; u32 step = 0; /* Simplex */ - struct simplex s = { 0 }; + struct { struct v2 a, b, c; } s = { 0 }; /* First point is support point towards shape centers */ if (step++ >= max_steps) goto abort; - { - s.a = menkowski_point(poly0, poly1, v2_norm(v2_sub(poly1.points[0], poly0.points[0]))); - s.len = 1; - } + s.a = menkowski_point(poly0, poly1, v2_sub(poly1.points[0], poly0.points[0])).p; /* Second point is support point towards origin */ if (step++ >= max_steps) goto abort; + b32 valid_line = false; { struct v2 dir = v2_neg(s.a); - struct v2 p = menkowski_point(poly0, poly1, dir); + struct v2 p = menkowski_point(poly0, poly1, dir).p; if (v2_dot(dir, p) >= 0) { s.b = s.a; s.a = p; - s.len = 2; + valid_line = true; } } - if (s.len == 2) { + if (valid_line) { while (true) { if (step++ >= max_steps) goto abort; /* Third point is support point in direction of line normal towards origin */ struct v2 dir = perp_towards_point(s.a, s.b, V2(0, 0)); - struct v2 p = menkowski_point(poly0, poly1, dir); + struct v2 p = menkowski_point(poly0, poly1, dir).p; if (v2_dot(dir, p) < 0) { colliding = false; break; @@ -127,7 +172,6 @@ struct gjk_result gjk(struct v2_array poly0, struct v2_array poly1, u32 max_step s.b = s.c; } else { /* Point is in simplex */ - s.len = 3; colliding = true; break; } @@ -136,150 +180,403 @@ struct gjk_result gjk(struct v2_array poly0, struct v2_array poly1, u32 max_step } abort: - return (struct gjk_result) { - .colliding = colliding, - .final_simplex = s - }; + return colliding; } -struct v2 epa(struct v2_array poly0, struct v2_array poly1, struct simplex simplex) + + + + + + +#if 0 +struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array poly1, u32 max_steps) { struct temp_arena scratch = scratch_begin_no_conflict(); - ASSERT(simplex.len == 3); - - struct v2 *proto = arena_dry_push(scratch.arena, struct v2); - { - struct v2 *tmp = arena_push_array(scratch.arena, struct v2, 3); - tmp[0] = simplex.a; - tmp[1] = simplex.b; - tmp[2] = simplex.c; - } - u32 proto_count = 3; - + b32 colliding = false; struct v2 pen = V2(0, 0); - f32 pen_len = F32_INFINITY; - while (true) { - pen = V2(0, 0); - pen_len = F32_INFINITY; + u32 step = 0; - /* Find dir from origin to closest edge */ - u32 pen_pe_index = 1; - 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]; - struct v2 pe = proto[pe_index]; + /* Simplex */ + struct gjk_extended_simplex s = { 0 }; - struct v2 vse = v2_sub(pe, ps); - struct v2 vso = v2_neg(ps); + /* 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 menkowski_result m = menkowski_point(poly0, poly1, dir); + s.a = m.p; + s.a0 = m.p0; + s.a1 = m.p1; + s.len = 1; - struct v2 vsd = v2_mul(vse, (v2_dot(vso, vse) / v2_dot(vse, vse))); - struct v2 pd = v2_add(ps, vsd); - - /* TODO: sq cmp */ - f32 pd_len = v2_len(pd); - if (pd_len < pen_len) { - pen_pe_index = pe_index; - pen = pd; - pen_len = pd_len; - } + /* Second point is support point towards origin */ + if (step++ >= max_steps) goto abort; + { + dir = v2_neg(s.a); + m = menkowski_point(poly0, poly1, dir); + if (v2_dot(dir, m.p) >= 0) { + s.b = s.a; + s.b0 = s.a0; + s.b1 = s.a1; + s.a = m.p; + s.a0 = m.p0; + s.a1 = m.p1; + s.len = 2; } + } - /* Find new point in dir */ -#if 1 - /* FIXME: Ensure convexity */ + if (s.len == 2) { + while (true) { + if (step++ >= max_steps) goto abort; - struct v2 p = menkowski_point(poly0, poly1, pen); + /* Third point is support point in direction of line normal towards origin */ + dir = perp_towards_point(s.a, s.b, V2(0, 0)); + m = menkowski_point(poly0, poly1, dir); + if (v2_dot(dir, m.p) < 0) { + colliding = false; + break; + } - /* Check unique */ - /* TODO: Better */ - /* TODO: Epsilon or iteration limit */ - { - b32 unique = true; - for (u32 i = 0; i < proto_count; ++i) { - if (v2_eq(p, proto[i])) { - unique = false; + s.c = s.b; + s.c0 = s.b0; + s.c1 = s.b1; + s.b = s.a; + s.b0 = s.a0; + s.b1 = s.a1; + s.a = m.p; + s.a0 = m.p0; + s.a1 = m.p1; + s.len = 3; + + dir = v2_neg(perp_towards_point(s.a, s.b, s.c)); /* Normal dir of ab pointing away from c */ + struct v2 a_to_origin = v2_neg(s.a); + if (v2_dot(dir, a_to_origin) >= 0) { + /* Point is in region ab, remove c from simplex (will happen automatically next iteration) */ + s.len = 2; + } else { + /* Point is not in region ab */ + dir = v2_neg(perp_towards_point(s.a, s.c, s.b)); /* Normal dir of ac pointing away from b */ + if (v2_dot(dir, a_to_origin) >= 0) { + /* Point is in region ac, remove b from simplex */ + s.b = s.c; + s.len = 2; + } else { + /* Point is in simplex */ + colliding = true; break; } } - if (!unique) { + } + } + + if (colliding) { /* Calculate penetration depth using epa */ + ASSERT(s.len == 3); + struct v2 *proto = arena_dry_push(scratch.arena, struct v2); + { + struct v2 *tmp = arena_push_array(scratch.arena, struct v2, 3); + tmp[0] = s.a; + tmp[1] = s.b; + tmp[2] = s.c; + } + u32 proto_count = 3; + + pen = V2(0, 0); + f32 pen_len = F32_INFINITY; + while (true) { + pen = V2(0, 0); + pen_len = F32_INFINITY; + + /* Find dir from origin to closest edge */ + u32 pen_pe_index = 1; + 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]; + struct v2 pe = proto[pe_index]; + + 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); + + /* TODO: sq cmp */ + f32 pd_len = v2_len(pd); + if (pd_len < pen_len) { + pen_pe_index = pe_index; + pen = pd; + pen_len = pd_len; + } + } + + /* Find new point in dir */ + m = menkowski_point(poly0, poly1, pen); + + /* Check unique */ + /* TODO: Better */ + /* TODO: Epsilon or iteration limit */ + { + b32 unique = true; + for (u32 i = 0; i < proto_count; ++i) { + if (v2_eq(m.p, proto[i])) { + unique = false; + break; + } + } + if (!unique) { + break; + } + } + + /* Insert point into prototype array */ + /* 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.p; + } + pen = v2_mul(v2_norm(pen), pen_len); + } + + abort: + struct gjk_extended_result res = { 0 }; + res.colliding = colliding; + res.colliding_pen = pen; + res.final_simplex = s; + scratch_end(scratch); + return res; +} + +#else + +struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array poly1, u32 max_steps) +{ + struct temp_arena scratch = scratch_begin_no_conflict(); + b32 colliding = false; + struct v2 pen = V2(0, 0); + u32 step = 0; + + /* 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 = menkowski_point(poly0, poly1, dir); + s.a = m; + s.len = 1; + +#if 0 + /* Second point is support point towards origin */ + { + if (step++ >= max_steps) goto abort; + dir = v2_neg(s.a.p); + m = menkowski_point(poly0, poly1, dir); + if (!v2_eq(s.a.p, m.p)) { + s.b = s.a; + s.a = m; + s.len = 2; + } + } +#endif + + while (true) { + /* Second point is support point towards origin */ + if (s.len == 1) { + if (step++ >= max_steps) goto abort; + dir = v2_neg(s.a.p); + m = menkowski_point(poly0, poly1, dir); + if (v2_eq(s.a.p, m.p)) { + colliding = false; break; } + + s.b = s.a; + s.a = m; + s.len = 2; } - /* Insert point into prototype array */ - /* 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] = p; -#else - /* FIXME: Maintain convexity */ - struct v2 p = menkowski_point(poly0, poly1, pen); - if (v2_eq(p, ps) || v2_eq(p, pe)) { - break; - } else { + /* Third point is support point in direction of line normal towards origin */ + if (step++ >= max_steps) goto abort; + dir = perp_towards_point(s.a.p, s.b.p, V2(0, 0)); + m = menkowski_point(poly0, poly1, dir); + if (v2_eq(m.p, s.a.p) || v2_eq(m.p, s.b.p)) { + colliding = false; break; } -#endif - } - pen = v2_mul(v2_norm(pen), pen_len); - scratch_end(scratch); - return pen; -} + s.c = s.b; + s.b = s.a; + s.a = m; + s.len = 3; + + if (step++ >= max_steps) goto abort; -/* TODO: Remove this (debugging) */ -struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1) -{ - DEBUGBREAKABLE; #if 0 - struct v2_array res = { .points = arena_dry_push(arena, struct v2) }; - for (u64 i = 0; i < poly0.count; ++i) { - struct v2 a = poly0.points[i]; - for (u64 j = 0; j < poly1.count; ++j) { - struct v2 b = poly1.points[j]; - *arena_push(arena, struct v2) = v2_sub(a, b); - ++res.count; + dir = v2_neg(perp_towards_point(s.a, s.b, s.c)); /* Normal dir of ab pointing away from c */ + if (v2_dot(dir, v2_neg(s.a)) >= 0 && v2_dot(v2_neg(s.a), v2_sub(s.b, s.a)) >= 0 && v2_dot(v2_neg(s.b), v2_sub(s.a, s.b))) { + /* Point is in region ab, remove c from simplex (will happen automatically next iteration) */ + s.len = 2; + } else { + /* Point is not in region ab */ + dir = v2_neg(perp_towards_point(s.a, s.c, s.b)); /* Normal dir of ac pointing away from b */ + if (v2_dot(dir, v2_neg(s.a)) >= 0 && v2_dot(v2_neg(s.a), v2_sub(s.c, s.a)) >= 0 && v2_dot(v2_neg(s.c), v2_sub(s.a, s.c))) { + /* Point is in region ac, remove b from simplex */ + s.b = s.c; + s.len = 2; + } else { + dir = v2_neg(perp_towards_point(s.b, s.c, s.a)); /* Normal dir of bc pointing away from a */ + if (v2_dot(dir, v2_neg(s.b)) >= 0 && v2_dot(v2_neg(s.b), v2_sub(s.c, s.b)) >= 0 && v2_dot(v2_neg(s.c), v2_sub(s.b, s.c))) { + /* Point is in region bc, remove a from simplex */ + s.a = s.b; + s.b = s.c; + s.len = 2; + } else { + /* Point is in simplex */ + colliding = true; + break; + } + } } - } - return res; #else - struct v2_array res = { .points = arena_dry_push(arena, struct v2) }; - u64 rays = 500; - for (u64 i = 0; i < rays; ++i) { - f32 angle = ((f32)i / rays) * (2 * PI); - struct v2 dir = v2_from_angle(angle); - struct v2 p = menkowski_point(poly0, poly1, dir); - if (res.count == 0 || !v2_eq(p, res.points[res.count - 1])) { - *arena_push(arena, struct v2) = p; - ++res.count; + b32 rab = v2_dot(perp_towards_point(s.a.p, s.b.p, s.c.p), v2_neg(s.a.p)) < 0; + b32 rac = v2_dot(perp_towards_point(s.a.p, s.c.p, s.b.p), v2_neg(s.a.p)) < 0; + b32 rbc = v2_dot(perp_towards_point(s.b.p, s.c.p, s.a.p), v2_neg(s.b.p)) < 0; + if (!rab && !rac && !rbc) { + colliding = true; + break; + } + + /* Simplex 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 */ + s.len = 2; + } else if (rac) { + /* Remove b */ + s.b = s.c; + s.len = 2; + } else if (rbc) { + /* Remove a */ + 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; } - } - return res; #endif + } + +#if 0 + b32 epa_specific = false; + struct v2 epa_specific_dir = V2(0, 0); + + if (!colliding) { + epa_specific = true; + epa_specific_dir = + } +#endif + + if (colliding) { + ASSERT(s.len == 3); + struct v2 *proto = arena_dry_push(scratch.arena, struct v2); + { + struct v2 *tmp = arena_push_array(scratch.arena, struct v2, 3); + tmp[0] = s.a.p; + tmp[1] = s.b.p; + tmp[2] = s.c.p; + } + u32 proto_count = 3; + + pen = V2(0, 0); + f32 pen_len = F32_INFINITY; + while (true) { + pen = V2(0, 0); + pen_len = F32_INFINITY; + + /* Find dir from origin to closest edge */ + u32 pen_pe_index = 1; + 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]; + struct v2 pe = proto[pe_index]; + + 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); + + /* TODO: sq cmp */ + f32 pd_len = v2_len(pd); + if (pd_len < pen_len) { + pen_pe_index = pe_index; + pen = pd; + pen_len = pd_len; + } + } + + /* Find new point in dir */ + m = menkowski_point(poly0, poly1, pen); + + /* Check unique */ + /* TODO: Better */ + /* TODO: Epsilon or iteration limit */ + { + b32 unique = true; + for (u32 i = 0; i < proto_count; ++i) { + if (v2_eq(m.p, proto[i])) { + unique = false; + break; + } + } + if (!unique) { + break; + } + } + + /* Insert point into prototype array */ + /* 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.p; + } + pen = v2_mul(v2_norm(pen), pen_len); + } + + abort: + struct gjk_extended_result res = { 0 }; + res.colliding = colliding; + res.colliding_pen = pen; + res.final_simplex = s; + scratch_end(scratch); + return res; } -/* Returns 1 if winding forward, -1 if backward */ -i32 poly_get_winding_order(struct v2_array poly) -{ - i32 res; - if (poly.count >= 3) { - struct v2 a = poly.points[0]; - struct v2 b = poly.points[1]; - struct v2 c = poly.points[2]; - if (v2_wedge(v2_sub(b, a), v2_sub(c, b)) > 0) { - res = 1; - } else { - res = -1; - } - } else { - res = -1; - } - return res; -} +#endif diff --git a/src/util.h b/src/util.h index 2dba5ce2..6158c2e7 100644 --- a/src/util.h +++ b/src/util.h @@ -186,22 +186,28 @@ INLINE void sleep_frame(sys_timestamp_t last_frame_time, f64 target_dt) /* TODO: Remove this */ -struct simplex { - u32 len; - struct v2 a, b, c; +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_result { +struct gjk_extended_simplex { + u32 len; + struct gjk_menkowski_point a, b, c; +}; + +struct gjk_extended_result { b32 colliding; - struct simplex final_simplex; + struct v2 colliding_pen; + struct gjk_extended_simplex final_simplex; }; struct v2 poly_support(struct v2_array a, struct v2 dir); -struct v2 menkowski_point(struct v2_array poly0, struct v2_array poly1, struct v2 dir); struct v2 perp_towards_point(struct v2 start, struct v2 end, struct v2 p); -struct gjk_result gjk(struct v2_array poly0, struct v2_array poly1, u32 max_steps); -struct v2 epa(struct v2_array poly0, struct v2_array poly1, struct simplex simplex); struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1); i32 poly_get_winding_order(struct v2_array poly); +b32 gjk_boolean(struct v2_array poly0, struct v2_array poly1, u32 max_steps); +struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array poly1, u32 max_steps); #endif