diff --git a/src/config.h b/src/config.h index 3414002d..f44215fe 100644 --- a/src/config.h +++ b/src/config.h @@ -43,7 +43,7 @@ * E.g: At 1.5, the user thread will render 75ms back in time (if game thread runs at 50FPS) */ #define USER_INTERP_OFFSET_TICK_RATIO 1.1 -#define USER_INTERP_ENABLED 1 +#define USER_INTERP_ENABLED 0 /* ========================== * * Settings diff --git a/src/entity.c b/src/entity.c index 49e3da07..be7cd695 100644 --- a/src/entity.c +++ b/src/entity.c @@ -279,9 +279,6 @@ struct xform entity_get_local_xform(struct entity *ent) void entity_set_xform(struct entity *ent, struct xform xf) { - /* TODO: Remove this (debugging) */ - if (v2_len_sq(xf.bx) == 0 || v2_len_sq(xf.by) == 0) DEBUGBREAK; - if (!xform_eq(xf, ent->cached_global_xform)) { struct entity_store *store = entity_get_store(ent); /* Update local xform */ diff --git a/src/entity.h b/src/entity.h index 519d6b98..6f465ade 100644 --- a/src/entity.h +++ b/src/entity.h @@ -120,6 +120,7 @@ struct entity { struct gjk_prototype prototype; struct v2 pendir; b32 solved; + i32 path; b32 test_torque_applied; diff --git a/src/game.c b/src/game.c index bdf4ad2f..cfa01788 100644 --- a/src/game.c +++ b/src/game.c @@ -340,7 +340,6 @@ INTERNAL void create_contact_manifolds(void) } } - /* Ensure manifold hasn't already been computed this iteration */ if (manifold) { if (manifold->last_manifold_iteration == manifold_iteration) { @@ -376,6 +375,7 @@ INTERNAL void create_contact_manifolds(void) manifold->prototype = res.prototype; manifold->simplex = res.simplex; manifold->solved = res.solved; + manifold->path = res.path; } if (res.num_points > 0) { @@ -471,29 +471,6 @@ INTERNAL void create_contact_manifolds(void) struct v2 vcp0 = v2_sub(point, e0_xf.og); struct v2 vcp1 = v2_sub(point, e1_xf.og); - - - /* TODO: Remove this (debugging) */ - if (F32_IS_NAN(scale0) || - F32_IS_NAN(scale1) || - F32_IS_NAN(m0) || - F32_IS_NAN(m1) || - F32_IS_NAN(i0) || - F32_IS_NAN(i1) || - F32_IS_NAN(inv_m0) || - F32_IS_NAN(inv_m1) || - F32_IS_NAN(inv_i0) || - F32_IS_NAN(inv_i1)) { - DEBUGBREAK; - } - - - - - - - - /* Normal mass */ { f32 vcp0_wedge = v2_wedge(vcp0, normal); @@ -563,7 +540,6 @@ INTERNAL void warm_start_contacts(void) f32 w1 = e1->angular_velocity; /* Warm start */ - f32 inv_num_contacts = 1.f / num_contacts; struct v2 normal = manifold->manifold_normal; struct v2 tangent = v2_perp(normal); for (u32 i = 0; i < num_contacts; ++i) { @@ -574,17 +550,10 @@ INTERNAL void warm_start_contacts(void) struct v2 vcp1 = v2_sub(p1, e1_xf.og); struct v2 impulse = v2_add(v2_mul(normal, contact->normal_impulse), v2_mul(tangent, contact->tangent_impulse)); - - (UNUSED)inv_num_contacts; - //impulse = v2_mul(impulse, inv_num_contacts); - v0 = v2_sub(v0, v2_mul(impulse, contact->inv_m0)); v1 = v2_add(v1, v2_mul(impulse, contact->inv_m1)); w0 -= v2_wedge(vcp0, impulse) * contact->inv_i0; w1 += v2_wedge(vcp1, impulse) * contact->inv_i1; - - /* TODO: Remove this (debugging) */ - if (F32_IS_NAN(v0.x) || F32_IS_NAN(v0.y) || F32_IS_NAN(v1.x) || F32_IS_NAN(v1.y) || F32_IS_NAN(w0) || F32_IS_NAN(w1)) DEBUGBREAK; } e0->linear_velocity = v0; @@ -648,8 +617,6 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) f32 w1 = e1->angular_velocity; u32 num_contacts = manifold->num_contacts; - f32 inv_num_contacts = 1.f / num_contacts; - (UNUSED)inv_num_contacts; if (num_contacts > 0 && entity_is_valid_and_active(e0) && entity_is_valid_and_active(e1)) { struct xform e0_xf = entity_get_xform(e0); struct xform e1_xf = entity_get_xform(e1); @@ -674,14 +641,15 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) velocity_bias = separation / dt; } else if (apply_bias) { /* Soft constraint */ - f32 contact_pushout_velocity = 3.0f; f32 contact_damping_ratio = 10.0f; f32 contact_hertz = (GAME_FPS * GAME_PHYSICS_SUBSTEPS) / 8.f; struct soft_result softness = make_soft(contact_hertz, contact_damping_ratio, dt); + f32 contact_pushout_velocity = 3.0f; velocity_bias = max_f32(softness.bias_rate * separation, -contact_pushout_velocity); + mass_scale = softness.mass_scale; impulse_scale = softness.impulse_scale; } @@ -695,44 +663,17 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) /* (to be applied along n) */ f32 vn = v2_dot(vrel, normal); f32 j = ((k * mass_scale) * (vn - velocity_bias)) - (contact->normal_impulse * impulse_scale); - //j *= inv_num_contacts; f32 old_impulse = contact->normal_impulse; f32 new_impulse = max_f32(contact->normal_impulse + j, 0); f32 delta = new_impulse - old_impulse; contact->normal_impulse = new_impulse; - - /* TODO: Remove this (debugging) */ - //if (math_fabs(delta) > 10) DEBUGBREAK; - - - - - struct v2 impulse = v2_mul(normal, delta); v0 = v2_sub(v0, v2_mul(impulse, contact->inv_m0)); v1 = v2_add(v1, v2_mul(impulse, contact->inv_m1)); w0 -= v2_wedge(vcp0, impulse) * contact->inv_i0; w1 += v2_wedge(vcp1, impulse) * contact->inv_i1; - - -#if 1 - v0 = v2_clamp_len(v0, GAME_MAX_LINEAR_VELOCITY); - v1 = v2_clamp_len(v1, GAME_MAX_LINEAR_VELOCITY); - w0 = clamp_f32(w0, -GAME_MAX_ANGULAR_VELOCITY, GAME_MAX_ANGULAR_VELOCITY); - w1 = clamp_f32(w1, -GAME_MAX_ANGULAR_VELOCITY, GAME_MAX_ANGULAR_VELOCITY); -#endif - - - - - - /* TODO: Remove this (debugging) */ - if ((math_fabs(v0.x) > 10000000) || (math_fabs(v0.y) > 10000000) || (math_fabs(v1.x) > 10000000) || (math_fabs(v1.y) > 10000000) || (math_fabs(w0) > 10000000) || (math_fabs(w1) > 10000000)) DEBUGBREAK; - - /* TODO: Remove this (debugging) */ - if (F32_IS_NAN(v0.x) || F32_IS_NAN(v0.y) || F32_IS_NAN(v1.x) || F32_IS_NAN(v1.y) || F32_IS_NAN(w0) || F32_IS_NAN(w1)) DEBUGBREAK; } /* Tangent impulse */ @@ -753,11 +694,10 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) /* (to be applied along t) */ f32 vt = v2_dot(vrel, tangent); f32 j = vt * k; - //j *= inv_num_contacts; //f32 friction = 0.6f; - f32 friction = 1.0f; - //f32 friction = F32_INFINITY; + //f32 friction = 1.0f; + f32 friction = F32_INFINITY; f32 max_friction = friction * contact->normal_impulse; f32 old_impulse = contact->tangent_impulse; f32 new_impulse = clamp_f32(old_impulse + j, -max_friction, max_friction); @@ -769,14 +709,8 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) v1 = v2_add(v1, v2_mul(impulse, contact->inv_m1)); w0 -= v2_wedge(vcp0, impulse) * contact->inv_i0; w1 += v2_wedge(vcp1, impulse) * contact->inv_i1; - - /* TODO: Remove this (debugging) */ - if (F32_IS_NAN(v0.x) || F32_IS_NAN(v0.y) || F32_IS_NAN(v1.x) || F32_IS_NAN(v1.y) || F32_IS_NAN(w0) || F32_IS_NAN(w1)) DEBUGBREAK; } - /* TODO: Remove this (debugging) */ - if (F32_IS_NAN(v0.x) || F32_IS_NAN(v0.y) || F32_IS_NAN(v1.x) || F32_IS_NAN(v1.y) || F32_IS_NAN(w0) || F32_IS_NAN(w1)) DEBUGBREAK; - e0->linear_velocity = v0; e0->angular_velocity = w0; e1->linear_velocity = v1; @@ -816,9 +750,6 @@ INTERNAL void integrate_velocities_from_forces(f32 dt) ent->linear_velocity = v2_clamp_len(ent->linear_velocity, GAME_MAX_LINEAR_VELOCITY); ent->angular_velocity = clamp_f32(ent->angular_velocity, -GAME_MAX_ANGULAR_VELOCITY, GAME_MAX_ANGULAR_VELOCITY); - /* TODO: Remove this (debugging) */ - if (F32_IS_NAN(ent->linear_velocity.x) || F32_IS_NAN(ent->linear_velocity.y) || F32_IS_NAN(ent->angular_velocity)) DEBUGBREAK; - /* Reset forces */ ent->force = V2(0, 0); ent->torque = 0; @@ -844,11 +775,6 @@ INTERNAL void integrate_positions_from_velocities(f32 dt) xf.og = v2_add(xf.og, tick_linear_velocity); xf = xform_rotated(xf, tick_angular_velocity); entity_set_xform(ent, xf); - - /* TODO: Remove this (debugging) */ - if (F32_IS_NAN(xf.bx.x) || F32_IS_NAN(xf.bx.y) || F32_IS_NAN(xf.by.x) || F32_IS_NAN(xf.by.y) || F32_IS_NAN(xf.og.x) || F32_IS_NAN(xf.og.y)) { - DEBUGBREAK; - } } } @@ -953,9 +879,9 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) case GAME_CMD_KIND_SPAWN_TEST: { logf_info("Spawning (test)"); -#if 0 +#if 1 for (u32 i = 0; i < 50; ++i) { - spawn_test_entities(-i); + spawn_test_entities(-(f32)i); } #else spawn_test_entities(0); diff --git a/src/gjk.c b/src/gjk.c index 6879ee98..8aeb1ad3 100644 --- a/src/gjk.c +++ b/src/gjk.c @@ -596,7 +596,7 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru struct gjk_contact_points_result res = ZI; /* TODO: Parameterize */ - const f32 tolerance = 0.001f; + const f32 tolerance = 0.005f; const f32 min_unique_pt_dist_sq = 0.001f * 0.001f; b32 colliding = false; @@ -640,7 +640,10 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru 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) break; + 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; @@ -653,13 +656,13 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru 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) || + 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) + 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; @@ -740,17 +743,13 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru { /* Unknown region (should be impossible) */ ASSERT(false); + res.path = -1; done = true; } break; } } } - /* TODO: Remove this (debugging) */ - if (F32_IS_NAN(s.a.x) || F32_IS_NAN(s.a.y) || F32_IS_NAN(s.b.x) || F32_IS_NAN(s.b.y) || F32_IS_NAN(s.c.x) || F32_IS_NAN(s.c.y)) { - DEBUGBREAK; - } - if (colliding) { /* ========================== * * Epa (to find collision normal from inside shape) @@ -802,8 +801,8 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru /* Find new point in dir */ DBGSTEP; { - /* Next point is in direction of line normal pointing outwards from shape */ - /* TODO: If winding order is guaranteed then this can become v2_perp_left/right */ + /* 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 */ @@ -818,36 +817,19 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru { b32 unique = true; for (u32 i = 0; i < proto_count; ++i) { - struct v2 p = proto[i]; - if (v2_len_sq(v2_sub(p, m)) < min_unique_pt_dist_sq) { + 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) { -#if 0 - f32 len = v2_len(normal); - if (len < min_pen_len) { - colliding = false; - } else { - normal = v2_mul(normal, 1.f / len); - } - break; -#elif 0 - /* Re-do normal calculation in high precision (for accuracy with small separation values) */ - struct v2_64 ps = V2_64_FROM_V2(proto[pen_ps_index]); - struct v2_64 pe = V2_64_FROM_V2(proto[pen_pe_index]); - struct v2_64 vse = v2_sub64(pe, ps); - struct v2_64 vso = v2_neg64(ps); - struct v2_64 vsd = v2_mul64(vse, (v2_dot64(vso, vse) / v2_len_sq64(vse))); - struct v2_64 normal64 = v2_add64(ps, vsd); - normal64 = v2_norm64(normal64); - normal = V2(normal64.x, normal64.y); - break; -#else + res.path = 1; normal = v2_norm(dir); break; -#endif } } @@ -862,23 +844,18 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru } proto[pen_pe_index] = m; } - } else if (simplex_is_closest_edge) { - if (s.len == 1) { - /* TODO? */ - } else { - 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)) { -#if 1 - normal = v2_norm(v2_perp_towards_dir(vab, dir)); -#else - normal = v2_norm(dir); -#endif - colliding = true; - } + } 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; } } @@ -1043,24 +1020,6 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru f32 a_sep = v2_dot(va0a1_clipped, normal); f32 b_sep = v2_dot(vb0b1_clipped, normal); - -#if 0 - if (a_sep < 0) { - struct gjk_contact_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 < 0) { - struct gjk_contact_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)); - } -#else - /* TODO: Remove this (debugging) */ - if (a_sep < -3 || b_sep < -3) DEBUGBREAK; - if (a_sep < tolerance) { struct gjk_contact_point *point = &points[num_points++]; point->id = id_a0 | (id_a1 << 4); @@ -1073,16 +1032,11 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru point->separation = b_sep; point->point = v2_add(b0_clipped, v2_mul(vb0b1_clipped, 0.5f)); } -#endif } } res.solved = true; abort: - - /* TODO: Remove this (testing) */ - //num_points = max_u32(num_points, 1); - 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]; diff --git a/src/gjk.h b/src/gjk.h index dde7105f..37966dd9 100644 --- a/src/gjk.h +++ b/src/gjk.h @@ -36,6 +36,7 @@ struct gjk_contact_points_result { /* For debugging */ b32 solved; + i32 path; struct gjk_simplex simplex; struct gjk_prototype prototype; }; diff --git a/src/math.h b/src/math.h index 39ed998c..fe47162b 100644 --- a/src/math.h +++ b/src/math.h @@ -898,15 +898,6 @@ INLINE struct xform xform_rotated(struct xform xf, f32 angle) res.bx.y = xf.bx.y * c + xf.by.y * s; res.by.x = xf.bx.x * -s + xf.by.x * c; res.by.y = xf.bx.y * -s + xf.by.y * c; - - - - /* TODO: Remove this (debugging) */ - if (v2_len_sq(res.bx) == 0 || v2_len_sq(res.by) == 0) DEBUGBREAK; - - - - return res; } diff --git a/src/user.c b/src/user.c index 2f08a530..11979f39 100644 --- a/src/user.c +++ b/src/user.c @@ -1152,12 +1152,18 @@ INTERNAL void user_update(void) f32 offset_px = -20; struct string fmt = STR( + "path: %F\n" + "e0 index: %F\n" + "e1 index: %F\n" "id: 0x%F\n" "impulse (n): %F\n" "impulse (t): %F\n" "separation: %F" ); struct string text = string_format(temp.arena, fmt, + FMT_SINT(ent->path), + FMT_UINT(e0->handle.idx), + FMT_UINT(e1->handle.idx), FMT_HEX(contact.id), FMT_FLOAT_P(contact.normal_impulse, 3), FMT_FLOAT_P(contact.tangent_impulse, 3),