diff --git a/src/config.h b/src/config.h index c45b6abe..2c2b3621 100644 --- a/src/config.h +++ b/src/config.h @@ -31,7 +31,7 @@ #define GAME_FPS 50.0 #define GAME_TIMESCALE 1.0 -#define GAME_PHYSICS_SUBSTEPS 16 +#define GAME_PHYSICS_SUBSTEPS 4 /* How many ticks back in time should the user blend between? * = * diff --git a/src/game.c b/src/game.c index e3e87d6f..50aba78c 100644 --- a/src/game.c +++ b/src/game.c @@ -390,8 +390,6 @@ INTERNAL void create_contact_manifolds(void) } manifold->manifold_normal = res.normal; - - /* Delete old contacts that are no longer present */ for (u32 i = 0; i < manifold->num_contacts; ++i) { struct contact *old = &manifold->contacts[i]; @@ -427,8 +425,10 @@ INTERNAL void create_contact_manifolds(void) } if (contact) { /* Update existing */ +#if 0 contact->normal_impulse = 0; contact->tangent_impulse = 0; +#endif } else { /* Insert new */ contact = &manifold->contacts[manifold->num_contacts++]; @@ -460,7 +460,7 @@ INTERNAL void create_contact_manifolds(void) -INTERNAL void warm_start_contacts(f32 dt) +INTERNAL void warm_start_contacts(void) { struct entity_store *store = G.tick.entity_store; @@ -469,32 +469,51 @@ INTERNAL void warm_start_contacts(f32 dt) if (!entity_is_valid_and_active(manifold)) continue; if (!entity_has_prop(manifold, ENTITY_PROP_MANIFOLD)) continue; + u32 num_contacts = manifold->num_contacts; struct entity *e0 = entity_from_handle(store, manifold->manifold_e0); struct entity *e1 = entity_from_handle(store, manifold->manifold_e1); - - if (entity_is_valid_and_active(e0) && entity_is_valid_and_active(e1)) { - struct v2 normal = manifold->manifold_normal; + 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); + struct v2 v0 = e0->linear_velocity; + struct v2 v1 = e1->linear_velocity; + f32 w0 = e0->angular_velocity; + f32 w1 = e1->angular_velocity; + + f32 scale0 = math_fabs(xform_get_determinant(e0_xf)); + f32 scale1 = math_fabs(xform_get_determinant(e1_xf)); + f32 m0 = e0->mass_unscaled * scale0; + f32 m1 = e1->mass_unscaled * scale1; + f32 i0 = e0->inertia_unscaled * scale0; + f32 i1 = e1->inertia_unscaled * scale1; + f32 inv_m0 = 1.f / m0; + f32 inv_m1 = 1.f / m1; + f32 inv_i0 = 1.f / i0; + f32 inv_i1 = 1.f / i1; /* Warm start */ + struct v2 normal = manifold->manifold_normal; + struct v2 tangent = v2_perp(normal); for (u32 i = 0; i < manifold->num_contacts; ++i) { struct contact *contact = &manifold->contacts[i]; - struct v2 p0 = xform_mul_v2(e0_xf, contact->point_local_e0); struct v2 p1 = xform_mul_v2(e1_xf, contact->point_local_e1); - f32 separation = v2_dot(v2_sub(p1, p0), normal) + contact->starting_separation; + struct v2 vcp0 = v2_sub(p0, e0_xf.og); + struct v2 vcp1 = v2_sub(p1, e1_xf.og); - (UNUSED)p0; - (UNUSED)p1; - (UNUSED)separation; - (UNUSED)dt; - - contact->normal_impulse = 0; + struct v2 impulse = v2_add(v2_mul(normal, contact->normal_impulse), v2_mul(tangent, contact->tangent_impulse)); + v0 = v2_sub(v0, v2_mul(impulse, inv_m0)); + v1 = v2_add(v1, v2_mul(impulse, inv_m1)); + w0 -= v2_wedge(vcp0, impulse) * inv_i0; + w1 += v2_wedge(vcp1, impulse) * inv_i1; } - } + e0->linear_velocity = v0; + e0->angular_velocity = w0; + e1->linear_velocity = v1; + e1->angular_velocity = w1; + } } } @@ -515,8 +534,13 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) struct v2 v0 = e0->linear_velocity; struct v2 v1 = e1->linear_velocity; + struct v2 v0_start = v0; + struct v2 v1_start = v1; + f32 w0 = e0->angular_velocity; f32 w1 = e1->angular_velocity; + f32 w0_start = w0; + f32 w1_start = w1; u32 num_contacts = manifold->num_contacts; f32 inv_num_contacts = 1.f / num_contacts; @@ -542,20 +566,19 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) struct contact *contact = &manifold->contacts[contact_index]; struct v2 p0 = xform_mul_v2(e0_xf, contact->point_local_e0); struct v2 p1 = xform_mul_v2(e1_xf, contact->point_local_e1); + struct v2 vcp0 = v2_sub(p0, e0_xf.og); + struct v2 vcp1 = v2_sub(p1, e1_xf.og); f32 bias = 0; if (apply_bias) { - const f32 bias_factor = 0.2f; - const f32 bias_slop = 0.005f; + const f32 bias_factor = 0.1f; + const f32 bias_slop = 0.02f; f32 penetration = v2_dot(v2_sub(p0, p1), normal) - contact->starting_separation; bias = (bias_factor / dt) * max_f32(penetration - bias_slop, 0); } - struct v2 vcp0 = v2_sub(p0, e0_xf.og); - struct v2 vcp1 = v2_sub(p1, e1_xf.og); - - struct v2 vel0 = v2_add(v0, v2_perp_mul(vcp0, w0)); - struct v2 vel1 = v2_add(v1, v2_perp_mul(vcp1, w1)); + struct v2 vel0 = v2_add(v0_start, v2_perp_mul(vcp0, w0_start)); + struct v2 vel1 = v2_add(v1_start, v2_perp_mul(vcp1, w1_start)); struct v2 vrel = v2_sub(vel0, vel1); f32 vcp0_wedge = v2_wedge(vcp0, normal); @@ -566,7 +589,7 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) /* (to be applied along n) */ f32 vn = v2_dot(vrel, normal); f32 j = (vn + bias) * k; - j *= inv_num_contacts; + //j *= inv_num_contacts; f32 old_impulse = contact->normal_impulse; f32 new_impulse = max_f32(contact->normal_impulse + j, 0); @@ -580,19 +603,17 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) w1 += v2_wedge(vcp1, impulse) * inv_i1; } -#if 1 /* Tangent impulse */ struct v2 tangent = v2_perp(normal); for (u32 contact_index = 0; contact_index < num_contacts; ++contact_index) { struct contact *contact = &manifold->contacts[contact_index]; struct v2 p0 = xform_mul_v2(e0_xf, contact->point_local_e0); struct v2 p1 = xform_mul_v2(e1_xf, contact->point_local_e1); - struct v2 vcp0 = v2_sub(p0, e0_xf.og); struct v2 vcp1 = v2_sub(p1, e1_xf.og); - struct v2 vel0 = v2_add(v0, v2_perp_mul(vcp0, w0)); - struct v2 vel1 = v2_add(v1, v2_perp_mul(vcp1, w1)); + struct v2 vel0 = v2_add(v0_start, v2_perp_mul(vcp0, w0_start)); + struct v2 vel1 = v2_add(v1_start, v2_perp_mul(vcp1, w1_start)); struct v2 vrel = v2_sub(vel0, vel1); f32 vcp0_wedge = v2_wedge(vcp0, tangent); @@ -600,12 +621,14 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) f32 k = (inv_m0 + inv_m1) + (inv_i0 * vcp0_wedge * vcp0_wedge) + (inv_i1 * vcp1_wedge * vcp1_wedge); k = k > 0.0f ? 1.0f / k : 0.0f; - /* (to be applied along n) */ + /* (to be applied along t) */ f32 vt = v2_dot(vrel, tangent); - float j = vt * k; - //j *= inv_num_contacts; + f32 j = vt * k; + j *= inv_num_contacts; - f32 friction = F32_INFINITY; + f32 friction = 0.6f; + //f32 friction = 1.0f; + //f32 friction = 999999.f; 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); @@ -618,7 +641,6 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) w0 -= v2_wedge(vcp0, impulse) * inv_i0; w1 += v2_wedge(vcp1, impulse) * inv_i1; } -#endif e0->linear_velocity = v0; e0->angular_velocity = w0; @@ -1322,16 +1344,17 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) { create_contact_manifolds(); + warm_start_contacts(); (UNUSED)create_contact_manifolds; (UNUSED)solve_collisions; (UNUSED)integrate_positions; (UNUSED)warm_start_contacts; + f32 substep_dt = dt / GAME_PHYSICS_SUBSTEPS; for (u32 i = 0; i < GAME_PHYSICS_SUBSTEPS; ++i) { #if 1 - //warm_start_contacts(dt); solve_collisions(substep_dt, true); integrate_positions(substep_dt); solve_collisions(substep_dt, false); /* Relaxation */