From c00d43b1deb0751c8404a72248ff6555599bb5cf Mon Sep 17 00:00:00 2001 From: jacob Date: Wed, 25 Sep 2024 17:02:46 -0500 Subject: [PATCH] resolve collisions using the new clipped contacts --- src/entity.h | 2 +- src/game.c | 185 ++++++++++++++++++++------------------------------- src/gjk.c | 18 +++-- src/user.c | 20 ++---- 4 files changed, 93 insertions(+), 132 deletions(-) diff --git a/src/entity.h b/src/entity.h index 18c57c8f..3885c8c2 100644 --- a/src/entity.h +++ b/src/entity.h @@ -66,7 +66,7 @@ struct entity_store { struct contact { struct v2 point; - u32 id; + u32 id; /* ID generated during clipping */ f32 separation; /* How far is p0 from p1 along normal */ f32 accumulated_impulse; /* Accumulated impulse along normal */ diff --git a/src/game.c b/src/game.c index b5b97218..d2319a0d 100644 --- a/src/game.c +++ b/src/game.c @@ -126,17 +126,17 @@ INTERNAL void spawn_test_entities(void) /* Player */ struct entity *player_ent; { - //struct v2 pos = V2(0.25, -3); - struct v2 pos = V2(0.25, -2); + struct v2 pos = V2(0.25, -3); + //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 / 4; + //f32 r = PI / 4; //f32 r = PI / 3; //f32 r = PI / 2; - //f32 r = 0; + f32 r = 0; f32 skew = 0; struct entity *e = entity_alloc(root); @@ -153,8 +153,8 @@ INTERNAL void spawn_test_entities(void) entity_enable_prop(e, ENTITY_PROP_PLAYER_CONTROLLED); //e->control_force = 4500; - e->control_force = 1200; - //e->control_force = 250; + //e->control_force = 1200; + e->control_force = 250; e->control_torque = 10; e->control.focus = V2(0, -1); @@ -196,8 +196,8 @@ INTERNAL void spawn_test_entities(void) if (!G.box_spawned) { G.box_spawned = true; - //struct v2 pos = V2(0.5, -1); - struct v2 pos = V2(1, -1); + struct v2 pos = V2(0.5, -1); + //struct v2 pos = V2(1, -1); struct v2 size = V2(1, 1); //f32 rot = PI / 4; f32 rot = 0; @@ -251,6 +251,25 @@ INTERNAL void spawn_test_entities(void) * TESTING * ========================== */ +INTERNAL void pre_step(void) +{ + struct entity_store *store = G.tick.entity_store; + + for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { + struct entity *manifold = &store->entities[entity_index]; + if (!(manifold->valid && entity_has_prop(manifold, ENTITY_PROP_ACTIVE))) continue; + if (!entity_has_prop(manifold, ENTITY_PROP_MANIFOLD)) continue; + + for (u32 i = 0; i < manifold->num_contacts; ++i) { + struct contact *contact = &manifold->contacts[i]; + contact->accumulated_impulse = 0; + } + } +} + + + + INTERNAL void create_contact_manifolds(void) { /* TODO: Remove this */ @@ -342,13 +361,12 @@ INTERNAL void create_contact_manifolds(void) }; } -#if 0 - const f32 remove_contact_threshold_global_dist_sq = 0.025f * 0.025f; - const f32 insert_contact_threshold_global_dist_sq = 0.025f * 0.025f; -#endif - struct gjk_contact_points_result res = gjk_contact_points(e0_poly, e1_poly); + /* Parts of algorithm are hard-coded to support 2 contact points */ + CT_ASSERT(ARRAY_COUNT(manifold->contacts) == 2); + CT_ASSERT(ARRAY_COUNT(res.points) == 2); + /* TODO: Remove this (debugging) */ if (manifold->valid) { manifold->prototype = res.prototype; @@ -382,113 +400,53 @@ INTERNAL void create_contact_manifolds(void) } manifold->manifold_normal = res.normal; - manifold->num_contacts = 0; - /* Insert new contacts that aren't too close to original contacts */ - for (u32 i = 0; i < res.num_points; ++i) { - b32 should_insert = true; -#if 0 - for (u32 j = 0; j < manifold->num_contacts; ++j) { - struct contact *contact = &manifold->contacts[j]; - struct v2 old_p0_world = xform_mul_v2(e0_xf, contact->p0_local); - struct v2 old_p1_world = xform_mul_v2(e1_xf, contact->p1_local); - f32 p0_far_enough = v2_len_sq(v2_sub(p0_world, old_p0_world)) >= insert_contact_threshold_global_dist_sq; - f32 p1_far_enough = v2_len_sq(v2_sub(p1_world, old_p1_world)) >= insert_contact_threshold_global_dist_sq; - if (!p0_far_enough || !p1_far_enough) { - should_insert = false; + /* Delete old contacts that are no longer present */ + for (u32 i = 0; i < manifold->num_contacts; ++i) { + struct contact *old = &manifold->contacts[i]; + u32 id = old->id; + b32 found = false; + for (u32 j = 0; j < res.num_points; ++j) { + if (res.points[j].id == id) { + found = true; break; } } -#endif - - if (should_insert) { - struct contact *c; - c = &manifold->contacts[manifold->num_contacts++]; - - u32 id = res.points[i].id; - struct v2 point = res.points[i].point; - f32 separation = res.points[i].separation; - - MEMZERO_STRUCT(c); - c->id = id; - c->point = point; - c->separation = separation; + if (!found) { + /* Delete contact by replacing with last in array */ + *old = manifold->contacts[--manifold->num_contacts]; + --i; } } -#if 0 - /* Manifold already has max amount of contacts, decide which 2 to keep. - * Logic: 1st contact to keep is deepest contact, 2nd contact is furthest point from 1st contact (in local space of first shape) */ - if (overflow_count > 0) { - /* Find contact with greatest depth */ - struct contact *deepest = NULL; + /* Update / insert returned contacts */ + for (u32 i = 0; i < res.num_points; ++i) { + struct gjk_contact_point *res_point = &res.points[i]; + struct v2 point = res_point->point; + f32 sep = res_point->separation; + u32 id = res_point->id; + struct contact *contact = NULL; { - f32 deepest_depth = -F32_INFINITY; - for (u32 i = 0; i < manifold->num_contacts; ++i) { - struct contact *c = &manifold->contacts[i]; - f32 depth = c->depth; - if (depth > deepest_depth) { - deepest = c; - deepest_depth = depth; + for (u32 j = 0; j < manifold->num_contacts; ++j) { + struct contact *t = &manifold->contacts[j]; + if (t->id == id) { + /* Update existing */ + contact = t; + contact->persisted = true; + break; } } - for (u32 i = 0; i < overflow_count; ++i) { - struct contact *c = &overflow[i]; - f32 depth = c->depth; - if (depth > deepest_depth) { - deepest = c; - deepest_depth = depth; - } + if (!contact) { + /* Insert new */ + contact = &manifold->contacts[manifold->num_contacts++]; + MEMZERO_STRUCT(contact); + contact->id = id; } } - /* Find contact furthest from deepest point */ - struct contact *furthest = NULL; - { - struct v2 deepest_p0 = deepest->p0_local; - f32 furthest_dist_sq = -F32_INFINITY; - for (u32 i = 0; i < manifold->num_contacts; ++i) { - struct contact *c = &manifold->contacts[i]; - f32 dist_sq = v2_len_sq(v2_sub(c->p0_local, deepest_p0)); - if (dist_sq > furthest_dist_sq) { - furthest = c; - furthest_dist_sq = dist_sq; - } - } - for (u32 i = 0; i < overflow_count; ++i) { - struct contact *c = &manifold->contacts[i]; - f32 dist_sq = v2_len_sq(v2_sub(c->p0_local, deepest_p0)); - if (dist_sq > furthest_dist_sq) { - furthest = c; - furthest_dist_sq = dist_sq; - } - } - } - /* Copy */ - /* TODO: Optimize */ - CT_ASSERT(ARRAY_COUNT(manifold->contacts) == 2); /* Logic assumes 2 contact points */ - { - struct contact deepest_cpy = *deepest; - struct contact furthest_cpy = *furthest; - manifold->contacts[0] = deepest_cpy; - manifold->contacts[1] = furthest_cpy; - } + contact->point = point; + contact->separation = sep; } -#endif - - - - - - /* TODO: Remove this (testing) */ -#if 0 - if (manifold->num_contacts > 1) { - if (manifold->contacts[1].depth > manifold->contacts[1].depth) { - manifold->contacts[0] = manifold->contacts[1]; - } - manifold->num_contacts = 1; - } -#endif } else if (manifold->valid) { @@ -514,7 +472,7 @@ INTERNAL void create_contact_manifolds(void) INTERNAL void solve_collisions(f32 dt, b32 apply_bias) { -#if 0 +#if 1 struct entity_store *store = G.tick.entity_store; for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { struct entity *manifold = &store->entities[entity_index]; @@ -556,11 +514,11 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) f32 bias = 0; if (apply_bias) { - //f32 bias_factor = 0.2f; - f32 bias_factor = 0.025f; + f32 bias_factor = 0.2f; + //f32 bias_factor = 0.025f; //f32 bias_slop = 0.0005f; - f32 bias_slop = 0.005f; + f32 bias_slop = 0.01f; //f32 bias_slop = 0.00f; bias = (bias_factor / dt) * max_f32((contact->separation - bias_slop), 0); @@ -576,7 +534,6 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) struct v2 vrel = v2_sub(vel1, vel0); f32 vn = v2_dot(vrel, normal); - /* FIXME: Clamp accumulated */ vn = max_f32(vn, 0); struct v2 idk0 = v2_perp_right_mul(vcp0, v2_wedge(vcp0, normal) * inv_i0); @@ -1060,7 +1017,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) * Create forces from control focus (aim) * ========================== */ -#if 1 +#if 0 for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { struct entity *ent = &store->entities[entity_index]; if (!(ent->valid && entity_has_prop(ent, ENTITY_PROP_ACTIVE))) continue; @@ -1213,7 +1170,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) * Create ground friction force (gravity) * ========================== */ -#if 1 +#if 0 /* TODO: Do this globally rather than creating entities for constant forces */ for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { struct entity *ent = &store->entities[entity_index]; @@ -1303,6 +1260,8 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) * ========================== */ { + pre_step(); + f32 substep_dt = dt / GAME_PHYSICS_SUBSTEPS; for (u32 i = 0; i < GAME_PHYSICS_SUBSTEPS; ++i) { diff --git a/src/gjk.c b/src/gjk.c index 65d8f639..8f5add8a 100644 --- a/src/gjk.c +++ b/src/gjk.c @@ -323,6 +323,7 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru /* TODO: Better */ { b32 unique = true; +#if 0 for (u32 i = 0; i < proto_count; ++i) { struct v2 edge_start = proto[i]; struct v2 edge_end = i < proto_count - 1 ? proto[i + 1] : proto[0]; @@ -331,6 +332,15 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru break; } } +#else + for (u32 i = 0; i < proto_count; ++i) { + struct v2 p = proto[i]; + if (v2_len_sq(v2_sub(p, m)) < epsilon) { + unique = false; + break; + } + } +#endif if (!unique) { normal = v2_norm(normal); break; @@ -353,9 +363,9 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru * Clipping * ========================== */ - /* FIXME: Limit max vertices in shape structure to 8 for id generation to be correct */ - ASSERT(shape0.count <= 8); - ASSERT(shape1.count <= 8); + /* FIXME: Limit max vertices in shape structure to at least < 16 for id generation to be correct */ + ASSERT(shape0.count <= 16); + ASSERT(shape1.count <= 16); DBGSTEP; { @@ -548,7 +558,7 @@ abort: } res.prototype.len = s.len; } - res.normal = normal; + res.normal = v2_neg(normal); res.points[0] = points[0]; res.points[1] = points[1]; res.num_points = num_points; diff --git a/src/user.c b/src/user.c index f2fd95a0..feafe6bb 100644 --- a/src/user.c +++ b/src/user.c @@ -1119,21 +1119,11 @@ INTERNAL void user_update(void) /* Draw contacts */ { f32 radius = 5; - - u32 colors[2] = { 0 }; - if (entity_has_prop(e0, ENTITY_PROP_PLAYER_CONTROLLED)) { - colors[0] = RGBA_32_F(1, 0, 0, 0.50); - colors[1] = RGBA_32_F(1, 1, 0, 0.50); - } else { - colors[0] = RGBA_32_F(1, 0, 1, 0.50); - colors[1] = RGBA_32_F(0, 1, 1, 0.50); - } - for (u32 i = 0; i < ent->num_contacts; ++i) { struct contact contact = ent->contacts[i]; /* Draw point */ { - u32 color = i < ARRAY_COUNT(colors) ? colors[i] : COLOR_RED; + u32 color = contact.persisted ? RGBA_32_F(1, 1, 0, 0.50) : RGBA_32_F(1, 0, 0, 0.50); //struct v2 point = xform_mul_v2(e0_xf, contact.p0_local); //struct v2 point = contact.p0_initial_world; struct v2 point = contact.point; @@ -1155,12 +1145,14 @@ INTERNAL void user_update(void) struct font *disp_font = font_load_async(STR("res/fonts/fixedsys.ttf"), 12.0f); if (disp_font) { f32 offset_px = -20; - u32 id = contact.id; struct string fmt = STR( - "id: 0x%F" + "id: 0x%F\n" + "impulse: %F" ); - struct string text = string_format(temp.arena, fmt, FMT_HEX(id)); + struct string text = string_format(temp.arena, fmt, + FMT_HEX(contact.id), + FMT_FLOAT(contact.accumulated_impulse)); draw_text(G.viewport_canvas, disp_font, v2_add(v2_round(xform_mul_v2(G.world_view, contact.point)), V2(0, offset_px)), text);