diff --git a/src/game.c b/src/game.c index 11f493a5..3432d3cb 100644 --- a/src/game.c +++ b/src/game.c @@ -969,9 +969,6 @@ INTERNAL void solve_contacts(f32 dt, b32 apply_bias) - - - /* ========================== * * TESTING MOTOR JOINT * ========================== */ @@ -1476,8 +1473,15 @@ INTERNAL void integrate_positions_from_velocities(f32 dt) INTERNAL f32 toi(struct collider_shape *c0, struct collider_shape *c1, struct xform xf0_t0, struct xform xf1_t0, struct xform xf0_t1, struct xform xf1_t1, - f32 tolerance) + f32 tolerance, u32 max_iterations) { + f32 t0 = 0; + f32 t1 = 1; + f32 t0_sep = 0; + f32 t1_sep = 0; + f32 t = 0; + f32 t_sep = F32_INFINITY; + /* Find direction p0 -> p1 at t=0 */ struct v2 dir; struct v2 dir_neg; @@ -1487,49 +1491,71 @@ INTERNAL f32 toi(struct collider_shape *c0, struct collider_shape *c1, /* Shapes are penetrating at t=0 */ return 0; } - dir = v2_norm(v2_sub(closest_points_res.p1, closest_points_res.p0)); + dir = v2_sub(closest_points_res.p1, closest_points_res.p0); + t0_sep = v2_len(dir); + dir = v2_div(dir, t0_sep); /* Normalize */ dir_neg = v2_neg(dir); } - /* Safety check that shapes penetrate at t=1 */ - f32 sep; { struct v2 p0 = collider_support_point(c0, xf0_t1, dir); struct v2 p1 = collider_support_point(c1, xf1_t1, dir_neg); - sep = v2_dot(dir, v2_sub(p1, p0)); - if (sep > tolerance) { + t1_sep = v2_dot(dir, v2_sub(p1, p0)); + if (t1_sep > 0) { /* Shapes are not penetrating at t=1 */ return 1; } } - /* Bisect until distance is within tolerance */ - /* TODO: Implement false position method as well? (should speed up more linear cases) */ - f32 t0 = 0.0; - f32 t1 = 1.0; - f32 t = 0.5f; - while (math_fabs(sep) > tolerance) { + u32 iteration = 0; + while (math_fabs(t_sep) > tolerance) { + if (iteration >= max_iterations) { + /* Not necessarily an error, but this should rarely occur and + * should be investigated if it does. If determined not an error then + * this assert should be removed or max_iterations / tolerance + * adjusted at call site. */ + ASSERT(false); + break; + } + + /* Use mix of bisection & false position method to find root + * (as described in https://box2d.org/files/ErinCatto_ContinuousCollision_GDC2013.pdf) */ + if (iteration & 1) { + /* Bisect */ + t = (t1 + t0) / 2.0; + } else { + /* False position (fastest for linear case) */ + f32 m = (t1_sep - t0_sep) / (t1 - t0); + t = (-t1_sep / m) + t1; + } + struct xform xf0 = xform_lerp(xf0_t0, xf0_t1, t); struct xform xf1 = xform_lerp(xf1_t0, xf1_t1, t); struct v2 p0 = collider_support_point(c0, xf0, dir); struct v2 p1 = collider_support_point(c1, xf1, dir_neg); - - sep = v2_dot(dir, v2_sub(p1, p0)); + t_sep = v2_dot(dir, v2_sub(p1, p0)); /* Update bracket */ - if (sep > 0) { + if (t_sep > 0) { t0 = t; + t0_sep = t_sep; } else { t1 = t; + t1_sep = t_sep; } - t = (t1 + t0) / 2.0; + + ++iteration; + } + + if (iteration > 1) { + DEBUGBREAKABLE; } return t; } -INTERNAL f32 determine_earliest_toi(f32 dt, f32 tolerance) +INTERNAL f32 determine_earliest_toi(f32 dt, f32 tolerance, u32 max_iterations) { __prof; f32 smallest_t = 1; @@ -1579,7 +1605,7 @@ INTERNAL f32 determine_earliest_toi(f32 dt, f32 tolerance) e1_xf_t1 = xform_basis_rotated_world(e1_xf_t1, tick_angular_velocity); } - f32 t = toi(&e0_collider, &e1_collider, e0_xf_t0, e1_xf_t0, e0_xf_t1, e1_xf_t1, tolerance); + f32 t = toi(&e0_collider, &e1_collider, e0_xf_t0, e1_xf_t0, e0_xf_t1, e1_xf_t1, tolerance, max_iterations); if (t != 0 && t < smallest_t) { smallest_t = t; } @@ -2029,8 +2055,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) bullet->bullet_src_dir = rel_dir; //bullet->bullet_impulse = 0.1f; //bullet->bullet_impulse = 0.25f; - //bullet->bullet_impulse = 5.f; - bullet->bullet_impulse = 100000.f; + bullet->bullet_impulse = 5.f; bullet->mass_unscaled = 0.04f; bullet->inertia_unscaled = 0.00001f; bullet->sprite_collider_slice = STR("shape"); @@ -2227,13 +2252,13 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) #if 1 { f32 remaining_dt = dt; + integrate_velocities_from_forces(dt); while (remaining_dt > 0) { - f32 min_toi = 0.000001f; - f32 tolerance = 0.001f; - f32 earliest_toi = max_f32(determine_earliest_toi(remaining_dt, tolerance), min_toi); + const f32 min_toi = 0.000001f; + const f32 tolerance = 0.00001f; + const u32 max_iterations = 128; + f32 earliest_toi = max_f32(determine_earliest_toi(remaining_dt, tolerance, max_iterations), min_toi); f32 step_dt = remaining_dt * earliest_toi; - - integrate_velocities_from_forces(step_dt); create_contacts(); create_mouse_joints(game_cmds);