From 2758bdc3940440bdcf3df88c6916c45a931c08f9 Mon Sep 17 00:00:00 2001 From: jacob Date: Wed, 2 Oct 2024 14:17:48 -0500 Subject: [PATCH] friction testing --- src/config.h | 2 +- src/entity.h | 3 +- src/game.c | 80 +++++++++++++++++++++++++++++++++++++++------------- src/user.c | 9 ++++-- 4 files changed, 69 insertions(+), 25 deletions(-) diff --git a/src/config.h b/src/config.h index 2c2b3621..c45b6abe 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 4 +#define GAME_PHYSICS_SUBSTEPS 16 /* How many ticks back in time should the user blend between? * = * diff --git a/src/entity.h b/src/entity.h index 65ac76ac..96ea2249 100644 --- a/src/entity.h +++ b/src/entity.h @@ -70,7 +70,8 @@ struct contact { u32 id; /* ID generated during clipping */ f32 starting_separation; /* How far are original points along normal */ - f32 accumulated_impulse; /* Accumulated impulse along normal */ + f32 normal_impulse; /* Accumulated impulse along normal */ + f32 tangent_impulse; /* Accumulated impulse along tangent */ b32 persisted; }; diff --git a/src/game.c b/src/game.c index ae69944d..e3e87d6f 100644 --- a/src/game.c +++ b/src/game.c @@ -161,8 +161,8 @@ INTERNAL void spawn_test_entities(void) entity_enable_prop(e, ENTITY_PROP_PHYSICAL); e->mass_unscaled = 100; - //e->inertia_unscaled = 25; - e->inertia_unscaled = F32_INFINITY; + e->inertia_unscaled = 25; + //e->inertia_unscaled = F32_INFINITY; e->linear_ground_friction = 1000; e->angular_ground_friction = 100; @@ -427,8 +427,8 @@ INTERNAL void create_contact_manifolds(void) } if (contact) { /* Update existing */ - contact->accumulated_impulse = 0; - contact->persisted = true; + contact->normal_impulse = 0; + contact->tangent_impulse = 0; } else { /* Insert new */ contact = &manifold->contacts[manifold->num_contacts++]; @@ -491,7 +491,7 @@ INTERNAL void warm_start_contacts(f32 dt) (UNUSED)separation; (UNUSED)dt; - contact->accumulated_impulse = 0; + contact->normal_impulse = 0; } } @@ -514,8 +514,8 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) struct entity *e1 = entity_from_handle(store, manifold->manifold_e1); struct v2 v0 = e0->linear_velocity; - f32 w0 = e0->angular_velocity; struct v2 v1 = e1->linear_velocity; + f32 w0 = e0->angular_velocity; f32 w1 = e1->angular_velocity; u32 num_contacts = manifold->num_contacts; @@ -536,11 +536,10 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) f32 inv_i0 = 1.f / i0; f32 inv_i1 = 1.f / i1; + /* Normal impulse */ struct v2 normal = manifold->manifold_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); @@ -555,31 +554,72 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) struct v2 vcp0 = v2_sub(p0, e0_xf.og); struct v2 vcp1 = v2_sub(p1, e1_xf.og); - struct v2 vel0 = v2_add(e0->linear_velocity, v2_perp_mul(vcp0, e0->angular_velocity)); - struct v2 vel1 = v2_add(e1->linear_velocity, v2_perp_mul(vcp1, e1->angular_velocity)); + struct v2 vel0 = v2_add(v0, v2_perp_mul(vcp0, w0)); + struct v2 vel1 = v2_add(v1, v2_perp_mul(vcp1, w1)); struct v2 vrel = v2_sub(vel0, vel1); - struct v2 idk0 = v2_perp_mul(vcp0, v2_wedge(vcp0, normal) * inv_i0); - struct v2 idk1 = v2_perp_mul(vcp1, v2_wedge(vcp1, normal) * inv_i1); - f32 k = inv_m0 + inv_m1 + v2_dot(normal, v2_add(idk0, idk1)); + f32 vcp0_wedge = v2_wedge(vcp0, normal); + f32 vcp1_wedge = v2_wedge(vcp1, normal); + 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) */ f32 vn = v2_dot(vrel, normal); - f32 j = (vn + bias) / k; + f32 j = (vn + bias) * k; j *= inv_num_contacts; - f32 old_accumulated_impulse = contact->accumulated_impulse; - f32 new_accumulated_impulse = max_f32(contact->accumulated_impulse + j, 0); - f32 delta = new_accumulated_impulse - old_accumulated_impulse; - contact->accumulated_impulse = new_accumulated_impulse; + 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; struct v2 impulse = v2_mul(normal, delta); v0 = v2_sub(v0, v2_mul(impulse, inv_m0)); - w0 -= v2_wedge(vcp0, impulse) * inv_i0; v1 = v2_add(v1, v2_mul(impulse, inv_m1)); + w0 -= v2_wedge(vcp0, impulse) * inv_i0; 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 vrel = v2_sub(vel0, vel1); + + f32 vcp0_wedge = v2_wedge(vcp0, tangent); + f32 vcp1_wedge = v2_wedge(vcp1, tangent); + 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) */ + f32 vt = v2_dot(vrel, tangent); + float j = vt * k; + //j *= inv_num_contacts; + + 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); + f32 delta = new_impulse - old_impulse; + contact->tangent_impulse = new_impulse; + + struct v2 impulse = v2_mul(tangent, delta); + 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; + } +#endif + e0->linear_velocity = v0; e0->angular_velocity = w0; e1->linear_velocity = v1; @@ -1282,7 +1322,6 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) { create_contact_manifolds(); - //warm_start_contacts(dt); (UNUSED)create_contact_manifolds; (UNUSED)solve_collisions; @@ -1292,6 +1331,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) 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 */ diff --git a/src/user.c b/src/user.c index 46bd3c5c..7bed6110 100644 --- a/src/user.c +++ b/src/user.c @@ -1127,7 +1127,8 @@ INTERNAL void user_update(void) struct v2 point = v2_add(p0, v2_mul(v2_sub(p1, p0), 0.5f)); /* Draw point */ { - u32 color = contact.persisted ? RGBA_32_F(1, 1, 0, 0.50) : RGBA_32_F(1, 0, 0, 0.50); + //u32 color = contact.persisted ? RGBA_32_F(1, 1, 0, 0.50) : RGBA_32_F(1, 0, 0, 0.50); + u32 color = RGBA_32_F(1, 1, 0, 0.50); //struct v2 point = xform_mul_v2(e0_xf, contact.p0_local); //struct v2 point = contact.p0_initial_world; draw_solid_circle(G.viewport_canvas, xform_mul_v2(G.world_view, point), radius, color, 10); @@ -1150,12 +1151,14 @@ INTERNAL void user_update(void) struct string fmt = STR( "id: 0x%F\n" - "impulse: %F\n" + "impulse (n): %F\n" + "impulse (t): %F\n" "separation: %F" ); struct string text = string_format(temp.arena, fmt, FMT_HEX(contact.id), - FMT_FLOAT_P(contact.accumulated_impulse, 3), + FMT_FLOAT_P(contact.normal_impulse, 3), + FMT_FLOAT_P(contact.tangent_impulse, 3), FMT_FLOAT_P(contact.starting_separation, 3));