diff --git a/src/collider.c b/src/collider.c index 764c0380..a75565b1 100644 --- a/src/collider.c +++ b/src/collider.c @@ -100,7 +100,8 @@ struct collider_collision_points_result collider_collision_points(struct collide const f32 tolerance = 0.005f; /* NOTE: Should always be less than tolerance, since colliding=true if origin is within this distance. */ - const f32 min_unique_pt_dist_sq = 0.0001f * 0.0001f; + //const f32 min_unique_pt_dist_sq = 0.0001f * 0.0001f; + const f32 min_unique_pt_dist_sq = 0.001f * 0.001f; /* To prevent extremely large prototypes when origin is in exact center of rounded feature */ const u32 max_epa_iterations = 64; @@ -155,14 +156,8 @@ struct collider_collision_points_result collider_collision_points(struct collide s.a = m; s.len = 2; - if (math_fabs(v2_wedge(v2_sub(s.b, s.a), v2_neg(s.a))) <= min_unique_pt_dist_sq) { - /* Third point is support point in direction of line normal towards origin */ - dir = v2_perp(v2_sub(s.b, s.a)); - } else { - /* 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)); - } - + /* 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)); } { @@ -178,6 +173,7 @@ struct collider_collision_points_result collider_collision_points(struct collide ) || math_fabs(v2_wedge(v2_sub(s.b, s.a), v2_sub(m, s.a))) < min_unique_pt_dist_sq) { + colliding = false; simplex_is_closest_edge = true; break; } @@ -186,15 +182,13 @@ struct collider_collision_points_result collider_collision_points(struct collide s.a = m; s.len = 3; -#if 1 - if (colliding || - 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) { + 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.b), v2_neg(s.b))) <= min_unique_pt_dist_sq) || + (math_fabs(v2_wedge(v2_sub(s.c, s.a), v2_neg(s.a))) <= min_unique_pt_dist_sq)) { /* Simplex lies on origin */ colliding = true; break; } -#endif } /* Determine region of the simplex in which the origin lies */ @@ -269,11 +263,6 @@ struct collider_collision_points_result collider_collision_points(struct collide * Epa (to find collision normal from inside shape) * ========================== */ - //const f32 epa_normal_epsilon_sq = 0; - //const f32 epa_normal_epsilon_sq = 0.01f * 0.01f; - //const f32 epa_normal_epsilon_sq = 0.001f * 0.001f; - const f32 epa_normal_epsilon_sq = F32_INFINITY; - proto = arena_dry_push(scratch.arena, struct v2); proto_count = 0; { @@ -290,96 +279,86 @@ struct collider_collision_points_result collider_collision_points(struct collide 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; + f32 closest_len_sq = F32_INFINITY; + struct v2 closest_a = ZI; + struct v2 closest_b = ZI; + u32 closest_b_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]; + u32 a_index = i; + u32 b_index = (i < proto_count - 1) ? (i + 1) : 0; + struct v2 a = proto[a_index]; + struct v2 b = proto[b_index]; - struct v2 vse = v2_sub(pe, ps); - struct v2 vso = v2_neg(ps); + struct v2 vab = v2_sub(b, a); + struct v2 vao = v2_neg(a); - struct v2 vsd = v2_mul(vse, clamp_f32(v2_dot(vso, vse) / v2_len_sq(vse), 0, 1)); - //struct v2 vsd = v2_mul(vse, v2_dot(vso, vse) / v2_len_sq(vse)); - struct v2 pd = v2_add(ps, vsd); + f32 proj_ratio = clamp_f32(v2_dot(vao, vab) / v2_len_sq(vab), 0, 1); + struct v2 proj = v2_add(a, v2_mul(vab, proj_ratio)); - f32 pd_len_sq = v2_len_sq(pd); - if (pd_len_sq < pen_len_sq - min_unique_pt_dist_sq) { - pen_ps_index = ps_index; - pen_pe_index = pe_index; - pen_len_sq = pd_len_sq; - pen = pd; + f32 proj_len_sq = v2_len_sq(proj); + if (proj_len_sq < closest_len_sq - min_unique_pt_dist_sq) { + closest_a = a; + closest_b = b; + closest_b_index = b_index; + closest_len_sq = proj_len_sq; } } - - /* TODO: Remove this (debugging) */ - s.a = proto[pen_ps_index]; - s.b = proto[pen_pe_index]; - s.len = 2; + struct v2 vab = v2_sub(closest_b, closest_a); /* Find new point in dir */ - DBGSTEP; - { - /* 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 vab = v2_sub(b, a); -#if 0 - if (pen_len_sq < epa_normal_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); - } -#else - dir = v2_mul(v2_perp(vab), winding); -#endif - } + dir = v2_mul(v2_perp(vab), winding); m = menkowski_point(shape0, shape1, xf0, xf1, dir); - /* Check unique */ - /* TODO: Better */ + /* TODO: Remove this (debugging) */ { - 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; + normal = v2_norm(dir); + s.a = closest_a; + s.b = closest_b; + s.len = 2; + } + + /* Check validity of new point */ + DBGSTEP; + { + b32 valid = true; + + { + const f32 validity_epsilon = min_unique_pt_dist_sq; /* Arbitrary */ + //const f32 validity_epsilon = 0.000000001f; /* Arbitrary */ + + struct v2 vam = v2_sub(m, closest_a); + struct v2 vbm = v2_sub(closest_b, closest_a); + + f32 dot = v2_dot(vab, vam) / v2_len_sq(vab); + + if (dot >= -validity_epsilon && dot <= 1 - validity_epsilon && (v2_wedge(vab, vam) * -winding) >= -validity_epsilon) { + /* New point is not between edge */ + valid = false; + } else if (v2_len_sq(vam) < min_unique_pt_dist_sq || v2_len_sq(vbm) < min_unique_pt_dist_sq) { + /* New point is too close to existing */ + valid = false; } } - if (!unique || epa_iterations >= max_epa_iterations) { + + if (!valid || epa_iterations >= max_epa_iterations) { res.path = 1; - if (pen_len_sq < epa_normal_epsilon_sq) { - normal = v2_norm(dir); - } else { - normal = v2_norm(pen); - } + 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) { + for (u32 i = proto_count - 1; i > closest_b_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; + proto[closest_b_index] = m; } } else if (simplex_is_closest_edge) { if (s.len == 1) { @@ -724,24 +703,11 @@ struct collider_collision_points_result collider_collision_points(struct collide res.solved = true; abort: - if (proto_count > 0) { - 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 = len; - } 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; + 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 = len; res.normal = normal; res.points[0] = points[0]; res.points[1] = points[1]; diff --git a/src/game.c b/src/game.c index 7d6f6ef1..6e2297cf 100644 --- a/src/game.c +++ b/src/game.c @@ -136,10 +136,13 @@ INTERNAL void spawn_test_entities(f32 offset) struct entity *e = entity_alloc(root); +#if 1 //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.5, -2); + //struct v2 pos = V2(0.5, -1.5); + struct v2 pos = V2(1, -1); + //struct v2 pos = V2(0.300121694803, -1.322724342346); //struct v2 pos = V2(1.0295, -1); pos = v2_add(pos, V2(0, offset)); @@ -147,7 +150,7 @@ INTERNAL void spawn_test_entities(f32 offset) //struct v2 size = V2(1, 1); struct v2 size = V2(0.5, 0.5); - //struct v2 size = V2(3.0, 0.5); + //struct v2 size = V2(0.5, 0.25); //struct v2 size = V2(1.0, 1.0); //struct v2 size = V2(1.5, 1.5); //f32 r = PI; @@ -159,6 +162,9 @@ INTERNAL void spawn_test_entities(f32 offset) f32 r = 0; struct xform xf = XFORM_TRS(.t = pos, .r = r, .s = size); +#else + struct xform xf = { .bx = {0.0382978655, -0.498547733}, .by = {0.498547733, 0.0382978655}, .og = {2.01672602, -1.06180537}, }; +#endif entity_set_xform(e, xf); @@ -196,6 +202,7 @@ INTERNAL void spawn_test_entities(f32 offset) { struct entity *e = entity_alloc(root); +#if 0 //struct v2 pos = V2(0.25, -10); //struct v2 pos = V2(0.25, -7); //struct v2 pos = V2(0.25, -5.27); @@ -215,6 +222,9 @@ INTERNAL void spawn_test_entities(f32 offset) f32 r = 0; struct xform xf = XFORM_TRS(.t = pos, .r = r, .s = size); +#else + struct xform xf = { .bx = {-0.108915359, -0.488007843}, .by = {0.488007843, -0.108915359}, .og = {1.62569654, -0.750172377}, }; +#endif entity_set_xform(e, xf); @@ -1082,11 +1092,11 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) #if 1 if (entity_has_prop(ent, ENTITY_PROP_TEST)) { //if ((true)) { -#if 0 +#if 1 ent->local_collider.points[0] = V2(0, 0); ent->local_collider.count = 1; ent->local_collider.radius = 0.5; -#elif 1 +#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; diff --git a/src/user.c b/src/user.c index 97c14ba5..72264fd0 100644 --- a/src/user.c +++ b/src/user.c @@ -1051,7 +1051,7 @@ INTERNAL void user_update(void) #if 0 b32 should_draw = false; for (u32 i = 0; i < ent->num_contacts; ++i) { - if (ent->contacts[i].starting_separation < -0.5) { + if (ent->contacts[i].starting_separation < -0.1) { should_draw = true; break; } @@ -1136,6 +1136,7 @@ INTERNAL void user_update(void) }; 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); + for (u64 i = 0; i < m.count; ++i) draw_solid_circle(G.viewport_canvas, m.points[i], 10, color, 10); } /* Draw simplex */