add false position method to toi root finding
This commit is contained in:
parent
967a408972
commit
6268e012f6
81
src/game.c
81
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);
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user