From 818ffa7eba7aaaa4ecd4b8908f0fa5c1d58aef15 Mon Sep 17 00:00:00 2001 From: jacob Date: Wed, 28 Aug 2024 20:21:55 -0500 Subject: [PATCH] start on scuffed epa --- src/entity.h | 11 +--- src/game.c | 169 ++++++++++++++++++++++++++++++++++++++++++++++----- src/math.h | 15 ++++- src/user.c | 29 ++++++++- 4 files changed, 199 insertions(+), 25 deletions(-) diff --git a/src/entity.h b/src/entity.h index 575ca314..c2ae9e31 100644 --- a/src/entity.h +++ b/src/entity.h @@ -55,8 +55,7 @@ struct entity_store { - -/* TODO: Remove this (testing) */ +/* TODO: Remove this */ struct simplex { struct v2 a, b, c; }; @@ -67,12 +66,6 @@ struct simplex { - - - - - - struct entity { /* ====================================================================== */ /* Metadata */ @@ -105,6 +98,8 @@ struct entity { /* TODO: Remove this (testing) */ b32 colliding; + struct simplex simplex; + struct v2 pen; diff --git a/src/game.c b/src/game.c index 2525538a..3e69b64c 100644 --- a/src/game.c +++ b/src/game.c @@ -124,7 +124,7 @@ INTERNAL void spawn_test_entities(void) struct v2 pos = V2(-1, -1); struct v2 size = V2(1, 1); f32 r = 0; - f32 skew = PI / 4; + f32 skew = 0; struct entity *e = entity_alloc(root); @@ -174,7 +174,8 @@ INTERNAL void spawn_test_entities(void) /* Box */ { - struct v2 pos = V2(0.5, -0.5); + //struct v2 pos = V2(0.5, -0.5); + struct v2 pos = V2(0.5, -1); struct v2 size = V2(1, 1); f32 rot = 0; struct entity *e = entity_alloc(root); @@ -207,12 +208,12 @@ INTERNAL void spawn_test_entities(void) * Collision test * ========================== */ -INTERNAL struct v2 quad_furthest_point(struct quad quad, struct v2 dir) +INTERNAL struct v2 poly_furthest_point(struct v2_array a, struct v2 dir) { - struct v2 furthest = quad.e[0]; + struct v2 furthest = a.points[0]; f32 furthest_dot = v2_dot(dir, furthest); - for (u32 i = 1; i < ARRAY_COUNT(quad.e); ++i) { - struct v2 p = quad.e[i]; + for (u32 i = 1; i < a.count; ++i) { + struct v2 p = a.points[i]; f32 dot = v2_dot(dir, p); if (dot > furthest_dot) { furthest = p; @@ -222,9 +223,9 @@ INTERNAL struct v2 quad_furthest_point(struct quad quad, struct v2 dir) return furthest; } -INTERNAL struct v2 quad_support_point(struct quad q0, struct quad q1, struct v2 dir) +INTERNAL struct v2 poly_support_point(struct v2_array poly0, struct v2_array poly1, struct v2 dir) { - return v2_sub(quad_furthest_point(q0, dir), quad_furthest_point(q1, v2_neg(dir))); + return v2_sub(poly_furthest_point(poly0, dir), poly_furthest_point(poly1, v2_neg(dir))); } INTERNAL struct v2 normal_towards_point(struct v2 start, struct v2 end, struct v2 p) @@ -232,7 +233,6 @@ INTERNAL struct v2 normal_towards_point(struct v2 start, struct v2 end, struct v struct v2 dir = v2_norm(v2_perp_cw(v2_sub(end, start))); i32 sign = 1 - ((v2_dot(dir, v2_sub(p, start)) < 0) << 1); return v2_mul(dir, sign); - } struct gjk_result { @@ -240,18 +240,18 @@ struct gjk_result { struct simplex final_simplex; }; -INTERNAL struct gjk_result gjk(struct quad q0, struct quad q1) +INTERNAL struct gjk_result gjk(struct v2_array poly0, struct v2_array poly1) { - struct v2 q0_center = v2_div(v2_add(q0.p4, v2_add(q0.p3, v2_add(q0.p2, q0.p1))), 4); - struct v2 q1_center = v2_div(v2_add(q1.p4, v2_add(q1.p3, v2_add(q1.p2, q1.p1))), 4); + struct v2 poly0_center = math_poly_center(poly0); + struct v2 poly1_center = math_poly_center(poly1); /* Simplex */ struct simplex s = { 0 }; u32 s_len = 0; /* Append first point to simplex */ - struct v2 dir = v2_norm(v2_sub(q1_center, q0_center)); - s.c = quad_support_point(q0, q1, dir); + struct v2 dir = v2_norm(v2_sub(poly1_center, poly0_center)); + s.c = poly_support_point(poly0, poly1, dir); s_len = 1; dir = v2_norm(v2_neg(s.c)); /* Next point is towards origin */ @@ -259,7 +259,7 @@ INTERNAL struct gjk_result gjk(struct quad q0, struct quad q1) b32 colliding = false; while (true) { /* Determine support point */ - struct v2 p = quad_support_point(q0, q1, dir); + struct v2 p = poly_support_point(poly0, poly1, dir); if (v2_dot(dir, p) < 0) { /* Point did not cross origin */ colliding = false; @@ -315,6 +315,71 @@ INTERNAL struct gjk_result gjk(struct quad q0, struct quad q1) }; } +INTERNAL struct v2 epa(struct v2_array poly0, struct v2_array poly1, struct simplex simplex) +{ + struct temp_arena scratch = scratch_begin_no_conflict(); + + struct v2 *poly = 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 poly_count = 3; + + struct v2 pen = V2(0, 0); + f32 pen_len = F32_INFINITY; + while (true) { + /* Find dir from origin to closest edge */ + struct v2 ps = V2(0, 0); + struct v2 pe = V2(0, 0); + for (u32 i = 0; i < poly_count; ++i) { + ps = poly[i]; + pe = poly[(i < poly_count - 1) ? i : 0]; + + struct v2 vso = v2_neg(ps); + struct v2 vse = v2_norm(v2_sub(pe, ps)); + + f32 dot = v2_dot(vso, vse); + + struct v2 vsd = v2_mul(vse, dot); + struct v2 pd = v2_add(ps, vsd); + + /* TODO: sq cmp */ + f32 pd_len = v2_len(pd); + if (pd_len < pen_len) { + pen = pd; + pen_len = pd_len; + } + } + + /* Find new point in dir */ +#if 0 + /* FIXME: Maintain convexity */ + struct v2 p = poly_support_point(poly0, poly1, pen); + if (v2_eq(p, ps) || v2_eq(p, pe)) { + break; + } else { + *arena_push(scratch.arena, struct v2) = p; + ++poly_count; + } +#else + /* FIXME: Maintain convexity */ + struct v2 p = poly_support_point(poly0, poly1, pen); + if (v2_eq(p, ps) || v2_eq(p, pe)) { + break; + } else { + break; + } +#endif + } + pen = v2_mul(v2_norm(pen), pen_len); + + scratch_end(scratch); + return pen; +} + /* ========================== * * Update * ========================== */ @@ -776,6 +841,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) * Collision * ========================== */ +#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; @@ -815,6 +881,79 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) 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; + 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 simplex simplex = { 0 }; + struct v2 pen = V2(0, 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_result res = gjk(e0_poly, e1_poly); + colliding = res.colliding; + simplex = res.final_simplex; + + if (colliding) { + /* Pen movement test */ + pen = epa(e0_poly, e1_poly, simplex); + +#if 0 + { + struct xform xf = e1_xf; + + //xf.og = v2_add(xf.og, v2_div(pen, 2)); + xf.og = v2_add(xf.og, pen); + + entity_set_xform(e1, xf); + e1->verlet_xform = xf; + } +#endif + + break; + } + } + + e0->colliding = colliding; + e0->simplex = simplex; + e0->pen = pen; + } +#endif /* ========================== * * Player aim diff --git a/src/math.h b/src/math.h index beb8eb04..673ca2e2 100644 --- a/src/math.h +++ b/src/math.h @@ -395,7 +395,7 @@ INLINE f32 math_rsqrt(f32 x) * Trig * ========================== */ -/* Some functions based on Cephes implementation (https://www.netlib.org/cephes/): +/* Functions based on Cephes implementation (https://www.netlib.org/cephes/): * - math_sin_approx * - math_cos_approx * - math_reduce_positive_to_pio4 @@ -1133,4 +1133,17 @@ INLINE struct quad quad_floor(struct quad quad) }; } +/* ========================== * + * Convex polygon + * ========================== */ + +INLINE struct v2 math_poly_center(struct v2_array a) +{ + struct v2 sum = V2(0, 0); + for (u64 i = 0; i < a.count; ++i) { + sum = v2_add(sum, a.points[i]); + } + return v2_div(sum, a.count); +} + #endif diff --git a/src/user.c b/src/user.c index a35d486c..f9ab73b4 100644 --- a/src/user.c +++ b/src/user.c @@ -360,7 +360,7 @@ INTERNAL void debug_draw_xform(struct xform xf) struct v2 pos = xform_mul_v2(G.world_view, xf.og); struct v2 x_ray = xform_basis_mul_v2(G.world_view, xform_get_right(xf)); - struct v2 y_ray = xform_basis_mul_v2(G.world_view, xform_get_down(xf)); + struct v2 y_ray = xform_basis_mul_v2(G.world_view, xform_get_up(xf)); f32 ray_scale = 1; x_ray = v2_mul(x_ray, ray_scale); @@ -954,6 +954,33 @@ INTERNAL void user_update(void) } } + /* Draw player collision */ + if (entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED)) { + b32 colliding = ent->colliding; + + /* Draw simplex */ + { + f32 thickness = 2; + u32 color = colliding ? COLOR_WHITE: COLOR_YELLOW; + struct simplex simplex = ent->simplex; + struct v2 simplex_points[] = { simplex.a, simplex.b, simplex.c }; + for (u64 i = 0; i < ARRAY_COUNT(simplex_points); ++i) simplex_points[i] = xform_mul_v2(G.world_view, simplex_points[i]); + struct v2_array simplex_array = { .count = ARRAY_COUNT(simplex_points), .points = simplex_points }; + draw_solid_poly_line(G.viewport_canvas, simplex_array, true, thickness, color); + } + + /* Draw pen */ + { + f32 thickness = 2; + u32 color = COLOR_RED; + + struct v2 start = G.world_view.og; + struct v2 ray = xform_basis_mul_v2(G.world_view, ent->pen); + + draw_solid_arrow_ray(G.viewport_canvas, start, ray, thickness, thickness * 4, color); + } + } + /* Draw hierarchy */ if (entity_has_prop(parent, ENTITY_PROP_ACTIVE) && !parent->is_root) { u32 color = RGBA_32_F(0.6, 0.6, 1, 0.75);