From 88334f2ab1ffdc3d803aa43f2b277fcaebccae40 Mon Sep 17 00:00:00 2001 From: jacob Date: Mon, 23 Sep 2024 15:00:14 -0500 Subject: [PATCH] test midpoint contact point --- src/entity.c | 8 +++- src/entity.h | 11 ++--- src/game.c | 111 +++++++++++++++++---------------------------------- src/gjk.c | 3 -- src/user.c | 32 +++++++++------ 5 files changed, 65 insertions(+), 100 deletions(-) diff --git a/src/entity.c b/src/entity.c index cbc9d88a..be7cd695 100644 --- a/src/entity.c +++ b/src/entity.c @@ -75,13 +75,19 @@ void entity_store_copy_replace(struct entity_store *dest, struct entity_store *s dest->entities = dest_entities; } -void entity_store_reset(struct entity_store *store) +void entity_store_release_all_entities(struct entity_store *store) { +#if 0 store->allocated = 0; store->reserved = 0; store->first_free = entity_nil_handle(); arena_pop_to(&store->arena, STORE_ENTITIES_OFFSET); store_make_root(store); +#else + struct entity *root = entity_from_handle(store, store->root); + entity_release(store, root); + store_make_root(store); +#endif } /* ========================== * diff --git a/src/entity.h b/src/entity.h index ea12cc6d..7c280fd1 100644 --- a/src/entity.h +++ b/src/entity.h @@ -64,15 +64,10 @@ struct entity_store { struct contact { - struct v2 p0_local; - struct v2 p0_initial_world; + struct v2 point; - struct v2 p1_local; - struct v2 p1_initial_world; - - f32 depth; /* How far is p0 from p1 along normal */ + f32 separation; /* How far is p0 from p1 along normal */ f32 accumulated_impulse; /* Accumulated impulse along normal */ - f32 delta_impulse; /* Change in impulse along normal */ b32 persisted; }; @@ -322,7 +317,7 @@ INLINE b32 entity_has_prop(struct entity *ent, enum entity_prop prop) struct entity_store *entity_store_alloc(void); void entity_store_release(struct entity_store *store); void entity_store_copy_replace(struct entity_store *dest, struct entity_store *src); -void entity_store_reset(struct entity_store *store); +void entity_store_release_all_entities(struct entity_store *store); /* Entity */ struct entity *entity_alloc(struct entity *parent); diff --git a/src/game.c b/src/game.c index 9bfe9d7e..6df49ac4 100644 --- a/src/game.c +++ b/src/game.c @@ -126,16 +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, -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 / 3; + f32 r = PI / 3; //f32 r = PI / 2; - f32 r = 0; + //f32 r = 0; f32 skew = 0; struct entity *e = entity_alloc(root); @@ -152,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); @@ -341,15 +342,9 @@ INTERNAL void create_contact_manifolds(void) }; } -#if 1 +#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; -#elif 0 - const f32 remove_contact_threshold_global_dist_sq = 0; - const f32 insert_contact_threshold_global_dist_sq = 0; -#else - const f32 remove_contact_threshold_global_dist_sq = F32_INFINITY; - const f32 insert_contact_threshold_global_dist_sq = F32_INFINITY; #endif struct gjk_contact_points_result res = gjk_contact_points(e0_poly, e1_poly); @@ -387,42 +382,7 @@ INTERNAL void create_contact_manifolds(void) } } - /* Update contact depths, and remove contacts that are no longer penetrating or have drifted too far from original position */ - struct v2 old_normal = manifold->manifold_normal; - for (i32 j = 0; j < (i32)manifold->num_contacts; ++j) { - struct contact *contact = &manifold->contacts[j]; - - struct v2 p0_world_initial = contact->p0_initial_world; - struct v2 p1_world_initial = contact->p1_initial_world; - - struct v2 p0_world_cur = xform_mul_v2(e0_xf, contact->p0_local); - struct v2 p1_world_cur = xform_mul_v2(e1_xf, contact->p1_local); - - f32 p0_close_enough = v2_len_sq(v2_sub(p0_world_cur, p0_world_initial)) <= remove_contact_threshold_global_dist_sq; - f32 p1_close_enough = v2_len_sq(v2_sub(p1_world_cur, p1_world_initial)) <= remove_contact_threshold_global_dist_sq; - - f32 depth_cur = v2_dot(old_normal, v2_sub(p1_world_cur, p0_world_cur)); - b32 still_penetrating = depth_cur >= 0; - - if (still_penetrating && p0_close_enough && p1_close_enough) { - /* Contact is still penetrating, mark persisted for warm starting */ - contact->persisted = true; - contact->depth = depth_cur; - } else { - /* Remove contact by swapping with last in array */ - manifold->contacts[j--] = manifold->contacts[--manifold->num_contacts]; - } - } - - - - - - /* FIXME: Test overflow */ - struct contact overflow[2]; - u32 overflow_count = 0; - CT_ASSERT(ARRAY_COUNT(overflow) >= ARRAY_COUNT(res.pairs)); /* Overflow should be able to hold collision result */ - + manifold->num_contacts = 0; /* Insert new contacts that aren't too close to original contacts */ for (u32 i = 0; i < res.num_pairs; ++i) { @@ -432,6 +392,7 @@ INTERNAL void create_contact_manifolds(void) struct v2 p0_world = new_pair.p0; struct v2 p1_world = new_pair.p1; +#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); @@ -443,26 +404,22 @@ INTERNAL void create_contact_manifolds(void) break; } } +#endif if (should_insert) { struct contact *c; - if (manifold->num_contacts < ARRAY_COUNT(manifold->contacts)) { - c = &manifold->contacts[manifold->num_contacts++]; - } else { - c = &overflow[overflow_count++]; - } + c = &manifold->contacts[manifold->num_contacts++]; f32 depth = v2_len(v2_sub(p1_world, p0_world)); + struct v2 point = v2_mul(v2_add(p0_world, p1_world), 0.5f); MEMZERO_STRUCT(c); - c->p0_local = xform_invert_mul_v2(e0_xf, p0_world); - c->p0_initial_world = p0_world; - c->p1_local = xform_invert_mul_v2(e1_xf, p1_world); - c->p1_initial_world = p1_world; - c->depth = depth; + c->point = point; + c->separation = depth; } } +#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) { @@ -519,12 +476,13 @@ INTERNAL void create_contact_manifolds(void) manifold->contacts[1] = furthest_cpy; } } +#endif /* NOTE: This assumes that all contacts will share the same normal as first contact */ - manifold->manifold_normal = v2_norm(v2_sub(manifold->contacts[0].p1_initial_world, manifold->contacts[0].p0_initial_world)); + manifold->manifold_normal = v2_norm(v2_sub(res.pairs[0].p1, res.pairs[0].p0)); @@ -543,7 +501,7 @@ INTERNAL void create_contact_manifolds(void) } else if (manifold->valid) { /* No longer colliding, delete manifold */ -#if 1 +#if 0 manifold->num_contacts = 0; entity_enable_prop(manifold, ENTITY_PROP_RELEASE); #else @@ -564,6 +522,7 @@ INTERNAL void create_contact_manifolds(void) INTERNAL void solve_collisions(f32 dt, b32 apply_bias) { +#if 0 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]; @@ -592,7 +551,7 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) f32 inv_i1 = 1.f / i1; struct queued_impulse { - struct v2 p0, p1; + struct v2 point; f32 impulse; }; struct queued_impulse queued_impulses[ARRAY_COUNT(manifold->contacts)]; @@ -601,8 +560,7 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) 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->p0_local); - struct v2 p1 = xform_mul_v2(e1_xf, contact->p1_local); + struct v2 p = contact->point; f32 bias = 0; if (apply_bias) { @@ -613,17 +571,17 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) f32 bias_slop = 0.005f; //f32 bias_slop = 0.00f; - bias = (bias_factor / dt) * max_f32((contact->depth - bias_slop), 0); + bias = (bias_factor / dt) * max_f32((contact->separation - bias_slop), 0); } - struct v2 vcp0 = v2_sub(p0, e0_xf.og); - struct v2 vcp1 = v2_sub(p1, e1_xf.og); + struct v2 vcp0 = v2_sub(p, e0_xf.og); + struct v2 vcp1 = v2_sub(p, e1_xf.og); - struct v2 p0_vel = v2_add(e0->linear_velocity, v2_perp_mul(vcp0, e0->angular_velocity)); - struct v2 p1_vel = v2_add(e1->linear_velocity, v2_perp_mul(vcp1, e1->angular_velocity)); + 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 vrel = v2_sub(p1_vel, p0_vel); + struct v2 vrel = v2_sub(vel1, vel0); f32 vn = v2_dot(vrel, normal); /* FIXME: Clamp accumulated */ @@ -645,8 +603,7 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) contact->accumulated_impulse = new_accumulated_impulse; queued_impulses[contact_index] = (struct queued_impulse) { - .p0 = p0, - .p1 = p1, + .point = p, .impulse = delta_impulse }; } @@ -654,11 +611,15 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) for (u32 i = 0; i < num_contacts; ++i) { struct queued_impulse qi = queued_impulses[i]; struct v2 impulse = v2_mul(normal, qi.impulse); - entity_apply_linear_impulse(e0, impulse, qi.p0); - entity_apply_linear_impulse(e1, v2_neg(impulse), qi.p1); + entity_apply_linear_impulse(e0, impulse, qi.point); + entity_apply_linear_impulse(e1, v2_neg(impulse), qi.point); } } } +#else + (UNUSED)dt; + (UNUSED)apply_bias; +#endif } @@ -775,7 +736,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) case GAME_CMD_KIND_CLEAR_ALL: { logf_info("Clearing level"); - entity_store_reset(store); + entity_store_release_all_entities(store); G.box_spawned = false; } break; @@ -1260,7 +1221,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) * Create ground friction force (gravity) * ========================== */ -#if 0 +#if 1 /* 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]; diff --git a/src/gjk.c b/src/gjk.c index 51eec46f..61d5494a 100644 --- a/src/gjk.c +++ b/src/gjk.c @@ -156,9 +156,6 @@ struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_arra - -/* TODO: Update this function to only calculate contact pairs if colliding. Will allow for shortcut-style GJK. - * `colliding` & `has_2nd_pair` become "num_pairs", where 0 = no collision. */ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, struct v2_array shape1) { struct temp_arena scratch = scratch_begin_no_conflict(); /* TODO: Only begin scratch for EPA */ diff --git a/src/user.c b/src/user.c index 40fbdb9b..36f8bd8a 100644 --- a/src/user.c +++ b/src/user.c @@ -1029,17 +1029,7 @@ INTERNAL void user_update(void) (UNUSED)e1_quad; (UNUSED)e1_poly; -#if 0 -#if 0 - /* Draw collision shapes */ - { - f32 thickness = 2; - u32 color = RGBA_32_F(1, 0, 0, 0.25); - struct quad quad = xform_mul_quad(G.world_view, e0_quad); - draw_solid_quad_line(G.viewport_canvas, quad, thickness, color); - } -#endif - +#if 1 /* Draw menkowski */ { @@ -1131,20 +1121,36 @@ INTERNAL void user_update(void) f32 radius = 5; for (u32 i = 0; i < ent->num_contacts; ++i) { struct contact contact = ent->contacts[i]; + /* Draw point */ { u32 color = entity_has_prop(e0, ENTITY_PROP_PLAYER_CONTROLLED) ? RGBA_32_F(1, 0, 0, 0.50) : RGBA_32_F(0, 1, 1, 0.50); - struct v2 point = xform_mul_v2(e0_xf, contact.p0_local); + //struct v2 point = xform_mul_v2(e0_xf, contact.p0_local); //struct v2 point = contact.p0_initial_world; + struct v2 point = contact.point; point = xform_mul_v2(G.world_view, point); draw_solid_circle(G.viewport_canvas, point, radius, color, 10); } + /* Draw normal */ + { + u32 color = COLOR_WHITE; + f32 len = 0.1; + f32 arrow_thickness = 2; + f32 arrow_height = 5; + struct v2 start = xform_mul_v2(G.world_view, contact.point); + struct v2 end = xform_mul_v2(G.world_view, v2_add(contact.point, v2_mul(v2_norm(ent->manifold_normal), len))); + draw_solid_arrow_line(G.viewport_canvas, start, end, arrow_thickness, arrow_height, color); + } + +#if 0 { u32 color = entity_has_prop(e1, ENTITY_PROP_PLAYER_CONTROLLED) ? RGBA_32_F(1, 0, 0, 0.50) : RGBA_32_F(0, 1, 1, 0.50); - struct v2 point = xform_mul_v2(e1_xf, contact.p1_local); + //struct v2 point = xform_mul_v2(e1_xf, contact.p1_local); //struct v2 point = contact.p1_initial_world; + struct v2 point = contact.point; point = xform_mul_v2(G.world_view, point); draw_solid_circle(G.viewport_canvas, point, radius, color, 10); } +#endif } } #endif