From 8050c3949608d2235b2daa62847a6d71c54038af Mon Sep 17 00:00:00 2001 From: jacob Date: Mon, 9 Sep 2024 12:08:28 -0500 Subject: [PATCH] minor cleanup --- src/game.c | 11 +-- src/gjk.c | 238 ++++++++++------------------------------------------- src/gjk.h | 6 +- src/user.c | 8 +- 4 files changed, 52 insertions(+), 211 deletions(-) diff --git a/src/game.c b/src/game.c index b2169d55..d6b1d121 100644 --- a/src/game.c +++ b/src/game.c @@ -734,6 +734,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) velocity = v2_sub(xf1.og, xf0.og); //velocity = V2(0.014992147684, -0.000010356307); + //velocity = V2(-0.0001, -0.01); //velocity = V2(0.005, 0); //velocity = V2(-500, -500); @@ -746,7 +747,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, xf0, xf1); + struct gjk_extended_result res = gjk_extended(e0_poly, e1_poly, velocity); colliding = res.colliding; point0 = res.p0; @@ -758,16 +759,8 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) solved = res.solved; if (colliding) { -#if 1 any_collision = true; pen = v2_sub(point1, point0); - /* Pen movement test */ #if 0 - f32 epsilon = 0.000100; pen = v2_add(pen, v2_mul(v2_norm(pen), epsilon)); -#endif - //xf0.og = v2_add(xf0.og, pen); - //e0->verlet_xform.og = v2_add(e0->verlet_xform.og, pen); - //e0->verlet_xform.og = v2_add(e0->verlet_xform.og, pen); -#endif } } diff --git a/src/gjk.c b/src/gjk.c index a22fc5f5..31b38751 100644 --- a/src/gjk.c +++ b/src/gjk.c @@ -269,13 +269,8 @@ struct poly_support_test_result { struct v2 original; }; -#if 1 - -INTERNAL struct poly_support_test_result poly_support_test(struct v2_array a, struct v2 dir, struct xform xf0, struct xform xf1) +INTERNAL struct poly_support_test_result poly_support_test(struct v2_array a, struct v2 dir, struct v2 linear_velocity) { - (UNUSED)xf0; - (UNUSED)xf1; - /* TODO: Could probably binary search for largest dot since shape is convex */ struct v2 furthest = V2(0, 0); struct v2 furthest_original = V2(0, 0); @@ -291,19 +286,17 @@ INTERNAL struct poly_support_test_result poly_support_test(struct v2_array a, st } } - struct v2 velocity = v2_sub(xf1.og, xf0.og); - //struct v2 velocity = V2(1, 1); for (u32 i = 0; i < a.count; ++i) { struct v2 p = a.points[i]; struct v2 modified; #if 0 - if (v2_dot(velocity, dir) > 0) { - modified = v2_add(p, velocity); + if (v2_dot(linear_velocity, dir) > 0) { + modified = v2_add(p, linear_velocity); } else { modified = p; } #else - modified = v2_add(p, velocity); + modified = v2_add(p, linear_velocity); #endif f32 dot = v2_dot(dir, modified); if (dot > furthest_dot) { @@ -318,86 +311,27 @@ INTERNAL struct poly_support_test_result poly_support_test(struct v2_array a, st res.original = furthest_original; return res; } -#else -INTERNAL struct poly_support_test_result poly_support_test(struct v2_array a, struct v2 dir, struct xform xf0, struct xform xf1) -{ - f32 epsilon = 0.001; - (UNUSED)xf0; - (UNUSED)xf1; - - /* TODO: Could probably binary search for largest dot since shape is convex */ - struct v2 furthest = V2(0, 0); - struct v2 furthest_original = V2(0, 0); - f32 furthest_wedge = -F32_INFINITY; - f32 furthest_dot = -F32_INFINITY; - - for (u32 i = 0; i < a.count; ++i) { - struct v2 p = a.points[i]; - f32 dot = v2_dot(dir, p); - f32 wedge = v2_wedge(dir, p); - if (dot > furthest_dot + epsilon || (dot > furthest_dot - epsilon && wedge > furthest_wedge)) { - furthest = p; - furthest_original = p; - furthest_dot = dot; - furthest_wedge = wedge; - } - } - - struct v2 velocity = v2_sub(xf1.og, xf0.og); - for (u32 i = 0; i < a.count; ++i) { - struct v2 p = a.points[i]; - struct v2 modified; -#if 0 - if (v2_dot(velocity, dir) > 0) { - modified = v2_add(p, velocity); - } else { - modified = p; - } -#else - modified = v2_add(p, velocity); -#endif - f32 dot = v2_dot(dir, modified); - f32 wedge = v2_wedge(dir, modified); - if (dot > furthest_dot + epsilon || (dot > furthest_dot - epsilon && wedge > furthest_wedge)) { - furthest = modified; - furthest_original = p; - furthest_dot = dot; - furthest_wedge = wedge; - } - } - - struct poly_support_test_result res = { 0 }; - res.p = furthest; - res.original = furthest_original; - return res; -} - -#endif - -INTERNAL struct gjk_menkowski_point menkowski_point_extended_test(struct v2_array poly0, struct v2_array poly1, struct v2 dir, struct xform xf0, struct xform xf1) +INTERNAL struct gjk_menkowski_point menkowski_point_extended_test(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, xf0, xf1); - struct poly_support_test_result res1 = poly_support_test(poly1, v2_neg(dir), XFORM_IDENT, XFORM_IDENT); + 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)); res.p0 = res0.original; res.p1 = res1.original; res.p = v2_sub(res0.p, res1.p); return res; } - -struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1, struct xform xf0, struct xform xf1) +struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1, struct v2 linear_velocity) { - (UNUSED)xf0; - (UNUSED)xf1; - struct v2 velocity = v2_sub(xf1.og, xf0.og); struct temp_arena scratch = scratch_begin_no_conflict(); - struct gjk_extended_result res = { 0 }; + /* FIXME: Divs by 0 */ + /* TODO: Verify epsilon */ - f32 epsilon = 0.00000001; + f32 epsilon = 0.00001; struct gjk_simplex s = { .len = 3, .a = V2(F32_NAN, F32_NAN), @@ -408,8 +342,7 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array struct v2 shape0_p = { 0 }; struct v2 shape1_p = { 0 }; b32 velocity_intersects = false; - b32 use_penetration_dir = false; - (UNUSED)use_penetration_dir; + f32 velocity_intersection = 0; /* TODO: Move this back down */ struct gjk_menkowski_point *proto = arena_dry_push(scratch.arena, struct gjk_menkowski_point); @@ -429,7 +362,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]), xf0, xf1); + s.a = menkowski_point_extended_test(shape0, shape1, v2_sub(shape1.points[0], shape0.points[0]), linear_velocity); s.len = 1; while (!colliding) { @@ -438,7 +371,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, xf0, xf1); + m = menkowski_point_extended_test(shape0, shape1, dir, linear_velocity); if (v2_eq(m.p, s.a.p)) { /* Point is the same */ break; @@ -446,20 +379,17 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array s.b = s.a; s.a = m; s.len = 2; -#if 1 if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) < epsilon) { /* New ab lies on origin */ break; } -#endif - /* 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 = menkowski_point_extended_test(shape0, shape1, dir, xf0, xf1); + m = menkowski_point_extended_test(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; @@ -475,7 +405,6 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array s.b = s.a; s.a = m; s.len = 3; -#if 1 if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) < epsilon) { /* New ab lies on origin */ s.len = 2; @@ -487,7 +416,6 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array s.len = 2; break; } -#endif } DBGSTEP; @@ -541,7 +469,6 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array /* ========================== * * Epa * ========================== */ - use_penetration_dir = !v2_eq(velocity, V2(0, 0)); proto = arena_dry_push(scratch.arena, struct gjk_menkowski_point); proto_count = 0; { @@ -553,7 +480,7 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array proto_count = 3; } - if (!use_penetration_dir) { + if (v2_eq(linear_velocity, V2(0, 0))) { /* ========================== * * Expand simplex towards closest edge to origin inside menkowski * ========================== */ @@ -595,7 +522,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, xf0, xf1); + m = menkowski_point_extended_test(shape0, shape1, dir, linear_velocity); /* Check unique */ /* TODO: Better */ @@ -634,42 +561,11 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array DBGSTEP; /* FIXME: Winding order of ps & pe index */ -#if 0 - f32 furthest = -F32_INFINITY; - 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 w = v2_wedge(vse, velocity); - if (w != 0) { - w = 1 / w; - f32 t1 = v2_wedge(vso, velocity) * w; - f32 t2 = v2_wedge(vso, vse) * w; - (UNUSED)t1; - (UNUSED)t2; - b32 intersects = 0 <= t1 && 0 <= t2 && t2 <= 1; - if (t2 > furthest) { - pen_ps_index = ps_index; - pen_pe_index = pe_index; - furthest = t2; - dir = v2_perp_towards_dir(vse, velocity); - //dir = velocity; - } - } - } -#else f32 highest_score = -F32_INFINITY; - u32 pen_ps_index = 0; - u32 pen_pe_index = 0; + u32 picked_start_index = 0; + u32 picked_end_index = 0; + b32 picked_intersects = false; + f32 picked_intersection = 0; for (u32 i = 0; i < proto_count; ++i) { u32 ps_index = i; u32 pe_index = (i < proto_count - 1) ? (i + 1) : 0; @@ -679,13 +575,13 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array struct v2 vse = v2_sub(pe, ps); struct v2 vso = v2_neg(ps); - f32 w = v2_wedge(vse, velocity); + f32 w = v2_wedge(vse, linear_velocity); f32 t1 = 0; f32 t2 = 0; b32 intersects = false; if (w != 0) { w = 1 / w; - t1 = v2_wedge(vso, velocity) * w; + t1 = v2_wedge(vso, linear_velocity) * w; t2 = v2_wedge(vso, vse) * w; intersects = -epsilon < t1 && t1 < 1 + epsilon && -epsilon < t2 && t2 < 1 + epsilon; } @@ -695,10 +591,10 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array if (intersects) { /* Score is intersection (+1 to ensure intersections get priority) */ score = 1 + t1; - next_dir = v2_perp_towards_dir(vse, velocity); + next_dir = v2_perp_towards_dir(vse, linear_velocity); } else { /* Score is inverse projection len */ - struct v2 vsv = v2_sub(velocity, ps); + struct v2 vsv = v2_sub(linear_velocity, ps); struct v2 vsd = v2_mul(vse, v2_dot(vse, vsv) / v2_dot(vse, vse)); f32 len = v2_len(v2_sub(vsd, vsv)); score = 1.0 - len; @@ -706,21 +602,24 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array } if (score > highest_score) { - pen_ps_index = ps_index; - pen_pe_index = pe_index; + picked_start_index = ps_index; + picked_end_index = pe_index; highest_score = score; + picked_intersects = intersects; + picked_intersection = t1; dir = next_dir; } } -#endif /* TODO: Move to break (debugging) */ - s.a = proto[pen_ps_index]; - s.b = proto[pen_pe_index]; + velocity_intersects = picked_intersects; + velocity_intersection = picked_intersection; + s.a = proto[picked_start_index]; + s.b = proto[picked_end_index]; s.len = 2; /* Find new point in dir */ - m = menkowski_point_extended_test(shape0, shape1, dir, xf0, xf1); + m = menkowski_point_extended_test(shape0, shape1, dir, linear_velocity); /* Check unique */ /* TODO: Better */ @@ -778,39 +677,7 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array } } - - - - - - - - -#if 0 /* Resolve points */ - if (s.len == 1) { - shape0_p = s.a.p0; - shape1_p = s.a.p1; - } else if (s.len == 2) { - /* FIXME: Winding order dependent? */ - ASSERT(s.len == 2); - 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 */ - 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); - /* 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; @@ -818,29 +685,16 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array ASSERT(s.len == 2); /* FIXME: Winding order dependent? */ f32 ratio; - if (use_penetration_dir) { - struct v2 vao = v2_neg(s.a.p); - struct v2 vab = v2_sub(s.b.p, s.a.p); - - f32 w = 1 / v2_wedge(vab, velocity); - f32 t1 = v2_wedge(vao, velocity) * w; - f32 t2 = v2_wedge(vao, vab) * w; - - velocity_intersects = 0 <= t1 && t1 <= 1 && 0 <= t2 && t2 <= 1; - -#if 1 + if (colliding && !v2_eq(linear_velocity, V2(0, 0))) { if (velocity_intersects) { /* Ratio between edge a & b that velocity intersection lies */ - ratio = t1; + ratio = velocity_intersection; } else { /* Ratio between edge a & b that projected velocity lies */ - struct v2 vap = v2_sub(velocity, s.a.p); + struct v2 vab = v2_sub(s.b.p, s.a.p); + struct v2 vap = v2_sub(linear_velocity, s.a.p); ratio = clamp_f32(v2_dot(vab, vap) / v2_dot(vab, vab), 0, 1); } -#else - /* Ratio between edge a & b that velocity intersection lies */ - ratio = t1; -#endif } else { /* Determine ratio between edge a & b that projected origin lies */ struct v2 vab = v2_sub(s.b.p, s.a.p); @@ -857,8 +711,6 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1_p = v2_add(shape1_p, s.a.p1); } -#endif - res.solved = true; abort: @@ -890,14 +742,14 @@ abort: /* TODO: Remove this (debugging) */ -struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1, struct xform xf0 ,struct xform xf1) +struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1, struct v2 linear_velocity) { 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_extended_test(poly0, poly1, dir, xf0, xf1).p; + struct v2 p = menkowski_point_extended_test(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; @@ -906,14 +758,10 @@ struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_ return res; } -struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_array poly1, struct xform xf0, struct xform xf1) +/* TODO: Remove this (debugging) */ +struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_array poly1, struct v2 linear_velocity) { - (UNUSED)xf0; - (UNUSED)xf1; - struct v2_array res = { .points = arena_dry_push(arena, struct v2) }; - - struct v2 velocity = v2_sub(xf1.og, xf0.og); for (u64 i = 0; i < poly0.count; ++i) { struct v2 p0 = poly0.points[i]; for (u64 j = 0; j < poly1.count; ++j) { @@ -924,7 +772,7 @@ struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_arra ++res.count; } { - *arena_push(arena, struct v2) = v2_add(diff, velocity);; + *arena_push(arena, struct v2) = v2_add(diff, linear_velocity); ++res.count; } } diff --git a/src/gjk.h b/src/gjk.h index 6d5ec859..9817a82c 100644 --- a/src/gjk.h +++ b/src/gjk.h @@ -62,11 +62,11 @@ struct gjk_extended_result { struct gjk_prototype prototype; }; -struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1, struct xform xf0, struct xform xf1); +struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1, struct v2 linear_velocity); -struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1, struct xform xf0, struct xform xf1); +struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1, struct v2 linear_velocity); -struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_array poly1, struct xform xf0, struct xform xf1); +struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_array poly1, struct v2 linear_velocity); #endif diff --git a/src/user.c b/src/user.c index 7e428464..9f63b32a 100644 --- a/src/user.c +++ b/src/user.c @@ -1057,7 +1057,7 @@ INTERNAL void user_update(void) //struct v2_array m = menkowski(temp.arena, ent_poly, e1_poly); - struct v2_array m = menkowski(temp.arena, ent_poly_xf0, e1_poly, ent->xf0, ent->xf1); + 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); @@ -1071,7 +1071,7 @@ INTERNAL void user_update(void) //struct v2_array m = menkowski(temp.arena, ent_poly, e1_poly); - struct v2_array m = cloud(temp.arena, ent_poly_xf0, e1_poly, ent->xf0, ent->xf1); + struct v2_array m = cloud(temp.arena, ent_poly_xf0, e1_poly, v2_sub(ent->xf1.og, ent->xf0.og)); for (u64 i = 0; i < m.count; ++i) { struct v2 p = xform_mul_v2(G.world_view, m.points[i]);; @@ -1203,8 +1203,8 @@ INTERNAL void user_update(void) /* Queue player move cmd */ f32 move_speed = 1.0f; if (G.bind_states[USER_BIND_KIND_WALK].is_held) { - //const f32 walk_ratio = 0.25f; - const f32 walk_ratio = 0.01f; + const f32 walk_ratio = 0.25f; + //const f32 walk_ratio = 0.01f; move_speed *= walk_ratio; }