From cf911f27b7b14eba2c91800930e219ba4924a202 Mon Sep 17 00:00:00 2001 From: jacob Date: Fri, 4 Oct 2024 09:18:09 -0500 Subject: [PATCH] soft contact testing --- src/game.c | 112 +++++++++++++++++++++++++++++++++++++++++------------ src/gjk.c | 21 +++++++--- src/user.c | 2 +- 3 files changed, 103 insertions(+), 32 deletions(-) diff --git a/src/game.c b/src/game.c index 50aba78c..15a7c857 100644 --- a/src/game.c +++ b/src/game.c @@ -126,16 +126,19 @@ INTERNAL void spawn_test_entities(void) /* Player */ struct entity *player_ent; { - struct v2 pos = V2(0.25, -10); + //struct v2 pos = V2(0.25, -10); + struct v2 pos = V2(0.25, -7); + //struct v2 pos = V2(0.25, -5.27); //struct v2 pos = V2(0.25, -2); //struct v2 pos = V2(1.1230469346046448864129274625156, -1); /* Touching right side of box */ //struct v2 pos = V2(1.1230469346046448864129274625156 - 0.0001, -1); /* Touching right side of box */ //struct v2 pos = V2(0.374142020941, -0.246118023992); /* Touching glitch spot */ //struct v2 size = V2(1, 1); struct v2 size = V2(0.5, 0.5); - f32 r = PI; - //f32 r = PI / 4; + //f32 r = PI; + f32 r = PI / 4; //f32 r = PI / 3; + //f32 r = 0.05; //f32 r = PI / 2; //f32 r = 0; f32 skew = 0; @@ -161,8 +164,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 = F32_INFINITY; + //e->inertia_unscaled = 25; e->linear_ground_friction = 1000; e->angular_ground_friction = 100; @@ -425,7 +428,7 @@ INTERNAL void create_contact_manifolds(void) } if (contact) { /* Update existing */ -#if 0 +#if 1 contact->normal_impulse = 0; contact->tangent_impulse = 0; #endif @@ -443,7 +446,7 @@ INTERNAL void create_contact_manifolds(void) } else if (manifold) { /* No longer colliding, delete manifold */ -#if 1 +#if 0 manifold->num_contacts = 0; entity_enable_prop(manifold, ENTITY_PROP_RELEASE); #else @@ -520,6 +523,37 @@ INTERNAL void warm_start_contacts(void) + + +struct soft_result { + f32 bias_rate; + f32 mass_scale; + f32 impulse_scale; +}; +INTERNAL struct soft_result make_soft(f32 hertz, f32 zeta, f32 h) +{ + if (hertz == 0.0f) { + return (struct soft_result) { .mass_scale = 1.0f }; + } else { + f32 omega = 2.0f * PI * hertz; + f32 a1 = 2.0f * zeta + h * omega; + f32 a2 = h * omega * a1; + f32 a3 = 1.0f / (1.0f + a2); + return (struct soft_result) { + .bias_rate = omega / a1, + .mass_scale = a2 * a3, + .impulse_scale = 1.0f / (1.0f + a2) + }; + } +} + + + + + + + + INTERNAL void solve_collisions(f32 dt, b32 apply_bias) { #if 1 @@ -534,13 +568,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; + //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; + //f32 w0_start = w0; + //f32 w1_start = w1; u32 num_contacts = manifold->num_contacts; f32 inv_num_contacts = 1.f / num_contacts; @@ -569,16 +603,42 @@ 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); - f32 bias = 0; - if (apply_bias) { - 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); + f32 penetration = -(v2_dot(v2_sub(p1, p0), normal) + contact->starting_separation); + + f32 velocity_bias = 0.0f; + f32 mass_scale = 1.0f; + f32 impulse_scale = 0.0f; + + + if (penetration <= 0.0f) { + velocity_bias = penetration / dt; + } else if (apply_bias) { +#if 0 + /* Baumgarte Stabilization */ + const f32 bias_factor = 0.2f; + const f32 bias_slop = 0.005f; + velocity_bias = (bias_factor / dt) * max_f32(-separation - bias_slop, 0); +#else + /* Soft constraint */ + f32 contact_pushout_velocity = 3.0f; + + + + f32 contact_damping_ratio = 10.0f; + + f32 contact_hertz = 25.0f; + contact_hertz = min_f32(contact_hertz, 0.25f / dt); + + struct soft_result softness = make_soft(contact_hertz, contact_damping_ratio, dt); + + velocity_bias = max_f32(softness.bias_rate * penetration, -contact_pushout_velocity); + mass_scale = softness.mass_scale; + impulse_scale = softness.impulse_scale; +#endif } - 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 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, normal); @@ -588,7 +648,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; + f32 j = ((k * mass_scale) * (vn + velocity_bias)) - (contact->normal_impulse * impulse_scale) ; //j *= inv_num_contacts; f32 old_impulse = contact->normal_impulse; @@ -612,8 +672,8 @@ 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(v0_start, v2_perp_mul(vcp0, w0_start)); - struct v2 vel1 = v2_add(v1_start, v2_perp_mul(vcp1, w1_start)); + 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); @@ -624,11 +684,11 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) /* (to be applied along t) */ f32 vt = v2_dot(vrel, tangent); f32 j = vt * k; - j *= inv_num_contacts; + //j *= inv_num_contacts; - f32 friction = 0.6f; + //f32 friction = 0.6f; //f32 friction = 1.0f; - //f32 friction = 999999.f; + 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); @@ -649,6 +709,7 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) } } #else + (UNUSED)make_soft; (UNUSED)dt; (UNUSED)apply_bias; #endif @@ -1355,6 +1416,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(); solve_collisions(substep_dt, true); integrate_positions(substep_dt); solve_collisions(substep_dt, false); /* Relaxation */ diff --git a/src/gjk.c b/src/gjk.c index ae5bf520..a4a4d0f8 100644 --- a/src/gjk.c +++ b/src/gjk.c @@ -193,7 +193,7 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru * ========================== */ { - /* FIXME: Collision is false when 2 shapes exactly overlap */ + /* First point is support point in shape's general directions to eachother */ dir = v2_sub(shape1.points[0], shape0.points[0]); @@ -256,6 +256,10 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru * Epa (to find collision normal) * ========================== */ + /* If penetration is less than this, collision will return false (to + * prevent wacky extrapolated normals at small penetration values) */ + const f32 min_pen_len = 0.0001f; + proto = arena_dry_push(scratch.arena, struct v2); proto_count = 0; { @@ -267,7 +271,7 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru proto_count = 3; } - while (true) { + while (colliding) { f32 pen_len_sq = F32_INFINITY; /* Find dir from origin to closest edge */ @@ -283,9 +287,7 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru struct v2 vse = v2_sub(pe, ps); struct v2 vso = v2_neg(ps); - f32 d1 = v2_dot(vso, vse); - f32 d2 = v2_dot(vse, vse); - struct v2 vsd = v2_mul(vse, (d1 / d2)); + struct v2 vsd = v2_mul(vse, (v2_dot(vso, vse) / v2_len_sq(vse))); struct v2 pd = v2_add(ps, vsd); f32 pd_len_sq = v2_len_sq(pd); @@ -329,7 +331,12 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru } } if (!unique) { - normal = v2_norm(normal); + f32 len = v2_len(normal); + if (len < min_pen_len) { + colliding = false; + } else { + normal = v2_mul(normal, 1.f / len); + } break; } } @@ -345,7 +352,9 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru } proto[pen_pe_index] = m; } + } + if (colliding) { /* ========================== * * Clipping * ========================== */ diff --git a/src/user.c b/src/user.c index 7bed6110..2ed10179 100644 --- a/src/user.c +++ b/src/user.c @@ -1030,7 +1030,7 @@ INTERNAL void user_update(void) (UNUSED)e1_quad; (UNUSED)e1_poly; -#if 0 +#if 1 /* Draw menkowski */ {