From f22aec28bbeb2cd93d8f477bceb08ee73af50189 Mon Sep 17 00:00:00 2001 From: jacob Date: Wed, 9 Oct 2024 13:05:01 -0500 Subject: [PATCH] more gjk collision work for rounded shapes --- src/collider.c | 1500 ++++++------------------------------------------ src/collider.h | 2 +- src/game.c | 82 ++- src/user.c | 14 +- 4 files changed, 269 insertions(+), 1329 deletions(-) diff --git a/src/collider.c b/src/collider.c index dc3cb8eb..44caab20 100644 --- a/src/collider.c +++ b/src/collider.c @@ -5,6 +5,7 @@ #if COLLIDER_DEBUG u32 collider_debug_steps = U32_MAX; +//u32 collider_debug_steps = 1000000; //u32 collider_debug_steps = 50; #endif @@ -85,8 +86,9 @@ struct collider_collision_points_result collider_collision_points(struct collide (UNUSED)radius1; /* TODO: Parameterize */ - const f32 tolerance = 0.001f; - const f32 min_unique_pt_dist_sq = 0.001f * 0.001f; + const f32 tolerance = 0.005f; /* How close can shapes be before collision is considered */ + const f32 min_unique_pt_dist_sq = 0.0001f * 0.0001f; + const u32 max_epa_iterations = 64; /* To prevent extremely large prototypes when origin is in exact center of rounded feature */ b32 colliding = false; b32 simplex_is_closest_edge = false; @@ -114,14 +116,15 @@ struct collider_collision_points_result collider_collision_points(struct collide * ========================== */ { /* First point is support point in shape's general directions to eachother */ - s.a = menkowski_point(shape0, shape1, xf0, xf1, v2_sub(xf1.og, xf0.og)); + dir = v2_sub(xf1.og, xf0.og); + if (v2_is_zero(dir)) dir = V2(1, 0); + s.a = menkowski_point(shape0, shape1, xf0, xf1, dir); s.len = 1; struct v2 removed_a = ZI; struct v2 removed_b = ZI; u32 num_removed = 0; - b32 done = false; - while (!done) { + while (true) { if (s.len == 1) { /* Second point is support point towards origin */ dir = v2_neg(s.a); @@ -131,7 +134,6 @@ struct collider_collision_points_result collider_collision_points(struct collide /* Check that new point is far enough away from existing point */ if (v2_len_sq(v2_sub(m, s.a)) < min_unique_pt_dist_sq) { simplex_is_closest_edge = true; - done = true; break; } s.b = s.a; @@ -148,105 +150,93 @@ struct collider_collision_points_result collider_collision_points(struct collide /* Check that new point is far enough away from existing points */ if (v2_len_sq(v2_sub(m, s.a)) < min_unique_pt_dist_sq || v2_len_sq(v2_sub(m, s.b)) < min_unique_pt_dist_sq || - ((num_removed >= 1) && ( + ( + (num_removed >= 1) && ( (v2_len_sq(v2_sub(m, removed_a)) < min_unique_pt_dist_sq) || - (num_removed >= 2 && v2_len_sq(v2_sub(m, removed_b)) < min_unique_pt_dist_sq) - )) || - math_fabs(v2_wedge(v2_sub(s.b, s.a), v2_sub(m, s.a))) < min_unique_pt_dist_sq - ) { + (num_removed >= 2 && v2_len_sq(v2_sub(m, removed_b)) < min_unique_pt_dist_sq)) + ) || + math_fabs(v2_wedge(v2_sub(s.b, s.a), v2_sub(m, s.a))) < min_unique_pt_dist_sq) + { simplex_is_closest_edge = true; - done = true; break; } s.c = s.b; s.b = s.a; s.a = m; s.len = 3; + + if (math_fabs(v2_wedge(v2_sub(s.b, s.a), v2_neg(s.a))) <= min_unique_pt_dist_sq || + math_fabs(v2_wedge(v2_sub(s.c, s.a), v2_neg(s.a))) <= min_unique_pt_dist_sq || + math_fabs(v2_wedge(v2_sub(s.c, s.b), v2_neg(s.b))) <= min_unique_pt_dist_sq) { + /* Simplex lies on origin */ + colliding = true; + break; + } } - /* Determine voronoi region of the simplex in which the origin lies */ + /* Determine region of the simplex in which the origin lies */ DBGSTEP; - i32 voronoi_mask = 0; struct v2 vab = v2_sub(s.b, s.a); struct v2 vac = v2_sub(s.c, s.a); struct v2 vbc = v2_sub(s.c, s.b); + struct v2 rab_dir = v2_perp_towards_dir(vab, v2_neg(vac)); struct v2 rac_dir = v2_perp_towards_dir(vac, v2_neg(vab)); struct v2 rbc_dir = v2_perp_towards_dir(vbc, vab); -#if 1 - voronoi_mask |= (v2_dot(rab_dir, v2_neg(s.a)) > 0) << 0; /* Regions ab, a, and b*/ - voronoi_mask |= (v2_dot(rac_dir, v2_neg(s.a)) > 0) << 1; /* Regions ac, a, and c */ - voronoi_mask |= (v2_dot(rbc_dir, v2_neg(s.b)) > 0) << 2; /* Regions bc, b, and c */ -#else - voronoi_mask = (v2_dot(rab_dir, v2_neg(s.a)) > 0) << 0; /* Regions ab, a, and b*/ - if (!voronoi_mask) { - voronoi_mask = (v2_dot(rac_dir, v2_neg(s.a)) > 0) << 1; /* Regions ac, a, and c */ - if (!voronoi_mask) { - voronoi_mask = (v2_dot(rbc_dir, v2_neg(s.b)) > 0) << 2; /* Regions bc, b, and c */ - } - } -#endif - /* Remove point or edge and determine next direction based on voronoi region */ - switch (voronoi_mask) { - case 0: - { /* No region, must be in simplex */ - colliding = true; - done = true; - } break; - case 1: - { /* Region ab, remove c */ - num_removed = 1; - removed_a = s.c; - s.len = 2; - dir = rab_dir; /* Next third point is in direction of region ab */ - } break; - case 2: - { /* Region ac, remove b */ - num_removed = 1; - removed_a = s.b; - s.len = 2; - s.b = s.c; - dir = rac_dir; /* Next third point is in direction of region ac */ - } break; - case 4: - { /* Region bc, remove a */ - num_removed = 1; - removed_a = s.a; - s.len = 2; - s.a = s.b; - s.b = s.c; - dir = rbc_dir; /* Next third point is in direction of region bc */ - } break; - case 3: - { /* Region a, remove bc */ - num_removed = 2; - removed_a = s.b; - removed_b = s.c; - s.len = 1; - } break; - case 5: - { /* Region b, remove ac */ - num_removed = 2; - removed_a = s.a; - removed_b = s.c; - s.len = 1; - s.a = s.b; - } break; - case 6: - { /* Region c, remove ab */ - num_removed = 2; - removed_a = s.a; - removed_b = s.b; - s.len = 1; - s.a = s.c; - } break; - default: - { - /* Unknown region (should be impossible) */ - ASSERT(false); - res.path = -1; - done = true; - } break; + + f32 rab_dot = v2_dot(rab_dir, v2_neg(s.a)); + f32 rac_dot = v2_dot(rac_dir, v2_neg(s.a)); + f32 rbc_dot = v2_dot(rbc_dir, v2_neg(s.b)); + + f32 vab_dot = v2_dot(vab, v2_neg(s.a)) / v2_len_sq(vab); + f32 vac_dot = v2_dot(vac, v2_neg(s.a)) / v2_len_sq(vac); + f32 vbc_dot = v2_dot(vbc, v2_neg(s.b)) / v2_len_sq(vbc); + + if (rab_dot >= 0 && vab_dot >= 0 && vab_dot <= 1) { + /* Region ab, remove c */ + num_removed = 1; + removed_a = s.c; + s.len = 2; + dir = rab_dir; /* Next third point is in direction of region ab */ + } else if (rac_dot >= 0 && vac_dot >= 0 && vac_dot <= 1) { + /* Region ac, remove b */ + num_removed = 1; + removed_a = s.b; + s.len = 2; + s.b = s.c; + dir = rac_dir; /* Next third point is in direction of region ac */ + } else if (rbc_dot >= 0 && vbc_dot >= 0 && vbc_dot <= 1) { + /* Region bc, remove a */ + num_removed = 1; + removed_a = s.a; + s.len = 2; + s.a = s.b; + s.b = s.c; + dir = rbc_dir; /* Next third point is in direction of region bc */ + } else if (vab_dot <= 0 && vac_dot <= 0) { + /* Region a, remove bc */ + num_removed = 2; + removed_a = s.b; + removed_b = s.c; + s.len = 1; + } else if (vab_dot >= 1 && vbc_dot <= 0) { + /* Region b, remove ac */ + num_removed = 2; + removed_a = s.a; + removed_b = s.c; + s.len = 1; + s.a = s.b; + } else if (vac_dot >= 1 && vbc_dot >= 1) { + /* Region c, remove ab */ + num_removed = 2; + removed_a = s.a; + removed_b = s.b; + s.len = 1; + s.a = s.c; + } else { + /* No region, must be in simplex */ + colliding = true; + break; } } } @@ -256,6 +246,8 @@ struct collider_collision_points_result collider_collision_points(struct collide * Epa (to find collision normal from inside shape) * ========================== */ + const f32 epa_epsilon_sq = 0.001f * 0.001f; + proto = arena_dry_push(scratch.arena, struct v2); proto_count = 0; { @@ -267,13 +259,16 @@ struct collider_collision_points_result collider_collision_points(struct collide proto_count = 3; } + u32 epa_iterations = 0; while (colliding) { + ++epa_iterations; 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; + struct v2 pen = ZI; for (u32 i = 0; i < proto_count; ++i) { u32 ps_index = i; u32 pe_index = (i < proto_count - 1) ? (i + 1) : 0; @@ -291,6 +286,7 @@ struct collider_collision_points_result collider_collision_points(struct collide pen_ps_index = ps_index; pen_pe_index = pe_index; pen_len_sq = pd_len_sq; + pen = pd; } } @@ -302,14 +298,18 @@ struct collider_collision_points_result collider_collision_points(struct collide /* Find new point in dir */ DBGSTEP; { - /* Next point is in direction of line normal pointing outwards from simplex */ /* TODO: If winding order is guaranteed then this can become v2_perp_left/right? */ struct v2 a = proto[pen_ps_index]; struct v2 b = proto[pen_pe_index]; - struct v2 n = proto[(pen_pe_index < proto_count - 1) ? (pen_pe_index + 1) : 0]; /* Next point along prototype after edge */ struct v2 vab = v2_sub(b, a); - struct v2 vna = v2_sub(a, n); - dir = v2_perp_towards_dir(vab, vna); + if (pen_len_sq < epa_epsilon_sq) { + /* Next point is in direction of line normal pointing outwards from simplex */ + struct v2 n = proto[(pen_pe_index < proto_count - 1) ? (pen_pe_index + 1) : 0]; /* Next point along prototype after edge */ + dir = v2_perp_towards_dir(vab, v2_sub(a, n)); + } else { + dir = v2_perp_towards_dir(vab, a); + } + } m = menkowski_point(shape0, shape1, xf0, xf1, dir); @@ -327,9 +327,13 @@ struct collider_collision_points_result collider_collision_points(struct collide break; } } - if (!unique) { + if (!unique || epa_iterations >= max_epa_iterations) { res.path = 1; - normal = v2_norm(dir); + if (pen_len_sq < epa_epsilon_sq) { + normal = v2_norm(dir); + } else { + normal = v2_norm(pen); + } break; } } @@ -381,22 +385,17 @@ struct collider_collision_points_result collider_collision_points(struct collide { const f32 wedge_epsilon = 0.001f; -#if 0 /* shape0 a -> b winding = clockwise */ u32 id_a0; u32 id_b0; - b32 a0_is_deepest; struct v2 a0; struct v2 b0; - struct v2 vab0; /* shape1 a -> b winding = counterclockwise */ u32 id_a1; u32 id_b1; - b32 a1_is_deepest; struct v2 a1; struct v2 b1; - struct v2 vab1; { u32 p_i = collider_support_point_index(shape0, xf0, normal); u32 a_i = (p_i > 0) ? (p_i - 1) : (count0 - 1); @@ -409,21 +408,29 @@ struct collider_collision_points_result collider_collision_points(struct collide struct v2 vap = v2_sub(p, a); struct v2 vpb = v2_sub(b, p); - /* FIXME: Make winding order independent */ + /* Swap a & b depending on winding order */ + if (v2_wedge(vap, vpb) < 0) { + u32 tmp_u32 = a_i; + a_i = b_i; + b_i = tmp_u32; + struct v2 tmp_v2 = a; + a = b; + b = tmp_v2; + tmp_v2 = vap; + vap = v2_neg(vpb); + vpb = v2_neg(tmp_v2); + } + if (v2_wedge(vap, normal) < (v2_wedge(vpb, normal) + wedge_epsilon)) { id_a0 = a_i; id_b0 = p_i; a0 = a; b0 = p; - vab0 = vap; - a0_is_deepest = false; } else { id_a0 = p_i; id_b0 = b_i; a0 = p; b0 = b; - vab0 = vpb; - a0_is_deepest = true; } } { @@ -440,139 +447,51 @@ struct collider_collision_points_result collider_collision_points(struct collide struct v2 vap = v2_sub(p, a); struct v2 vpb = v2_sub(b, p); - /* FIXME: Make winding order independent */ + /* Swap a & b depending on winding order */ + if (v2_wedge(vap, vpb) > 0) { + u32 tmp_u32 = a_i; + a_i = b_i; + b_i = tmp_u32; + struct v2 tmp_v2 = a; + a = b; + b = tmp_v2; + tmp_v2 = vap; + vap = v2_neg(vpb); + vpb = v2_neg(tmp_v2); + } + if (v2_wedge(vap, normal) < (v2_wedge(vpb, normal) + wedge_epsilon)) { id_a1 = a_i; id_b1 = p_i; a1 = a; b1 = p; - vab1 = vap; - a1_is_deepest = false; } else { id_a1 = p_i; id_b1 = b_i; a1 = p; b1 = b; - vab1 = vpb; - a1_is_deepest = true; } } +#if 1 if (radius0 > 0.0) { struct v2 scale = xform_get_scale(xf0); - - struct v2 normal_radius = v2_with_len(normal, radius0); - normal_radius = v2_mul_v2(normal_radius, scale); - - struct v2 perp_radius = v2_with_len(v2_neg(v2_perp(v2_sub(b0, a0))), radius0); - perp_radius = v2_mul_v2(perp_radius, scale); - - if (a0_is_deepest) { - a0 = v2_add(a0, normal_radius); - b0 = v2_add(b0, perp_radius); - } else { - a0 = v2_add(a0, perp_radius); - b0 = v2_add(b0, normal_radius); - } + struct v2 normal_radius = v2_mul_v2(v2_mul(normal, radius0), scale); + a0 = v2_add(a0, normal_radius); + b0 = v2_add(b0, normal_radius); } if (radius1 > 0.0) { struct v2 scale = xform_get_scale(xf1); - - struct v2 normal_radius = v2_with_len(normal, radius1); - normal_radius = v2_mul_v2(normal_radius, scale); - - struct v2 perp_radius = v2_with_len(v2_neg(v2_perp(v2_sub(b1, a1))), radius1); - perp_radius = v2_mul_v2(perp_radius, scale); - - if (a1_is_deepest) { - a1 = v2_sub(a1, normal_radius); - b1 = v2_sub(b1, perp_radius); - } else { - a1 = v2_sub(a1, perp_radius); - b1 = v2_sub(b1, normal_radius); - } + struct v2 normal_radius = v2_mul_v2(v2_mul(normal, radius1), scale); + a1 = v2_sub(a1, normal_radius); + b1 = v2_sub(b1, normal_radius); } #else - /* shape0 a -> b winding = clockwise */ - u32 id_a0; - u32 id_b0; - struct v2 a0; - struct v2 b0; - struct v2 vab0; - - /* shape1 a -> b winding = counterclockwise */ - u32 id_a1; - u32 id_b1; - struct v2 a1; - struct v2 b1; - struct v2 vab1; - { - u32 p_i = collider_support_point_index(shape0, xf0, normal); - u32 a_i = (p_i > 0) ? (p_i - 1) : (count0 - 1); - u32 b_i = ((p_i + 1) < count0) ? (p_i + 1) : 0; - - struct v2 p = xform_mul_v2(xf0, points0[p_i]); - struct v2 a = xform_mul_v2(xf0, points0[a_i]); - struct v2 b = xform_mul_v2(xf0, points0[b_i]); - - struct v2 vap = v2_sub(p, a); - struct v2 vpb = v2_sub(b, p); - - /* FIXME: Make winding order independent */ - if (v2_wedge(vap, normal) < (v2_wedge(vpb, normal) + wedge_epsilon)) { - id_a0 = a_i; - id_b0 = p_i; - a0 = a; - b0 = p; - vab0 = vap; - } else { - id_a0 = p_i; - id_b0 = b_i; - a0 = p; - b0 = b; - vab0 = vpb; - } - } - { - struct v2 neg_normal = v2_neg(normal); - - u32 p_i = collider_support_point_index(shape1, xf1, neg_normal); - u32 a_i = ((p_i + 1) < count1) ? (p_i + 1) : 0; - u32 b_i = (p_i > 0) ? (p_i - 1) : (count1 - 1); - - struct v2 p = xform_mul_v2(xf1, points1[p_i]); - struct v2 a = xform_mul_v2(xf1, points1[a_i]); - struct v2 b = xform_mul_v2(xf1, points1[b_i]); - - struct v2 vap = v2_sub(p, a); - struct v2 vpb = v2_sub(b, p); - - /* FIXME: Make winding order independent */ - if (v2_wedge(vap, normal) < (v2_wedge(vpb, normal) + wedge_epsilon)) { - id_a1 = a_i; - id_b1 = p_i; - a1 = a; - b1 = p; - vab1 = vap; - } else { - id_a1 = p_i; - id_b1 = b_i; - a1 = p; - b1 = b; - vab1 = vpb; - } - } - if (radius0 > 0.0) { struct v2 scale = xform_get_scale(xf0); - - struct v2 normal_radius = v2_with_len(normal, radius0); - normal_radius = v2_mul_v2(normal_radius, scale); - - struct v2 perp_radius = v2_with_len(v2_neg(v2_perp(v2_sub(b0, a0))), radius0); - perp_radius = v2_mul_v2(perp_radius, scale); - + struct v2 normal_radius = v2_mul_v2(v2_mul(normal, radius0), scale); + struct v2 perp_radius = v2_mul_v2(v2_with_len(v2_neg(v2_perp(v2_sub(b0, a0))), radius0), scale); if (v2_dot(a0, normal) >= v2_dot(b0, normal)) { a0 = v2_add(a0, normal_radius); b0 = v2_add(b0, perp_radius); @@ -584,13 +503,8 @@ struct collider_collision_points_result collider_collision_points(struct collide if (radius1 > 0.0) { struct v2 scale = xform_get_scale(xf1); - - struct v2 normal_radius = v2_with_len(normal, radius1); - normal_radius = v2_mul_v2(normal_radius, scale); - - struct v2 perp_radius = v2_with_len(v2_neg(v2_perp(v2_sub(b1, a1))), radius1); - perp_radius = v2_mul_v2(perp_radius, scale); - + struct v2 normal_radius = v2_mul_v2(v2_mul(normal, radius1), scale); + struct v2 perp_radius = v2_mul_v2(v2_with_len(v2_neg(v2_perp(v2_sub(b1, a1))), radius1), scale); if (v2_dot(a1, normal) <= v2_dot(b1, normal)) { a1 = v2_sub(a1, normal_radius); b1 = v2_sub(b1, perp_radius); @@ -601,105 +515,98 @@ struct collider_collision_points_result collider_collision_points(struct collide } #endif - - - - - - - - - - - f32 a0t = 0; f32 a1t = 0; f32 b0t = 0; f32 b1t = 0; - struct v2 vba0 = v2_neg(vab0); - struct v2 vba1 = v2_neg(vab1); + struct v2 vab0 = v2_sub(b0, a0); + struct v2 vab1 = v2_sub(b1, a1); + { - { - struct v2 va0a1 = v2_sub(a1, a0); - struct v2 va1a0 = v2_neg(va0a1); - { - f32 w = v2_wedge(vab0, normal); - if (w != 0) { - w = 1 / w; - a0t = v2_wedge(va0a1, normal) * w; - } - } - { - f32 w = v2_wedge(vab1, normal); - if (w != 0) { - w = 1 / w; - a1t = v2_wedge(va1a0, normal) * w; - } - } + struct v2 va0a1 = v2_sub(a1, a0); + struct v2 vb0b1 = v2_sub(b1, b0); + + f32 vab0_wedge_normal = v2_wedge(vab0, normal); + f32 vab1_wedge_normal = v2_wedge(vab1, normal); + f32 va0a1_wedge_normal = v2_wedge(va0a1, normal); + f32 vb0b1_wedge_normal = v2_wedge(vb0b1, normal); + + if (math_fabs(vab0_wedge_normal) > 0.01f) { + f32 w = 1 / vab0_wedge_normal; + a0t = clamp_f32(va0a1_wedge_normal * w, 0, 1); + b0t = clamp_f32(vb0b1_wedge_normal * -w, 0, 1); } - { - struct v2 vb0b1 = v2_sub(b1, b0); - struct v2 vb1b0 = v2_neg(vb0b1); - { - f32 w = v2_wedge(vba0, normal); - if (w != 0) { - w = 1 / w; - b0t = v2_wedge(vb0b1, normal) * w; - } - } - { - f32 w = v2_wedge(vba1, normal); - if (w != 0) { - w = 1 / w; - b1t = v2_wedge(vb1b0, normal) * w; - } - } + + if (math_fabs(vab1_wedge_normal) > 0.01f) { + f32 w = 1 / vab1_wedge_normal; + a1t = clamp_f32(-va0a1_wedge_normal * w, 0, 1); + b1t = clamp_f32(-vb0b1_wedge_normal * -w, 0, 1); } } - a0t = clamp_f32(a0t, 0, 1); - a1t = clamp_f32(a1t, 0, 1); - b0t = clamp_f32(b0t, 0, 1); - b1t = clamp_f32(b1t, 0, 1); - struct v2 a0_clipped = v2_add(a0, v2_mul(vab0, a0t)); struct v2 a1_clipped = v2_add(a1, v2_mul(vab1, a1t)); - struct v2 b0_clipped = v2_add(b0, v2_mul(vba0, b0t)); - struct v2 b1_clipped = v2_add(b1, v2_mul(vba1, b1t)); + struct v2 b0_clipped = v2_add(b0, v2_mul(vab0, -b0t)); + struct v2 b1_clipped = v2_add(b1, v2_mul(vab1, -b1t)); struct v2 va0a1_clipped = v2_sub(a1_clipped, a0_clipped); struct v2 vb0b1_clipped = v2_sub(b1_clipped, b0_clipped); f32 a_sep = v2_dot(va0a1_clipped, normal); f32 b_sep = v2_dot(vb0b1_clipped, normal); + + struct v2 contact_a = v2_add(a0_clipped, v2_mul(va0a1_clipped, 0.5f)); + struct v2 contact_b = v2_add(b0_clipped, v2_mul(vb0b1_clipped, 0.5f)); + + //b32 merge_contacts = v2_len_sq(v2_sub(contact_b, contact_a)) < (0.001f * 0.001f); + b32 merge_contacts = false; +#if 1 + if (math_fabs(v2_dot(vab0, normal)) > (0.01f) && + math_fabs(v2_dot(vab1, normal)) > (0.01f)) { + merge_contacts = true; + } +#endif + + if (a_sep < tolerance) { struct collider_collision_point *point = &points[num_points++]; point->id = id_a0 | (id_a1 << 4); point->separation = a_sep; - point->point = v2_add(a0_clipped, v2_mul(va0a1_clipped, 0.5f)); + point->point = contact_a; } - if (b_sep < tolerance) { + + if (b_sep < tolerance && !merge_contacts) { struct collider_collision_point *point = &points[num_points++]; point->id = id_b0 | (id_b1 << 4); point->separation = b_sep; - point->point = v2_add(b0_clipped, v2_mul(vb0b1_clipped, 0.5f)); + point->point = contact_b; } + + +#if 0 res.a0 = a0_clipped; res.a1 = a1_clipped; res.b0 = b0_clipped; res.b1 = b1_clipped; +#else + res.a0 = a0; + res.a1 = a1; + res.b0 = b0; + res.b1 = b1; +#endif } } res.solved = true; abort: if (proto_count > 0) { - for (u32 i = 0; i < min_u32(proto_count, ARRAY_COUNT(res.prototype.points)); ++i) { + u32 len = min_u32(proto_count, ARRAY_COUNT(res.prototype.points)); + for (u32 i = 0; i < len; ++i) { res.prototype.points[i] = proto[i]; } - res.prototype.len = proto_count; + res.prototype.len = len; } else { if (s.len >= 1) { res.prototype.points[0] = s.a; @@ -835,1036 +742,3 @@ b32 collider_collision_boolean(struct collider_shape *shape0, struct collider_sh return false; } #endif - - - - - - -/* ========================== * - * Polygon-only GJK (unused) - * ========================== */ - -#if 0 -INTERNAL struct v2 collider_support_point(struct collider_shape *a, struct v2 dir) -{ - struct v2 furthest = V2(0, 0); - switch (a->kind) { - case COLLIDER_SHAPE_KIND_POLYGON: - { - /* TODO: Could probably binary search for largest dot since shape is convex */ - struct v2 *points = a->polygon.points; - u32 count = a->polygon.count; - furthest = points[0]; - f32 furthest_dot = v2_dot(dir, furthest); - for (u32 i = 1; i < count; ++i) { - struct v2 p = points[i]; - f32 dot = v2_dot(dir, p); - if (dot > furthest_dot) { - furthest = p; - furthest_dot = dot; - } - } - } break; - - case COLLIDER_SHAPE_KIND_CIRCLE: - { - dir = v2_norm(dir); - furthest = v2_add(a->circle.pos, v2_mul(dir, a->circle.radius)); - } break; - } - return furthest; -} - -INTERNAL u32 poly_support_point_index(struct collider_shape *a, struct v2 dir) -{ - /* TODO: Could probably binary search for largest dot since shape is convex */ - ASSERT(a->kind == COLLIDER_SHAPE_KIND_POLYGON); /* Method for polygons only */ - struct v2 *points = a->polygon.points; - u32 count = a->polygon.count; - u32 furthest = 0; - f32 furthest_dot = v2_dot(dir, points[0]); - for (u32 i = 1; i < count; ++i) { - struct v2 p = points[i]; - f32 dot = v2_dot(dir, p); - if (dot > furthest_dot) { - furthest = i; - furthest_dot = dot; - } - } - return furthest; -} - -struct collider_collision_points_result collider_collision_points(struct collider_shape *shape0, struct collider_shape *shape1) -{ - struct temp_arena scratch = scratch_begin_no_conflict(); /* TODO: Only begin scratch for EPA */ - struct collider_collision_points_result res = ZI; - - /* TODO: Parameterize */ - const f32 tolerance = 0.0025f; - const f32 min_unique_pt_dist_sq = 0.001f * 0.001f; - - b32 colliding = false; - b32 simplex_is_closest_edge = false; - - struct collider_simplex s = ZI; - struct v2 *proto = NULL; - u32 proto_count = 0; - - struct v2 normal = ZI; - struct collider_collision_point points[2] = ZI; - u32 num_points = 0; - - struct v2 dir = ZI; - struct v2 m = ZI; - -#if COLLIDER_DEBUG - u32 dbg_step = 0; -#endif - - /* ========================== * - * GJK - * - * Determine encapsulating simplex if colliding, or closest edge / point to - * origin on simplex (for check if shape distances are within tolerance) - * ========================== */ - { - /* First point is support point in shape's general directions to eachother */ - s.a = menkowski_point(shape0, shape1, v2_sub(starting_point(shape1), starting_point(shape0))); - s.len = 1; - - struct v2 removed_a = ZI; - struct v2 removed_b = ZI; - u32 num_removed = 0; - b32 done = false; - while (!done) { - if (s.len == 1) { - /* Second point is support point towards origin */ - dir = v2_neg(s.a); - - DBGSTEP; - m = menkowski_point(shape0, shape1, dir); - /* Check that new point is far enough away from existing point */ - if (v2_len_sq(v2_sub(m, s.a)) < min_unique_pt_dist_sq) { - done = true; - break; - } - s.b = s.a; - s.a = m; - s.len = 2; - - /* Third point is support point in direction of line normal towards origin */ - dir = v2_perp_towards_dir(v2_sub(s.b, s.a), v2_neg(s.a)); - } - - { - DBGSTEP; - m = menkowski_point(shape0, shape1, dir); - /* Check that new point is far enough away from existing points */ - if (v2_len_sq(v2_sub(m, s.a)) < min_unique_pt_dist_sq || - v2_len_sq(v2_sub(m, s.b)) < min_unique_pt_dist_sq || - ((num_removed >= 1) && ( - (v2_len_sq(v2_sub(m, removed_a)) < min_unique_pt_dist_sq) || - (num_removed >= 2 && v2_len_sq(v2_sub(m, removed_b)) < min_unique_pt_dist_sq) - )) || - math_fabs(v2_wedge(v2_sub(s.b, s.a), v2_sub(m, s.a))) < min_unique_pt_dist_sq - ) { - simplex_is_closest_edge = true; - done = true; - break; - } - s.c = s.b; - s.b = s.a; - s.a = m; - s.len = 3; - } - - /* Determine voronoi region of the simplex in which the origin lies */ - i32 voronoi_mask = 0; - struct v2 vab = v2_sub(s.b, s.a); - struct v2 vac = v2_sub(s.c, s.a); - struct v2 vbc = v2_sub(s.c, s.b); - struct v2 rab_dir = v2_perp_towards_dir(vab, v2_neg(vac)); - struct v2 rac_dir = v2_perp_towards_dir(vac, v2_neg(vab)); - struct v2 rbc_dir = v2_perp_towards_dir(vbc, vab); - voronoi_mask |= (v2_dot(rab_dir, v2_neg(s.a)) > 0) << 0; /* Regions ab, a, and b*/ - voronoi_mask |= (v2_dot(rac_dir, v2_neg(s.a)) > 0) << 1; /* Regions ac, a, and c */ - voronoi_mask |= (v2_dot(rbc_dir, v2_neg(s.b)) > 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 */ - colliding = true; - done = true; - } break; - case 1: - { /* Region ab, remove c */ - num_removed = 1; - removed_a = s.c; - s.len = 2; - dir = rab_dir; /* Next third point is in direction of region ab */ - } break; - case 2: - { /* Region ac, remove b */ - num_removed = 1; - removed_a = s.b; - s.len = 2; - s.b = s.c; - dir = rac_dir; /* Next third point is in direction of region ac */ - } break; - case 4: - { /* Region bc, remove a */ - num_removed = 1; - removed_a = s.a; - s.len = 2; - s.a = s.b; - s.b = s.c; - dir = rbc_dir; /* Next third point is in direction of region bc */ - } break; - case 3: - { /* Region a, remove bc */ - num_removed = 2; - removed_a = s.b; - removed_b = s.c; - s.len = 1; - } break; - case 5: - { /* Region b, remove ac */ - num_removed = 2; - removed_a = s.a; - removed_b = s.c; - s.len = 1; - s.a = s.b; - } break; - case 6: - { /* Region c, remove ab */ - num_removed = 2; - removed_a = s.a; - removed_b = s.b; - s.len = 1; - s.a = s.c; - } break; - default: - { - /* Unknown region (should be impossible) */ - ASSERT(false); - res.path = -1; - done = true; - } break; - } - } - } - - if (colliding) { - /* ========================== * - * Epa (to find collision normal from inside shape) - * ========================== */ - - proto = arena_dry_push(scratch.arena, struct v2); - proto_count = 0; - { - ASSERT(s.len == 3); - struct v2 *tmp = arena_push_array(scratch.arena, struct v2, 3); - tmp[0] = s.a; - tmp[1] = s.b; - tmp[2] = s.c; - proto_count = 3; - } - - while (colliding) { - 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]; - 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_len_sq(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_len_sq = pd_len_sq; - } - } - - /* TODO: Remove this (debugging) */ - s.a = proto[pen_ps_index]; - s.b = proto[pen_pe_index]; - s.len = 2; - - /* Find new point in dir */ - DBGSTEP; - { - /* Next point is in direction of line normal pointing outwards from simplex */ - /* TODO: If winding order is guaranteed then this can become v2_perp_left/right? */ - struct v2 a = proto[pen_ps_index]; - struct v2 b = proto[pen_pe_index]; - struct v2 n = proto[(pen_pe_index < proto_count - 1) ? (pen_pe_index + 1) : 0]; /* Next point along prototype after edge */ - struct v2 vab = v2_sub(b, a); - struct v2 vna = v2_sub(a, n); - dir = v2_perp_towards_dir(vab, vna); - } - m = menkowski_point(shape0, shape1, dir); - - /* Check unique */ - /* TODO: Better */ - { - b32 unique = true; - for (u32 i = 0; i < proto_count; ++i) { - struct v2 edge_start = proto[i]; - struct v2 edge_end = i < proto_count - 1 ? proto[i + 1] : proto[0]; - struct v2 vsm = v2_sub(m, edge_start); - if (v2_len_sq(vsm) < min_unique_pt_dist_sq || - math_fabs(v2_wedge(v2_sub(edge_end, edge_start), vsm)) < min_unique_pt_dist_sq) { - unique = false; - break; - } - } - if (!unique) { - res.path = 1; - normal = v2_norm(dir); - break; - } - } - - /* Insert point into prototype */ - /* FIXME: Preserve winding order */ - arena_push(scratch.arena, struct collider_menkowski_point); - ++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; - } - } else if (simplex_is_closest_edge && s.len > 1) { - /* Shapes are not overlapping (origin is outside of simplex). Project - * origin to determine if distance is within tolerance. */ - ASSERT(s.len == 2); - struct v2 vab = v2_sub(s.b, s.a); - struct v2 vao = v2_neg(s.a); - f32 ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1); - struct v2 p = v2_add(s.a, v2_mul(vab, ratio)); - if (v2_len_sq(p) <= (tolerance * tolerance)) { - res.path = 2; - normal = v2_norm(dir); - colliding = true; - } - } - - if (colliding) { - /* ========================== * - * Clip to determine final points - * ========================== */ - - /* Max vertices must be < 16 to fit in 4 bit ids */ - CT_ASSERT(ARRAY_COUNT(shape0->polygon.points) <= 16); - - DBGSTEP; - { - const f32 wedge_epsilon = 0.001f; - - /* shape0 a -> b winding = clockwise */ - u32 id_a0; - u32 id_b0; - struct v2 a0; - struct v2 b0; - struct v2 vab0; - - /* shape1 a -> b winding = counterclockwise */ - u32 id_a1; - u32 id_b1; - struct v2 a1; - struct v2 b1; - struct v2 vab1; - { - struct v2 *shape0_points = shape0->polygon.points; - u32 shape0_count = shape0->polygon.count; - - u32 p_i = poly_support_point_index(shape0, normal); - u32 a_i = (p_i > 0) ? (p_i - 1) : (shape0_count - 1); - u32 b_i = ((p_i + 1) < shape0_count) ? (p_i + 1) : 0; - - struct v2 p = shape0_points[p_i]; - struct v2 a = shape0_points[a_i]; - struct v2 b = shape0_points[b_i]; - - struct v2 vap = v2_sub(p, a); - struct v2 vpb = v2_sub(b, p); - - /* FIXME: Make winding order independent */ - if (v2_wedge(vap, normal) < (v2_wedge(vpb, normal) + wedge_epsilon)) { - id_a0 = a_i; - id_b0 = p_i; - a0 = a; - b0 = p; - vab0 = vap; - } else { - id_a0 = p_i; - id_b0 = b_i; - a0 = p; - b0 = b; - vab0 = vpb; - } - } - { - struct v2 neg_normal = v2_neg(normal); - - struct v2 *shape1_points = shape1->polygon.points; - u32 shape1_count = shape1->polygon.count; - - u32 p_i = poly_support_point_index(shape1, neg_normal); - u32 a_i = ((p_i + 1) < shape1_count) ? (p_i + 1) : 0; - u32 b_i = (p_i > 0) ? (p_i - 1) : (shape1_count - 1); - - struct v2 p = shape1_points[p_i]; - struct v2 a = shape1_points[a_i]; - struct v2 b = shape1_points[b_i]; - - struct v2 vap = v2_sub(p, a); - struct v2 vpb = v2_sub(b, p); - - /* FIXME: Make winding order independent */ - if (v2_wedge(vap, normal) < (v2_wedge(vpb, normal) + wedge_epsilon)) { - id_a1 = a_i; - id_b1 = p_i; - a1 = a; - b1 = p; - vab1 = vap; - } else { - id_a1 = p_i; - id_b1 = b_i; - a1 = p; - b1 = b; - vab1 = vpb; - } - } - - f32 a0t = 0; - f32 a1t = 0; - f32 b0t = 0; - f32 b1t = 0; - - struct v2 vba0 = v2_neg(vab0); - struct v2 vba1 = v2_neg(vab1); - { - { - struct v2 va0a1 = v2_sub(a1, a0); - struct v2 va1a0 = v2_neg(va0a1); - { - f32 w = v2_wedge(vab0, normal); - if (w != 0) { - w = 1 / w; - a0t = v2_wedge(va0a1, normal) * w; - } - } - { - f32 w = v2_wedge(vab1, normal); - if (w != 0) { - w = 1 / w; - a1t = v2_wedge(va1a0, normal) * w; - } - } - } - { - struct v2 vb0b1 = v2_sub(b1, b0); - struct v2 vb1b0 = v2_neg(vb0b1); - { - f32 w = v2_wedge(vba0, normal); - if (w != 0) { - w = 1 / w; - b0t = v2_wedge(vb0b1, normal) * w; - } - } - { - f32 w = v2_wedge(vba1, normal); - if (w != 0) { - w = 1 / w; - b1t = v2_wedge(vb1b0, normal) * w; - } - } - } - } - - a0t = clamp_f32(a0t, 0, 1); - a1t = clamp_f32(a1t, 0, 1); - b0t = clamp_f32(b0t, 0, 1); - b1t = clamp_f32(b1t, 0, 1); - - struct v2 a0_clipped = v2_add(a0, v2_mul(vab0, a0t)); - struct v2 a1_clipped = v2_add(a1, v2_mul(vab1, a1t)); - struct v2 b0_clipped = v2_add(b0, v2_mul(vba0, b0t)); - struct v2 b1_clipped = v2_add(b1, v2_mul(vba1, b1t)); - - struct v2 va0a1_clipped = v2_sub(a1_clipped, a0_clipped); - struct v2 vb0b1_clipped = v2_sub(b1_clipped, b0_clipped); - - f32 a_sep = v2_dot(va0a1_clipped, normal); - f32 b_sep = v2_dot(vb0b1_clipped, normal); - if (a_sep < tolerance) { - struct collider_collision_point *point = &points[num_points++]; - point->id = id_a0 | (id_a1 << 4); - point->separation = a_sep; - point->point = v2_add(a0_clipped, v2_mul(va0a1_clipped, 0.5f)); - } - if (b_sep < tolerance) { - struct collider_collision_point *point = &points[num_points++]; - point->id = id_b0 | (id_b1 << 4); - point->separation = b_sep; - point->point = v2_add(b0_clipped, v2_mul(vb0b1_clipped, 0.5f)); - } - } - } - - 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]; - } - res.prototype.len = proto_count; - } else { - if (s.len >= 1) { - res.prototype.points[0] = s.a; - if (s.len >= 2) { - res.prototype.points[1] = s.b; - if (s.len >= 3) { - res.prototype.points[2] = s.c; - } - } - } - res.prototype.len = s.len; - } - res.normal = normal; - res.points[0] = points[0]; - res.points[1] = points[1]; - res.num_points = num_points; - res.simplex = s; - scratch_end(scratch); - return res; -} - -/* ========================== * - * Debug functions - * TODO: Remove these - * ========================== */ - - /* TODO: Remove this (debugging) */ -struct v2_array menkowski(struct arena *arena, struct collider_shape *shape0, struct collider_shape *shape1) -{ - 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(shape0, shape1, dir); - if (res.count == 0 || !v2_eq(p, res.points[res.count - 1])) { - *arena_push(arena, struct v2) = p; - ++res.count; - } - } - return res; -} - -/* TODO: Remove this (debugging) */ -struct v2_array cloud(struct arena *arena, struct collider_shape *shape0, struct collider_shape *shape1) -{ - struct v2_array res = { .points = arena_dry_push(arena, struct v2) }; - if (shape0->kind == COLLIDER_SHAPE_KIND_POLYGON && shape1->kind == COLLIDER_SHAPE_KIND_POLYGON) { - struct v2 *points0 = shape0->polygon.points; - struct v2 *points1 = shape1->polygon.points; - u32 count0 = shape0->polygon.count; - u32 count1 = shape1->polygon.count; - for (u64 i = 0; i < count0; ++i) { - struct v2 p0 = points0[i]; - for (u64 j = 0; j < count1; ++j) { - struct v2 p1 = points1[j]; - *arena_push(arena, struct v2) = v2_sub(p0, p1); - ++res.count; - } - } - } - return res; -} - - - - - - - - - -struct collider_collision_points_result collider_collision_points(struct collider_shape *shape0, struct collider_shape *shape1) -{ - struct temp_arena scratch = scratch_begin_no_conflict(); /* TODO: Only begin scratch for EPA */ - struct collider_collision_points_result res = ZI; - - /* TODO: Parameterize */ - const f32 tolerance = 0.0025f; - const f32 min_unique_pt_dist_sq = 0.001f * 0.001f; - - b32 colliding = false; - b32 simplex_is_closest_edge = false; - - struct collider_simplex s = ZI; - struct v2 *proto = NULL; - u32 proto_count = 0; - - struct v2 normal = ZI; - struct collider_collision_point points[2] = ZI; - u32 num_points = 0; - - struct v2 dir = ZI; - struct v2 m = ZI; - -#if COLLIDER_DEBUG - u32 dbg_step = 0; -#endif - - /* ========================== * - * GJK - * - * Determine encapsulating simplex if colliding, or closest edge / point to - * origin on simplex (for check if shape distances are within tolerance) - * ========================== */ - { - /* First point is support point in shape's general directions to eachother */ - s.a = menkowski_point(shape0, shape1, v2_sub(starting_point(shape1), starting_point(shape0))); - s.len = 1; - - struct v2 removed_a = ZI; - struct v2 removed_b = ZI; - u32 num_removed = 0; - b32 done = false; - while (!done) { - if (s.len == 1) { - /* Second point is support point towards origin */ - dir = v2_neg(s.a); - - DBGSTEP; - m = menkowski_point(shape0, shape1, dir); - /* Check that new point is far enough away from existing point */ - if (v2_len_sq(v2_sub(m, s.a)) < min_unique_pt_dist_sq) { - done = true; - break; - } - s.b = s.a; - s.a = m; - s.len = 2; - - /* Third point is support point in direction of line normal towards origin */ - dir = v2_perp_towards_dir(v2_sub(s.b, s.a), v2_neg(s.a)); - } - - { - DBGSTEP; - m = menkowski_point(shape0, shape1, dir); - /* Check that new point is far enough away from existing points */ - if (v2_len_sq(v2_sub(m, s.a)) < min_unique_pt_dist_sq || - v2_len_sq(v2_sub(m, s.b)) < min_unique_pt_dist_sq || - ((num_removed >= 1) && ( - (v2_len_sq(v2_sub(m, removed_a)) < min_unique_pt_dist_sq) || - (num_removed >= 2 && v2_len_sq(v2_sub(m, removed_b)) < min_unique_pt_dist_sq) - )) || - math_fabs(v2_wedge(v2_sub(s.b, s.a), v2_sub(m, s.a))) < min_unique_pt_dist_sq - ) { - simplex_is_closest_edge = true; - done = true; - break; - } - s.c = s.b; - s.b = s.a; - s.a = m; - s.len = 3; - } - - /* Determine voronoi region of the simplex in which the origin lies */ - i32 voronoi_mask = 0; - struct v2 vab = v2_sub(s.b, s.a); - struct v2 vac = v2_sub(s.c, s.a); - struct v2 vbc = v2_sub(s.c, s.b); - struct v2 rab_dir = v2_perp_towards_dir(vab, v2_neg(vac)); - struct v2 rac_dir = v2_perp_towards_dir(vac, v2_neg(vab)); - struct v2 rbc_dir = v2_perp_towards_dir(vbc, vab); - voronoi_mask |= (v2_dot(rab_dir, v2_neg(s.a)) > 0) << 0; /* Regions ab, a, and b*/ - voronoi_mask |= (v2_dot(rac_dir, v2_neg(s.a)) > 0) << 1; /* Regions ac, a, and c */ - voronoi_mask |= (v2_dot(rbc_dir, v2_neg(s.b)) > 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 */ - colliding = true; - done = true; - } break; - case 1: - { /* Region ab, remove c */ - num_removed = 1; - removed_a = s.c; - s.len = 2; - dir = rab_dir; /* Next third point is in direction of region ab */ - } break; - case 2: - { /* Region ac, remove b */ - num_removed = 1; - removed_a = s.b; - s.len = 2; - s.b = s.c; - dir = rac_dir; /* Next third point is in direction of region ac */ - } break; - case 4: - { /* Region bc, remove a */ - num_removed = 1; - removed_a = s.a; - s.len = 2; - s.a = s.b; - s.b = s.c; - dir = rbc_dir; /* Next third point is in direction of region bc */ - } break; - case 3: - { /* Region a, remove bc */ - num_removed = 2; - removed_a = s.b; - removed_b = s.c; - s.len = 1; - } break; - case 5: - { /* Region b, remove ac */ - num_removed = 2; - removed_a = s.a; - removed_b = s.c; - s.len = 1; - s.a = s.b; - } break; - case 6: - { /* Region c, remove ab */ - num_removed = 2; - removed_a = s.a; - removed_b = s.b; - s.len = 1; - s.a = s.c; - } break; - default: - { - /* Unknown region (should be impossible) */ - ASSERT(false); - res.path = -1; - done = true; - } break; - } - } - } - - if (colliding) { - /* ========================== * - * Epa (to find collision normal from inside shape) - * ========================== */ - - proto = arena_dry_push(scratch.arena, struct v2); - proto_count = 0; - { - ASSERT(s.len == 3); - struct v2 *tmp = arena_push_array(scratch.arena, struct v2, 3); - tmp[0] = s.a; - tmp[1] = s.b; - tmp[2] = s.c; - proto_count = 3; - } - - while (colliding) { - 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]; - 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_len_sq(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_len_sq = pd_len_sq; - } - } - - /* TODO: Remove this (debugging) */ - s.a = proto[pen_ps_index]; - s.b = proto[pen_pe_index]; - s.len = 2; - - /* Find new point in dir */ - DBGSTEP; - { - /* Next point is in direction of line normal pointing outwards from simplex */ - /* TODO: If winding order is guaranteed then this can become v2_perp_left/right? */ - struct v2 a = proto[pen_ps_index]; - struct v2 b = proto[pen_pe_index]; - struct v2 n = proto[(pen_pe_index < proto_count - 1) ? (pen_pe_index + 1) : 0]; /* Next point along prototype after edge */ - struct v2 vab = v2_sub(b, a); - struct v2 vna = v2_sub(a, n); - dir = v2_perp_towards_dir(vab, vna); - } - m = menkowski_point(shape0, shape1, dir); - - /* Check unique */ - /* TODO: Better */ - { - b32 unique = true; - for (u32 i = 0; i < proto_count; ++i) { - struct v2 edge_start = proto[i]; - struct v2 edge_end = i < proto_count - 1 ? proto[i + 1] : proto[0]; - struct v2 vsm = v2_sub(m, edge_start); - if (v2_len_sq(vsm) < min_unique_pt_dist_sq || - math_fabs(v2_wedge(v2_sub(edge_end, edge_start), vsm)) < min_unique_pt_dist_sq) { - unique = false; - break; - } - } - if (!unique) { - res.path = 1; - normal = v2_norm(dir); - break; - } - } - - /* Insert point into prototype */ - /* FIXME: Preserve winding order */ - arena_push(scratch.arena, struct collider_menkowski_point); - ++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; - } - } else if (simplex_is_closest_edge && s.len > 1) { - /* Shapes are not overlapping (origin is outside of simplex). Project - * origin to determine if distance is within tolerance. */ - ASSERT(s.len == 2); - struct v2 vab = v2_sub(s.b, s.a); - struct v2 vao = v2_neg(s.a); - f32 ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1); - struct v2 p = v2_add(s.a, v2_mul(vab, ratio)); - if (v2_len_sq(p) <= (tolerance * tolerance)) { - res.path = 2; - normal = v2_norm(dir); - colliding = true; - } - } - - if (colliding) { - /* ========================== * - * Clip to determine final points - * ========================== */ - - /* Max vertices must be < 16 to fit in 4 bit ids */ - CT_ASSERT(ARRAY_COUNT(shape0->polygon.points) <= 16); - - DBGSTEP; - { - const f32 wedge_epsilon = 0.001f; - - /* shape0 a -> b winding = clockwise */ - u32 id_a0; - u32 id_b0; - struct v2 a0; - struct v2 b0; - struct v2 vab0; - - /* shape1 a -> b winding = counterclockwise */ - u32 id_a1; - u32 id_b1; - struct v2 a1; - struct v2 b1; - struct v2 vab1; - { - struct v2 *shape0_points = shape0->polygon.points; - u32 shape0_count = shape0->polygon.count; - - u32 p_i = poly_support_point_index(shape0, normal); - u32 a_i = (p_i > 0) ? (p_i - 1) : (shape0_count - 1); - u32 b_i = ((p_i + 1) < shape0_count) ? (p_i + 1) : 0; - - struct v2 p = shape0_points[p_i]; - struct v2 a = shape0_points[a_i]; - struct v2 b = shape0_points[b_i]; - - struct v2 vap = v2_sub(p, a); - struct v2 vpb = v2_sub(b, p); - - /* FIXME: Make winding order independent */ - if (v2_wedge(vap, normal) < (v2_wedge(vpb, normal) + wedge_epsilon)) { - id_a0 = a_i; - id_b0 = p_i; - a0 = a; - b0 = p; - vab0 = vap; - } else { - id_a0 = p_i; - id_b0 = b_i; - a0 = p; - b0 = b; - vab0 = vpb; - } - } - { - struct v2 neg_normal = v2_neg(normal); - - struct v2 *shape1_points = shape1->polygon.points; - u32 shape1_count = shape1->polygon.count; - - u32 p_i = poly_support_point_index(shape1, neg_normal); - u32 a_i = ((p_i + 1) < shape1_count) ? (p_i + 1) : 0; - u32 b_i = (p_i > 0) ? (p_i - 1) : (shape1_count - 1); - - struct v2 p = shape1_points[p_i]; - struct v2 a = shape1_points[a_i]; - struct v2 b = shape1_points[b_i]; - - struct v2 vap = v2_sub(p, a); - struct v2 vpb = v2_sub(b, p); - - /* FIXME: Make winding order independent */ - if (v2_wedge(vap, normal) < (v2_wedge(vpb, normal) + wedge_epsilon)) { - id_a1 = a_i; - id_b1 = p_i; - a1 = a; - b1 = p; - vab1 = vap; - } else { - id_a1 = p_i; - id_b1 = b_i; - a1 = p; - b1 = b; - vab1 = vpb; - } - } - - f32 a0t = 0; - f32 a1t = 0; - f32 b0t = 0; - f32 b1t = 0; - - struct v2 vba0 = v2_neg(vab0); - struct v2 vba1 = v2_neg(vab1); - { - { - struct v2 va0a1 = v2_sub(a1, a0); - struct v2 va1a0 = v2_neg(va0a1); - { - f32 w = v2_wedge(vab0, normal); - if (w != 0) { - w = 1 / w; - a0t = v2_wedge(va0a1, normal) * w; - } - } - { - f32 w = v2_wedge(vab1, normal); - if (w != 0) { - w = 1 / w; - a1t = v2_wedge(va1a0, normal) * w; - } - } - } - { - struct v2 vb0b1 = v2_sub(b1, b0); - struct v2 vb1b0 = v2_neg(vb0b1); - { - f32 w = v2_wedge(vba0, normal); - if (w != 0) { - w = 1 / w; - b0t = v2_wedge(vb0b1, normal) * w; - } - } - { - f32 w = v2_wedge(vba1, normal); - if (w != 0) { - w = 1 / w; - b1t = v2_wedge(vb1b0, normal) * w; - } - } - } - } - - a0t = clamp_f32(a0t, 0, 1); - a1t = clamp_f32(a1t, 0, 1); - b0t = clamp_f32(b0t, 0, 1); - b1t = clamp_f32(b1t, 0, 1); - - struct v2 a0_clipped = v2_add(a0, v2_mul(vab0, a0t)); - struct v2 a1_clipped = v2_add(a1, v2_mul(vab1, a1t)); - struct v2 b0_clipped = v2_add(b0, v2_mul(vba0, b0t)); - struct v2 b1_clipped = v2_add(b1, v2_mul(vba1, b1t)); - - struct v2 va0a1_clipped = v2_sub(a1_clipped, a0_clipped); - struct v2 vb0b1_clipped = v2_sub(b1_clipped, b0_clipped); - - f32 a_sep = v2_dot(va0a1_clipped, normal); - f32 b_sep = v2_dot(vb0b1_clipped, normal); - if (a_sep < tolerance) { - struct collider_collision_point *point = &points[num_points++]; - point->id = id_a0 | (id_a1 << 4); - point->separation = a_sep; - point->point = v2_add(a0_clipped, v2_mul(va0a1_clipped, 0.5f)); - } - if (b_sep < tolerance) { - struct collider_collision_point *point = &points[num_points++]; - point->id = id_b0 | (id_b1 << 4); - point->separation = b_sep; - point->point = v2_add(b0_clipped, v2_mul(vb0b1_clipped, 0.5f)); - } - } - } - - 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]; - } - res.prototype.len = proto_count; - } else { - if (s.len >= 1) { - res.prototype.points[0] = s.a; - if (s.len >= 2) { - res.prototype.points[1] = s.b; - if (s.len >= 3) { - res.prototype.points[2] = s.c; - } - } - } - res.prototype.len = s.len; - } - res.normal = normal; - res.points[0] = points[0]; - res.points[1] = points[1]; - res.num_points = num_points; - res.simplex = s; - scratch_end(scratch); - return res; -} -#endif diff --git a/src/collider.h b/src/collider.h index b00f1b35..4814513c 100644 --- a/src/collider.h +++ b/src/collider.h @@ -29,7 +29,7 @@ struct collider_collision_point { u32 id; /* Based on polygon edge-to-edge */ }; -struct collider_prototype { struct v2 points[256]; u32 len; }; +struct collider_prototype { struct v2 points[64]; u32 len; }; struct collider_collision_points_result { struct v2 normal; diff --git a/src/game.c b/src/game.c index 168d9182..0cf520f3 100644 --- a/src/game.c +++ b/src/game.c @@ -166,6 +166,61 @@ INTERNAL void spawn_test_entities(f32 offset) entity_enable_prop(e, ENTITY_PROP_PLAYER_CONTROLLED); //e->control_force = 4500; + e->control_force = 1200; + //e->control_force = 250; + e->control_torque = 10; + e->control.focus = V2(0, -1); + + entity_enable_prop(e, ENTITY_PROP_PHYSICAL); + e->mass_unscaled = 100; + //e->inertia_unscaled = F32_INFINITY; + e->inertia_unscaled = 25; + e->linear_ground_friction = 1000; + e->angular_ground_friction = 100; + + //entity_enable_prop(e, ENTITY_PROP_TEST); + + player_ent = e; + } + + { + //struct v2 pos = V2(0.25, -10); + //struct v2 pos = V2(0.25, -7); + //struct v2 pos = V2(0.25, -5.27); + //struct v2 pos = V2(0.85, -2); + struct v2 pos = V2(0.25, -2); + //struct v2 pos = V2(1.1230469346046448864129274625156, -1); /* Touching right side of box */ + //struct v2 pos = V2(1.1230469346046448864129274625156 - 0.0001, -1); /* Touching right side of box */ + //struct v2 pos = V2(0.374142020941, -0.246118023992); /* Touching glitch spot */ + + pos = v2_add(pos, V2(0, offset)); + pos = v2_add(pos, V2(0, offset_all)); + + //struct v2 size = V2(1, 1); + struct v2 size = V2(0.5, 0.5); + //f32 r = PI; + //f32 r = PI / 4; + //f32 r = PI / 3; + //f32 r = 0.05; + //f32 r = PI / 2; + f32 r = 0; + //f32 skew = PI / 4; + f32 skew = 0; + + struct entity *e = entity_alloc(root); + + struct xform xf = XFORM_TRS(.t = pos, .r = r, .s = size); + xf = xform_skewed_to(xf, skew); + entity_set_xform(e, xf); + + //e->sprite = sprite_tag_from_path(STR("res/graphics/tim.ase")); + e->sprite = sprite_tag_from_path(STR("res/graphics/box.ase")); + //e->sprite_span_name = STR("idle.unarmed"); + //e->sprite_span_name = STR("idle.one_handed"); + e->sprite_span_name = STR("idle.two_handed"); + + //entity_enable_prop(e, ENTITY_PROP_PLAYER_CONTROLLED); + //e->control_force = 4500; //e->control_force = 1200; e->control_force = 250; e->control_torque = 10; @@ -173,14 +228,12 @@ INTERNAL void spawn_test_entities(f32 offset) entity_enable_prop(e, ENTITY_PROP_PHYSICAL); e->mass_unscaled = 100; - e->inertia_unscaled = F32_INFINITY; - //e->inertia_unscaled = 25; + //e->inertia_unscaled = F32_INFINITY; + e->inertia_unscaled = 25; e->linear_ground_friction = 1000; e->angular_ground_friction = 100; //entity_enable_prop(e, ENTITY_PROP_TEST); - - player_ent = e; } /* Weapon */ @@ -206,7 +259,7 @@ INTERNAL void spawn_test_entities(f32 offset) } /* Box */ -#if 1 +#if 0 if (!G.first_spawn) { struct v2 pos = V2(0.5, -1); //struct v2 pos = V2(0.5, 29); @@ -575,7 +628,7 @@ INTERNAL struct soft_result make_soft(f32 hertz, f32 zeta, f32 h) INTERNAL void solve_collisions(f32 dt, b32 apply_bias) { -#if 1 +#if 0 struct entity_store *store = G.tick.entity_store; for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { struct entity *manifold = &store->entities[entity_index]; @@ -957,16 +1010,25 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("shape"), ent->animation_frame); ent->local_collider = collider_from_quad(xform_mul_quad(ent->sprite_local_xform, quad_from_rect(slice.rect))); - if (entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED)) { + //if (entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED)) { + if ((true)) { #if 0 ent->local_collider.points[0] = V2(0, 0); ent->local_collider.count = 1; ent->local_collider.radius = 0.5; -#else +#elif 0 ent->local_collider.points[0] = V2(0, 0.5); ent->local_collider.points[1] = V2(0, -0.5); ent->local_collider.count = 2; ent->local_collider.radius = 0.25; +#elif 1 + ent->local_collider.points[0] = V2(-0.5, 0.5); + ent->local_collider.points[1] = V2(0.5, 0.5); + ent->local_collider.points[2] = V2(0, -0.5); + ent->local_collider.count = 3; + ent->local_collider.radius = 0.25; +#else + ent->local_collider.radius = 0.25; #endif } } @@ -1358,7 +1420,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) * Create ground friction force (gravity) * ========================== */ -#if 0 +#if 1 /* TODO: Do this globally rather than creating entities for constant forces */ for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { struct entity *ent = &store->entities[entity_index]; @@ -1434,7 +1496,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) #endif solve_collisions(substep_dt, true); integrate_positions_from_velocities(substep_dt); - solve_collisions(substep_dt, false); /* Relaxation */ + //solve_collisions(substep_dt, false); /* Relaxation */ } } #else diff --git a/src/user.c b/src/user.c index 275195a3..b297c899 100644 --- a/src/user.c +++ b/src/user.c @@ -1137,7 +1137,7 @@ INTERNAL void user_update(void) { struct font *disp_font = font_load_async(STR("res/fonts/fixedsys.ttf"), 12.0f); if (disp_font) { - f32 offset_px = -20; + f32 offset_px = 10; struct string fmt = STR( "path: %F\n" @@ -1146,7 +1146,9 @@ INTERNAL void user_update(void) "id: 0x%F\n" "impulse (n): %F\n" "impulse (t): %F\n" - "separation: %F" + "separation: %F\n" + "normal: (%F, %F)\n" + "num contacts: %F" ); struct string text = string_format(temp.arena, fmt, FMT_SINT(ent->res.path), @@ -1155,7 +1157,9 @@ INTERNAL void user_update(void) FMT_HEX(contact.id), FMT_FLOAT_P(contact.normal_impulse, 3), FMT_FLOAT_P(contact.tangent_impulse, 3), - FMT_FLOAT_P(contact.starting_separation, 6)); + FMT_FLOAT_P(contact.starting_separation, 6), + FMT_FLOAT_P(ent->manifold_normal.x, 6), FMT_FLOAT_P(ent->manifold_normal.y, 6), + FMT_UINT(ent->num_contacts)); draw_text(G.viewport_canvas, disp_font, v2_add(v2_round(xform_mul_v2(G.world_view, point)), V2(0, offset_px)), text); @@ -1168,8 +1172,8 @@ INTERNAL void user_update(void) #if 1 /* Draw clipping */ { - f32 thickness = 2; - u32 color = RGBA_32_F(1, 0, 1, 0.75); + f32 thickness = 4; + u32 color = RGBA_32_F(1, 0, 1, 0.5); { struct v2 start = xform_mul_v2(G.world_view, ent->res.a0); struct v2 end = xform_mul_v2(G.world_view, ent->res.b0);