jacob/power_play
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add false position method to toi root finding

Browse Source
This commit is contained in:
jacob 2025-01-06 16:33:14 -06:00
parent 967a408972
commit 6268e012f6
1 changed files with 53 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

81
src/game.c
View File

@ -969,9 +969,6 @@ INTERNAL void solve_contacts(f32 dt, b32 apply_bias)
/* ========================== * /* ========================== *
* TESTING MOTOR JOINT * 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, INTERNAL f32 toi(struct collider_shape *c0, struct collider_shape *c1,
struct xform xf0_t0, struct xform xf1_t0, struct xform xf0_t0, struct xform xf1_t0,
struct xform xf0_t1, struct xform xf1_t1, 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 */ /* Find direction p0 -> p1 at t=0 */
struct v2 dir; struct v2 dir;
struct v2 dir_neg; 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 */ /* Shapes are penetrating at t=0 */
return 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); 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 p0 = collider_support_point(c0, xf0_t1, dir);
struct v2 p1 = collider_support_point(c1, xf1_t1, dir_neg); struct v2 p1 = collider_support_point(c1, xf1_t1, dir_neg);
sep = v2_dot(dir, v2_sub(p1, p0)); t1_sep = v2_dot(dir, v2_sub(p1, p0));
if (sep > tolerance) { if (t1_sep > 0) {
/* Shapes are not penetrating at t=1 */ /* Shapes are not penetrating at t=1 */
return 1; return 1;
} }
} }
/* Bisect until distance is within tolerance */ u32 iteration = 0;
/* TODO: Implement false position method as well? (should speed up more linear cases) */ while (math_fabs(t_sep) > tolerance) {
f32 t0 = 0.0; if (iteration >= max_iterations) {
f32 t1 = 1.0; /* Not necessarily an error, but this should rarely occur and
f32 t = 0.5f; * should be investigated if it does. If determined not an error then
while (math_fabs(sep) > tolerance) { * 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 xf0 = xform_lerp(xf0_t0, xf0_t1, t);
struct xform xf1 = xform_lerp(xf1_t0, xf1_t1, t); struct xform xf1 = xform_lerp(xf1_t0, xf1_t1, t);
struct v2 p0 = collider_support_point(c0, xf0, dir); struct v2 p0 = collider_support_point(c0, xf0, dir);
struct v2 p1 = collider_support_point(c1, xf1, dir_neg); struct v2 p1 = collider_support_point(c1, xf1, dir_neg);
t_sep = v2_dot(dir, v2_sub(p1, p0));
sep = v2_dot(dir, v2_sub(p1, p0));
/* Update bracket */ /* Update bracket */
if (sep > 0) { if (t_sep > 0) {
t0 = t; t0 = t;
t0_sep = t_sep;
} else { } else {
t1 = t; t1 = t;
t1_sep = t_sep;
} }
t = (t1 + t0) / 2.0;
++iteration;
}
if (iteration > 1) {
DEBUGBREAKABLE;
} }
return t; return t;
} }
INTERNAL f32 determine_earliest_toi(f32 dt, f32 tolerance) INTERNAL f32 determine_earliest_toi(f32 dt, f32 tolerance, u32 max_iterations)
{ {
__prof; __prof;
f32 smallest_t = 1; 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); 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) { if (t != 0 && t < smallest_t) {
smallest_t = 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_src_dir = rel_dir;
//bullet->bullet_impulse = 0.1f; //bullet->bullet_impulse = 0.1f;
//bullet->bullet_impulse = 0.25f; //bullet->bullet_impulse = 0.25f;
//bullet->bullet_impulse = 5.f; bullet->bullet_impulse = 5.f;
bullet->bullet_impulse = 100000.f;
bullet->mass_unscaled = 0.04f; bullet->mass_unscaled = 0.04f;
bullet->inertia_unscaled = 0.00001f; bullet->inertia_unscaled = 0.00001f;
bullet->sprite_collider_slice = STR("shape"); bullet->sprite_collider_slice = STR("shape");
@ -2227,13 +2252,13 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
#if 1 #if 1
{ {
f32 remaining_dt = dt; f32 remaining_dt = dt;
integrate_velocities_from_forces(dt);
while (remaining_dt > 0) { while (remaining_dt > 0) {
f32 min_toi = 0.000001f; const f32 min_toi = 0.000001f;
f32 tolerance = 0.001f; const f32 tolerance = 0.00001f;
f32 earliest_toi = max_f32(determine_earliest_toi(remaining_dt, tolerance), min_toi); 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; f32 step_dt = remaining_dt * earliest_toi;
integrate_velocities_from_forces(step_dt);
create_contacts(); create_contacts();
create_mouse_joints(game_cmds); create_mouse_joints(game_cmds);