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

soft contact testing

Browse Source
This commit is contained in:
jacob 2024-10-04 09:18:09 -05:00
parent 3c275dfc9a
commit cf911f27b7
3 changed files with 103 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

112
src/game.c
View File

@ -126,16 +126,19 @@ INTERNAL void spawn_test_entities(void)
/* Player */ /* Player */
struct entity *player_ent; 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(0.25, -2);
//struct v2 pos = V2(1.1230469346046448864129274625156, -1); /* Touching right side of box */ //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(1.1230469346046448864129274625156 - 0.0001, -1); /* Touching right side of box */
//struct v2 pos = V2(0.374142020941, -0.246118023992); /* Touching glitch spot */ //struct v2 pos = V2(0.374142020941, -0.246118023992); /* Touching glitch spot */
//struct v2 size = V2(1, 1); //struct v2 size = V2(1, 1);
struct v2 size = V2(0.5, 0.5); struct v2 size = V2(0.5, 0.5);
f32 r = PI; //f32 r = PI;
//f32 r = PI / 4; f32 r = PI / 4;
//f32 r = PI / 3; //f32 r = PI / 3;
//f32 r = 0.05;
//f32 r = PI / 2; //f32 r = PI / 2;
//f32 r = 0; //f32 r = 0;
f32 skew = 0; f32 skew = 0;
@ -161,8 +164,8 @@ INTERNAL void spawn_test_entities(void)
entity_enable_prop(e, ENTITY_PROP_PHYSICAL); entity_enable_prop(e, ENTITY_PROP_PHYSICAL);
e->mass_unscaled = 100; 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->linear_ground_friction = 1000;
e->angular_ground_friction = 100; e->angular_ground_friction = 100;
@ -425,7 +428,7 @@ INTERNAL void create_contact_manifolds(void)
} }
if (contact) { if (contact) {
/* Update existing */ /* Update existing */
#if 0 #if 1
contact->normal_impulse = 0; contact->normal_impulse = 0;
contact->tangent_impulse = 0; contact->tangent_impulse = 0;
#endif #endif
@ -443,7 +446,7 @@ INTERNAL void create_contact_manifolds(void)
} else if (manifold) { } else if (manifold) {
/* No longer colliding, delete manifold */ /* No longer colliding, delete manifold */
#if 1 #if 0
manifold->num_contacts = 0; manifold->num_contacts = 0;
entity_enable_prop(manifold, ENTITY_PROP_RELEASE); entity_enable_prop(manifold, ENTITY_PROP_RELEASE);
#else #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) INTERNAL void solve_collisions(f32 dt, b32 apply_bias)
{ {
#if 1 #if 1
@ -534,13 +568,13 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias)
struct v2 v0 = e0->linear_velocity; struct v2 v0 = e0->linear_velocity;
struct v2 v1 = e1->linear_velocity; struct v2 v1 = e1->linear_velocity;
struct v2 v0_start = v0; //struct v2 v0_start = v0;
struct v2 v1_start = v1; //struct v2 v1_start = v1;
f32 w0 = e0->angular_velocity; f32 w0 = e0->angular_velocity;
f32 w1 = e1->angular_velocity; f32 w1 = e1->angular_velocity;
f32 w0_start = w0; //f32 w0_start = w0;
f32 w1_start = w1; //f32 w1_start = w1;
u32 num_contacts = manifold->num_contacts; u32 num_contacts = manifold->num_contacts;
f32 inv_num_contacts = 1.f / 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 vcp0 = v2_sub(p0, e0_xf.og);
struct v2 vcp1 = v2_sub(p1, e1_xf.og); struct v2 vcp1 = v2_sub(p1, e1_xf.og);
f32 bias = 0; f32 penetration = -(v2_dot(v2_sub(p1, p0), normal) + contact->starting_separation);
if (apply_bias) {
const f32 bias_factor = 0.1f; f32 velocity_bias = 0.0f;
const f32 bias_slop = 0.02f; f32 mass_scale = 1.0f;
f32 penetration = v2_dot(v2_sub(p0, p1), normal) - contact->starting_separation; f32 impulse_scale = 0.0f;
bias = (bias_factor / dt) * max_f32(penetration - bias_slop, 0);
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 vel0 = v2_add(v0, v2_perp_mul(vcp0, w0));
struct v2 vel1 = v2_add(v1_start, v2_perp_mul(vcp1, w1_start)); struct v2 vel1 = v2_add(v1, v2_perp_mul(vcp1, w1));
struct v2 vrel = v2_sub(vel0, vel1); struct v2 vrel = v2_sub(vel0, vel1);
f32 vcp0_wedge = v2_wedge(vcp0, normal); 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) */ /* (to be applied along n) */
f32 vn = v2_dot(vrel, normal); 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; //j *= inv_num_contacts;
f32 old_impulse = contact->normal_impulse; 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 vcp0 = v2_sub(p0, e0_xf.og);
struct v2 vcp1 = v2_sub(p1, e1_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 vel0 = v2_add(v0, v2_perp_mul(vcp0, w0));
struct v2 vel1 = v2_add(v1_start, v2_perp_mul(vcp1, w1_start)); struct v2 vel1 = v2_add(v1, v2_perp_mul(vcp1, w1));
struct v2 vrel = v2_sub(vel0, vel1); struct v2 vrel = v2_sub(vel0, vel1);
f32 vcp0_wedge = v2_wedge(vcp0, tangent); 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) */ /* (to be applied along t) */
f32 vt = v2_dot(vrel, tangent); f32 vt = v2_dot(vrel, tangent);
f32 j = vt * k; 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 = 1.0f;
//f32 friction = 999999.f; f32 friction = 999999.f;
f32 max_friction = friction * contact->normal_impulse; f32 max_friction = friction * contact->normal_impulse;
f32 old_impulse = contact->tangent_impulse; f32 old_impulse = contact->tangent_impulse;
f32 new_impulse = clamp_f32(old_impulse + j, -max_friction, max_friction); 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 #else
(UNUSED)make_soft;
(UNUSED)dt; (UNUSED)dt;
(UNUSED)apply_bias; (UNUSED)apply_bias;
#endif #endif
@ -1355,6 +1416,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
f32 substep_dt = dt / GAME_PHYSICS_SUBSTEPS; f32 substep_dt = dt / GAME_PHYSICS_SUBSTEPS;
for (u32 i = 0; i < GAME_PHYSICS_SUBSTEPS; ++i) { for (u32 i = 0; i < GAME_PHYSICS_SUBSTEPS; ++i) {
#if 1 #if 1
//warm_start_contacts();
solve_collisions(substep_dt, true); solve_collisions(substep_dt, true);
integrate_positions(substep_dt); integrate_positions(substep_dt);
solve_collisions(substep_dt, false); /* Relaxation */ solve_collisions(substep_dt, false); /* Relaxation */

21
src/gjk.c
View File

@ -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 */ /* First point is support point in shape's general directions to eachother */
dir = v2_sub(shape1.points[0], shape0.points[0]); 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) * 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 = arena_dry_push(scratch.arena, struct v2);
proto_count = 0; proto_count = 0;
{ {
@ -267,7 +271,7 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru
proto_count = 3; proto_count = 3;
} }
while (true) { while (colliding) {
f32 pen_len_sq = F32_INFINITY; f32 pen_len_sq = F32_INFINITY;
/* Find dir from origin to closest edge */ /* 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 vse = v2_sub(pe, ps);
struct v2 vso = v2_neg(ps); struct v2 vso = v2_neg(ps);
f32 d1 = v2_dot(vso, vse); struct v2 vsd = v2_mul(vse, (v2_dot(vso, vse) / v2_len_sq(vse)));
f32 d2 = v2_dot(vse, vse);
struct v2 vsd = v2_mul(vse, (d1 / d2));
struct v2 pd = v2_add(ps, vsd); struct v2 pd = v2_add(ps, vsd);
f32 pd_len_sq = v2_len_sq(pd); 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) { 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; break;
} }
} }
@ -345,7 +352,9 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru
} }
proto[pen_pe_index] = m; proto[pen_pe_index] = m;
} }
}
if (colliding) {
/* ========================== * /* ========================== *
* Clipping * Clipping
* ========================== */ * ========================== */

2
src/user.c
View File

@ -1030,7 +1030,7 @@ INTERNAL void user_update(void)
(UNUSED)e1_quad; (UNUSED)e1_quad;
(UNUSED)e1_poly; (UNUSED)e1_poly;
#if 0 #if 1
/* Draw menkowski */ /* Draw menkowski */
{ {