From 81777548214811a62841688ed96a65c2eefabb4d Mon Sep 17 00:00:00 2001 From: jacob Date: Tue, 10 Sep 2024 13:56:26 -0500 Subject: [PATCH] revert to non-swept epa --- src/game.c | 34 ++++------ src/gjk.c | 183 +++++++++++++++++++++++++++++++++++++++++++++++------ src/gjk.h | 15 ++--- src/user.c | 9 +-- 4 files changed, 186 insertions(+), 55 deletions(-) diff --git a/src/game.c b/src/game.c index d6b1d121..3a233dea 100644 --- a/src/game.c +++ b/src/game.c @@ -598,25 +598,24 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) if (ent->ground_friction != 0) { struct xform verlet_xform = ent->verlet_xform; struct xform xf = entity_get_xform(ent); - struct v2 velocity = v2_div(v2_sub(xf.og, verlet_xform.og), dt); + struct v2 linear_velocity = v2_div(v2_sub(xf.og, verlet_xform.og), dt); - /* Ground friction */ - if (!v2_eq(velocity, V2(0, 0))) { + if (!v2_eq(linear_velocity, V2(0, 0))) { /* FIXME: Incorrect behavior at low FPS & low entity density */ const f32 clamp_epsilon = 0.01; - f32 velocity_len = v2_len(velocity); - if (velocity_len >= clamp_epsilon) { - f32 force_len = -velocity_len * ent->ground_friction; + f32 linear_velocity_len = v2_len(linear_velocity); + if (linear_velocity_len >= clamp_epsilon) { + f32 force_len = -linear_velocity_len * ent->ground_friction; struct entity *force = entity_alloc(ent); entity_enable_prop(force, ENTITY_PROP_FORCE); - force->force = v2_mul(v2_norm(velocity), force_len); + force->force = v2_mul(v2_norm(linear_velocity), force_len); activate_now(force); } else { - /* If velocity is below clamp_epsilon, stop entity movement. */ + /* If linear_velocity is below clamp_epsilon, stop entity movement. */ f32 mass = ent->mass_unscaled * math_fabs(xform_get_determinant(xf)); struct entity *impulse = entity_alloc(ent); entity_enable_prop(impulse, ENTITY_PROP_IMPULSE); - impulse->force = v2_mul(v2_neg(velocity), mass); + impulse->force = v2_mul(v2_neg(linear_velocity), mass); activate_now(impulse); } } @@ -688,7 +687,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) 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(xf0, e0_quad); + e0_quad = xform_mul_quad(xf1, e0_quad); e0_poly = (struct v2_array) { .count = ARRAY_COUNT(e0_quad.e), .points = e0_quad.e @@ -747,7 +746,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) xf1.og = v2_add(xf0.og, velocity); } - struct gjk_extended_result res = gjk_extended(e0_poly, e1_poly, velocity); + struct gjk_extended_result res = gjk_extended(e0_poly, e1_poly); colliding = res.colliding; point0 = res.p0; @@ -782,28 +781,23 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) } (UNUSED)any_collision; -#if 1 { - //if (!v2_eq(pen, V2(0, 0))) { +#if 0 if (any_collision) { - xf1.og = v2_add(xf0.og, pen); + xf1.og = v2_add(xf1.og, pen); //e0->verlet_xform.og = xf0.og; e0->verlet_xform.og = v2_sub(xf1.og, velocity); } +#endif entity_set_xform(e0, xf1); } -#else - { - entity_set_xform(e0, e0->predicted_xform); - } -#endif } /* ========================== * * Player aim * ========================== */ -#if 0 +#if 1 for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { struct entity *ent = &store->entities[entity_index]; if (!(ent->valid && entity_has_prop(ent, ENTITY_PROP_ACTIVE))) continue; diff --git a/src/gjk.c b/src/gjk.c index 31b38751..84ddfa33 100644 --- a/src/gjk.c +++ b/src/gjk.c @@ -77,8 +77,6 @@ b32 gjk_boolean(struct v2_array shape0, struct v2_array shape1) return false; } -#if 0 - INTERNAL struct gjk_menkowski_point menkowski_point_extended(struct v2_array poly0, struct v2_array poly1, struct v2 dir) { struct gjk_menkowski_point res; @@ -88,11 +86,9 @@ INTERNAL struct gjk_menkowski_point menkowski_point_extended(struct v2_array pol return res; } -struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1, struct xform shape0_xf0, struct xform shape0_xf1) +struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1) { - (UNUSED)shape0_xf0; - (UNUSED)shape0_xf1; - + struct temp_arena scratch = scratch_begin_no_conflict(); struct gjk_extended_result res = { 0 }; /* TODO: Verify epsilon */ @@ -101,6 +97,8 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array b32 colliding = false; struct v2 shape0_p = { 0 }; struct v2 shape1_p = { 0 }; + struct gjk_menkowski_point *proto = NULL; + u32 proto_count = 0; #if GJK_DEBUG u32 dbg_step = 0; @@ -211,6 +209,94 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array } } + if (colliding) { + /* ========================== * + * Epa + * ========================== */ + proto = arena_dry_push(scratch.arena, struct gjk_menkowski_point); + proto_count = 0; + { + ASSERT(s.len == 3); + struct gjk_menkowski_point *tmp = arena_push_array(scratch.arena, struct gjk_menkowski_point, 3); + tmp[0] = s.a; + tmp[1] = s.b; + tmp[2] = s.c; + proto_count = 3; + } + + /* ========================== * + * Expand simplex towards closest edge to origin inside menkowski + * ========================== */ + + while (true) { + 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); + + f32 d1 = v2_dot(vso, vse); + f32 d2 = v2_dot(vse, vse); + struct v2 vsd = v2_mul(vse, (d1 / d2)); + 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_len_sq = pd_len_sq; + dir = pd; + } + } + + /* TODO: Move to break (debugging) */ + s.a = proto[pen_ps_index]; + s.b = proto[pen_pe_index]; + s.len = 2; + + /* Find new point in dir */ + m = menkowski_point_extended(shape0, shape1, dir); + + /* Check unique */ + /* TODO: Better */ + DBGSTEP; + { + 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))) < epsilon) { + unique = false; + break; + } + } + if (!unique) { + 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; + } + } + /* Resolve points */ if (s.len == 1) { shape0_p = s.a.p0; @@ -235,11 +321,30 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1_p = v2_add(shape1_p, s.a.p1); } + res.solved = true; abort: + if (proto_count > 0) { + for (u32 i = 0; i < min_u32(proto_count, ARRAY_COUNT(res.prototype.points)); ++i) { + res.prototype.points[i] = proto[i].p; + } + res.prototype.len = proto_count; + } else { + if (s.len >= 1) { + res.prototype.points[0] = s.a.p; + if (s.len >= 2) { + res.prototype.points[1] = s.b.p; + if (s.len >= 3) { + res.prototype.points[2] = s.c.p; + } + } + } + res.prototype.len = s.len; + } res.colliding = colliding; res.p0 = shape0_p; res.p1 = shape1_p; res.simplex = s; + scratch_end(scratch); return res; } @@ -260,16 +365,53 @@ struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_ return res; } -#else +/* TODO: Remove this (debugging) */ +struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_array poly1) +{ + struct v2_array res = { .points = arena_dry_push(arena, struct v2) }; + for (u64 i = 0; i < poly0.count; ++i) { + struct v2 p0 = poly0.points[i]; + for (u64 j = 0; j < poly1.count; ++j) { + struct v2 p1 = poly1.points[j]; + *arena_push(arena, struct v2) = v2_sub(p0, p1); + ++res.count; + } + } + + return res; +} -struct poly_support_test_result { + + + + + + +/* ========================== * + * Swept GJK (unused) + * ========================== */ + +#if 0 + +struct gjk_swept_result { + b32 colliding; + struct v2 p0, p1; /* Closest points on each shape */ + + /* For debugging */ + struct gjk_simplex simplex; + b32 velocity_intersects; + b32 solved; + struct gjk_prototype prototype; +}; + +struct poly_support_swept_result { struct v2 p; struct v2 original; }; -INTERNAL struct poly_support_test_result poly_support_test(struct v2_array a, struct v2 dir, struct v2 linear_velocity) +INTERNAL struct poly_support_swept_result poly_support_swept(struct v2_array a, struct v2 dir, struct v2 linear_velocity) { /* TODO: Could probably binary search for largest dot since shape is convex */ struct v2 furthest = V2(0, 0); @@ -306,17 +448,17 @@ INTERNAL struct poly_support_test_result poly_support_test(struct v2_array a, st } } - struct poly_support_test_result res = { 0 }; + struct poly_support_swept_result res = { 0 }; res.p = furthest; res.original = furthest_original; return res; } -INTERNAL struct gjk_menkowski_point menkowski_point_extended_test(struct v2_array poly0, struct v2_array poly1, struct v2 dir, struct v2 linear_velocity) +INTERNAL struct gjk_menkowski_point menkowski_point_extended_swept(struct v2_array poly0, struct v2_array poly1, struct v2 dir, struct v2 linear_velocity) { struct gjk_menkowski_point res; - struct poly_support_test_result res0 = poly_support_test(poly0, dir, linear_velocity); - struct poly_support_test_result res1 = poly_support_test(poly1, v2_neg(dir), V2(0, 0)); + struct poly_support_swept_result res0 = poly_support_swept(poly0, dir, linear_velocity); + struct poly_support_swept_result res1 = poly_support_swept(poly1, v2_neg(dir), V2(0, 0)); res.p0 = res0.original; res.p1 = res1.original; res.p = v2_sub(res0.p, res1.p); @@ -362,7 +504,7 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array /* Determine encapsulating simplex if colliding, or closest edge / point to origin on simplex */ { /* First point is support point in shape's general directions to eachother */ - s.a = menkowski_point_extended_test(shape0, shape1, v2_sub(shape1.points[0], shape0.points[0]), linear_velocity); + s.a = menkowski_point_extended_swept(shape0, shape1, v2_sub(shape1.points[0], shape0.points[0]), linear_velocity); s.len = 1; while (!colliding) { @@ -371,7 +513,7 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array dir = v2_neg(s.a.p); DBGSTEP; - m = menkowski_point_extended_test(shape0, shape1, dir, linear_velocity); + m = menkowski_point_extended_swept(shape0, shape1, dir, linear_velocity); if (v2_eq(m.p, s.a.p)) { /* Point is the same */ break; @@ -389,7 +531,7 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array { DBGSTEP; - m = menkowski_point_extended_test(shape0, shape1, dir, linear_velocity); + m = menkowski_point_extended_swept(shape0, shape1, dir, linear_velocity); if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_sub(m.p, s.a.p))) < epsilon) { /* New point is on existing line ab */ break; @@ -432,7 +574,7 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array voronoi_mask |= (v2_dot(rbc_dir, v2_neg(s.b.p)) >= 0) << 2; /* Regions bc, b, and c */ /* Remove point or edge and determine next direction based on voronoi region */ switch (voronoi_mask) { - case 0: { /* No region, must be in simplex */ + default: { /* No region, must be in simplex */ colliding = true; } break; case 1: { /* Region ab, remove c */ @@ -522,7 +664,7 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array s.len = 2; /* Find new point in dir */ - m = menkowski_point_extended_test(shape0, shape1, dir, linear_velocity); + m = menkowski_point_extended_swept(shape0, shape1, dir, linear_velocity); /* Check unique */ /* TODO: Better */ @@ -619,7 +761,7 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array s.len = 2; /* Find new point in dir */ - m = menkowski_point_extended_test(shape0, shape1, dir, linear_velocity); + m = menkowski_point_extended_swept(shape0, shape1, dir, linear_velocity); /* Check unique */ /* TODO: Better */ @@ -679,6 +821,7 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array /* Resolve points */ if (s.len == 1) { + ASSERT(!colliding); /* Is this possible? Would make calculating collision normal tricky. */ shape0_p = s.a.p0; shape1_p = s.a.p1; } else { @@ -749,7 +892,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 = menkowski_point_extended_test(poly0, poly1, dir, linear_velocity).p; + struct v2 p = menkowski_point_extended_swept(poly0, poly1, dir, linear_velocity).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/gjk.h b/src/gjk.h index 9817a82c..17f1f7b2 100644 --- a/src/gjk.h +++ b/src/gjk.h @@ -21,15 +21,16 @@ struct gjk_simplex { /* Returns simple true or false indicating shape collision */ b32 gjk_boolean(struct v2_array shape0, struct v2_array shape1); -#if 0 - +struct gjk_prototype { struct v2 points[64]; u32 len; }; struct gjk_extended_result { b32 colliding; struct v2 p0, p1; /* Closest points (or penetrating points if colliding) on each shape */ /* For debugging */ + b32 solved; struct gjk_simplex simplex; b32 velocity_intersects; + struct gjk_prototype prototype; }; /* Returns shape whether shapes are colliding well as closest / penetrating points on each shape. @@ -39,17 +40,13 @@ struct gjk_extended_result { * points. Otherwise, the penetrating points will be calculated using the * supplied direction. */ -struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1, struct v2 penetration_dir); +struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1); struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1); -#else +struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_array poly1); -/* TODO: Remove this (debugging) */ -struct gjk_prototype { - struct v2 points[256]; - u32 len; -}; +#if 0 struct gjk_extended_result { b32 colliding; diff --git a/src/user.c b/src/user.c index 9f63b32a..f3cee023 100644 --- a/src/user.c +++ b/src/user.c @@ -1055,9 +1055,8 @@ INTERNAL void user_update(void) f32 thickness = 2; (UNUSED)thickness; - //struct v2_array m = menkowski(temp.arena, ent_poly, e1_poly); - - struct v2_array m = menkowski(temp.arena, ent_poly_xf0, e1_poly, v2_sub(ent->xf1.og, ent->xf0.og)); + struct v2_array m = menkowski(temp.arena, ent_poly_xf0, e1_poly); + //struct v2_array m = menkowski(temp.arena, ent_poly_xf0, e1_poly, v2_sub(ent->xf1.og, ent->xf0.og)); for (u64 i = 0; i < m.count; ++i) m.points[i] = xform_mul_v2(G.world_view, m.points[i]); draw_solid_poly_line(G.viewport_canvas, m, true, thickness, color); @@ -1069,9 +1068,7 @@ INTERNAL void user_update(void) u32 color = RGBA_32_F(1, 1, 1, 1); f32 radius = 2; - //struct v2_array m = menkowski(temp.arena, ent_poly, e1_poly); - - struct v2_array m = cloud(temp.arena, ent_poly_xf0, e1_poly, v2_sub(ent->xf1.og, ent->xf0.og)); + struct v2_array m = cloud(temp.arena, ent_poly_xf0, e1_poly); for (u64 i = 0; i < m.count; ++i) { struct v2 p = xform_mul_v2(G.world_view, m.points[i]);;