From 41d56ac13cae73d4b86ca3012cb52a91d26a0566 Mon Sep 17 00:00:00 2001 From: jacob Date: Wed, 18 Sep 2024 20:07:58 -0500 Subject: [PATCH] more physics testing (separate logic to functions & re-calculate constraint depth each iteration) --- src/game.c | 873 ++++++++++++++++++++++++++++++----------------------- src/gjk.c | 2 +- 2 files changed, 492 insertions(+), 383 deletions(-) diff --git a/src/game.c b/src/game.c index a97751e3..9275a6c0 100644 --- a/src/game.c +++ b/src/game.c @@ -17,10 +17,12 @@ GLOBAL struct { struct sys_thread game_thread; b32 paused; + struct sprite_scope *sprite_frame_scope; /* Game thread input */ struct sys_mutex game_cmds_mutex; struct arena game_cmds_arena; + struct entity *root; /* Ticks */ struct sys_mutex prev_tick_mutex; @@ -127,10 +129,10 @@ INTERNAL void spawn_test_entities(void) //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 +155,8 @@ INTERNAL void spawn_test_entities(void) entity_enable_prop(e, ENTITY_PROP_PHYSICAL); e->mass_unscaled = 100; - //e->inertia_unscaled = 1; - e->inertia_unscaled = F32_INFINITY; + e->inertia_unscaled = 1; + //e->inertia_unscaled = F32_INFINITY; e->linear_ground_friction = 1000; e->angular_ground_friction = 100; @@ -224,6 +226,460 @@ INTERNAL void spawn_test_entities(void) e->camera_quad_xform = XFORM_TRS(.s = V2(width, height)); } } + + + + + + + + + + + + + + +/* ========================== * + * TESTING + * ========================== */ + +INTERNAL void create_contact_manifolds(void) +{ + struct entity_store *store = G.tick.entity_store; + struct sprite_scope *sprite_frame_scope = G.sprite_frame_scope; + struct entity *root = G.root; + for (u64 e0_index = 0; e0_index < store->reserved; ++e0_index) { + struct entity *e0 = &store->entities[e0_index]; + if (!(e0->valid && entity_has_prop(e0, ENTITY_PROP_ACTIVE))) continue; + if (!entity_has_prop(e0, ENTITY_PROP_PHYSICAL)) continue; + if (!entity_has_prop(e0, ENTITY_PROP_PLAYER_CONTROLLED) && !entity_has_prop(e0, ENTITY_PROP_BULLET)) continue; + + struct xform e0_xf = entity_get_xform(e0); + struct quad e0_quad; + struct v2_array e0_poly; + { + struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, e0->sprite); + struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("shape"), e0->animation_frame); + e0_quad = xform_mul_quad(e0->sprite_local_xform, quad_from_rect(slice.rect)); + e0_quad = xform_mul_quad(e0_xf, e0_quad); + e0_poly = (struct v2_array) { + .count = ARRAY_COUNT(e0_quad.e), + .points = e0_quad.e + }; + } + + for (u64 e1_index = 0; e1_index < store->reserved; ++e1_index) { + struct entity *e1 = &store->entities[e1_index]; + if (e1 == e0) continue; + if (!(e1->valid && entity_has_prop(e1, ENTITY_PROP_ACTIVE))) continue; + if (!entity_has_prop(e1, ENTITY_PROP_PHYSICAL)) continue; + if (entity_has_prop(e1, ENTITY_PROP_PLAYER_CONTROLLED)) continue; + + struct xform e1_xf = entity_get_xform(e1); + struct quad e1_quad; + struct v2_array e1_poly; + { + struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, e1->sprite); + struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("shape"), e1->animation_frame); + e1_quad = xform_mul_quad(e1->sprite_local_xform, quad_from_rect(slice.rect)); + e1_quad = xform_mul_quad(e1_xf, e1_quad); + e1_poly = (struct v2_array) { + .count = ARRAY_COUNT(e1_quad.e), + .points = e1_quad.e + }; + } + + + /* TODO: Remove this */ + static struct arena dict_arena = { 0 }; + static struct fixed_dict dict = { 0 }; + if (dict.buckets_count == 0) { + dict_arena = arena_alloc(GIGABYTE(64)); + dict = fixed_dict_init(&dict_arena, 4096); + } + + struct string manifold_key; + { + /* FIXME: Contact should be same regardless of entity order */ + u64 manifold_hash = hash_fnv64(HASH_FNV64_BASIS, BUFFER_FROM_STRUCT(&e0->handle)); + manifold_hash = hash_fnv64(manifold_hash, BUFFER_FROM_STRUCT(&e1->handle)); + manifold_key = STRING_FROM_BUFFER(BUFFER_FROM_STRUCT(&manifold_hash)); + } + + struct entity *manifold = fixed_dict_get(&dict, manifold_key); + + + + +#if 1 + const f32 remove_contact_threshold_global_dist_sq = 0.005 * 0.005; + const f32 insert_contact_threshold_global_dist_sq = 0.005 * 0.005; +#else + const f32 remove_contact_threshold_global_dist_sq = 0; + const f32 insert_contact_threshold_global_dist_sq = 0; +#endif + + + + struct gjk_contact_points_result res = gjk_contact_points(e0_poly, e1_poly); + if (res.num_pairs > 0) { + if (!manifold) { + manifold = entity_alloc(root); + manifold->manifold_e0 = e0->handle; + manifold->manifold_e1 = e1->handle; + /* TODO: Should we recalculate normal as more contact points are added? */ + manifold->manifold_normal = v2_norm(v2_sub(res.pairs[0].p1, res.pairs[0].p0)); + entity_enable_prop(manifold, ENTITY_PROP_MANIFOLD); + activate_now(manifold); + fixed_dict_set(&dict_arena, &dict, manifold_key, manifold); + } + + + + + /* TODO: Remove this (debugging) */ + manifold->prototype = res.prototype; + manifold->simplex = res.simplex; + + + + + + + + + /* 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 */ + + + /* Insert new contacts that aren't too close to original contacts */ + for (u32 i = 0; i < res.num_pairs; ++i) { + b32 should_insert = true; + + struct gjk_contact_pair new_pair = res.pairs[i]; + struct v2 p0_world = new_pair.p0; + struct v2 p1_world = new_pair.p1; + + 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; + break; + } + } + + 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++]; + } + + 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 = v2_len(v2_sub(p1_world, p0_world)); + } + } + + /* 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; + { + 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 i = 0; i < overflow_count; ++i) { + struct contact *c = &overflow[i]; + f32 depth = c->depth; + if (depth > deepest_depth) { + deepest = c; + deepest_depth = depth; + } + } + } + /* 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 */ + manifold->contacts[0] = *deepest; + manifold->contacts[1] = *furthest; + } + + + + + /* 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)); + + + + + + /* 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) { + /* No longer colliding, delete contact */ + manifold->num_contacts = 0; + fixed_dict_set(&dict_arena, &dict, manifold_key, NULL); + } + } + } +} + + + + + +INTERNAL void solve_collisions(f32 dt, b32 apply_bias) +{ + 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; + + struct entity *e0 = entity_from_handle(store, manifold->manifold_e0); + struct entity *e1 = entity_from_handle(store, manifold->manifold_e1); + + + + + + + + if (manifold->num_contacts > 0 && e0->valid && e1->valid) { + struct xform e0_xf = entity_get_xform(e0); + struct xform e1_xf = entity_get_xform(e1); + + f32 scale0 = math_fabs(xform_get_determinant(e0_xf)); + f32 scale1 = math_fabs(xform_get_determinant(e1_xf)); + f32 m0 = e0->mass_unscaled * scale0; + f32 m1 = e1->mass_unscaled * scale1; + f32 i0 = e0->inertia_unscaled * scale0; + f32 i1 = e1->inertia_unscaled * scale1; + f32 inv_m0 = 1.f / m0; + f32 inv_m1 = 1.f / m1; + f32 inv_i0 = 1.f / i0; + f32 inv_i1 = 1.f / i1; + + + struct v2 e0_accumulated_linear_impulse = { 0 }; + f32 e0_accumulated_angular_impulse = 0; + struct v2 e1_accumulated_linear_impulse = { 0 }; + f32 e1_accumulated_angular_impulse = 0; + + struct v2 normal = manifold->manifold_normal; + for (u32 contact_index = 0; contact_index < manifold->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); + + f32 bias = 0; + if (apply_bias) { + //f32 bias_factor = 0.2; + f32 bias_factor = 0.1; + + f32 bias_slop = 0.001; + //f32 bias_slop = 0.001; + //f32 bias_slop = 0.00; + + bias = (bias_factor / dt) * max((contact->depth - bias_slop), 0); + } + + + struct v2 vcp0 = v2_sub(p0, e0_xf.og); + struct v2 vcp1 = v2_sub(p1, 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 vrel = v2_sub(p1_vel, p0_vel); + + f32 vn = v2_dot(vrel, normal); + /* FIXME: Clamp accumulated */ + vn = max_f32(vn, 0); + + 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)); + + /* (to be applied along n) */ + f32 j = (vn + bias) / k; + //j = max_f32(j, 0); + + struct v2 imp = v2_mul(normal, j); + + if (!e0->test_collided) { + e0->test_collided = true; + } + + e0_accumulated_linear_impulse = v2_add(e0_accumulated_linear_impulse, imp); + e1_accumulated_linear_impulse = v2_add(e1_accumulated_linear_impulse, v2_neg(imp)); + + e0_accumulated_angular_impulse += v2_wedge(vcp0, imp); + e1_accumulated_angular_impulse += v2_wedge(vcp1, v2_neg(imp)); + + //entity_apply_linear_impulse(e0, imp, p0); + //entity_apply_linear_impulse(e1, v2_neg(imp), p1); + } + + //entity_apply_linear_impulse(e0, imp, p0); + //entity_apply_linear_impulse(e1, v2_neg(imp), p1); + + + /* TODO: Remove this (testing) */ +#if 1 + /* Divide impulses by number of contact points. */ + { + f32 inv_num_contacts = 1.f / manifold->num_contacts; + e0_accumulated_linear_impulse = v2_mul(e0_accumulated_linear_impulse, inv_num_contacts); + e1_accumulated_linear_impulse = v2_mul(e1_accumulated_linear_impulse, inv_num_contacts); + e0_accumulated_angular_impulse *= inv_num_contacts; + e1_accumulated_angular_impulse *= inv_num_contacts; + } +#endif + + entity_apply_linear_impulse_to_center(e0, e0_accumulated_linear_impulse); + entity_apply_angular_impulse(e0, e0_accumulated_angular_impulse); + entity_apply_linear_impulse_to_center(e1, e1_accumulated_linear_impulse); + entity_apply_angular_impulse(e1, e1_accumulated_angular_impulse); + } else { + entity_enable_prop(manifold, ENTITY_PROP_RELEASE); + } + } +} + + + + +INTERNAL void integrate_positions(f32 dt) +{ + struct entity_store *store = G.tick.entity_store; + + 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; + if (!entity_has_prop(ent, ENTITY_PROP_PHYSICAL)) continue; + + struct xform xf = entity_get_xform(ent); + struct v2 tick_linear_velocity = v2_mul(ent->linear_velocity, dt); + f32 tick_angular_velocity = ent->angular_velocity * dt; + xf.og = v2_add(xf.og, tick_linear_velocity); + xf = xform_rotated(xf, tick_angular_velocity); + entity_set_xform(ent, xf); + } +} + + + + + + + + + + + + + + + + + + + + + + + /* ========================== * * Update @@ -259,9 +715,12 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) f64 dt = G.tick.dt; f64 time = G.tick.time; + G.sprite_frame_scope = sprite_scope_begin(); + G.root = entity_from_handle(G.tick.entity_store, G.tick.entity_store->root); + + struct entity *root = G.root; struct entity_store *store = G.tick.entity_store; - struct entity *root = entity_from_handle(store, store->root); - struct sprite_scope *sprite_frame_scope = sprite_scope_begin(); + struct sprite_scope *sprite_frame_scope = G.sprite_frame_scope; (UNUSED)dt; (UNUSED)time; @@ -859,398 +1318,48 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) ent->torque = 0; } + + + + + + + +#if 0 /* ========================== * * Create contact manifolds * ========================== */ - for (u64 e0_index = 0; e0_index < store->reserved; ++e0_index) { - struct entity *e0 = &store->entities[e0_index]; - if (!(e0->valid && entity_has_prop(e0, ENTITY_PROP_ACTIVE))) continue; - if (!entity_has_prop(e0, ENTITY_PROP_PHYSICAL)) continue; - if (!entity_has_prop(e0, ENTITY_PROP_PLAYER_CONTROLLED) && !entity_has_prop(e0, ENTITY_PROP_BULLET)) continue; - - struct xform e0_xf = entity_get_xform(e0); - struct quad e0_quad; - struct v2_array e0_poly; - { - struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, e0->sprite); - struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("shape"), e0->animation_frame); - e0_quad = xform_mul_quad(e0->sprite_local_xform, quad_from_rect(slice.rect)); - e0_quad = xform_mul_quad(e0_xf, e0_quad); - e0_poly = (struct v2_array) { - .count = ARRAY_COUNT(e0_quad.e), - .points = e0_quad.e - }; - } - - for (u64 e1_index = 0; e1_index < store->reserved; ++e1_index) { - struct entity *e1 = &store->entities[e1_index]; - if (e1 == e0) continue; - if (!(e1->valid && entity_has_prop(e1, ENTITY_PROP_ACTIVE))) continue; - if (!entity_has_prop(e1, ENTITY_PROP_PHYSICAL)) continue; - if (entity_has_prop(e1, ENTITY_PROP_PLAYER_CONTROLLED)) continue; - - struct xform e1_xf = entity_get_xform(e1); - struct quad e1_quad; - struct v2_array e1_poly; - { - struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, e1->sprite); - struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("shape"), e1->animation_frame); - e1_quad = xform_mul_quad(e1->sprite_local_xform, quad_from_rect(slice.rect)); - e1_quad = xform_mul_quad(e1_xf, e1_quad); - e1_poly = (struct v2_array) { - .count = ARRAY_COUNT(e1_quad.e), - .points = e1_quad.e - }; - } - - - /* TODO: Remove this */ - static struct arena dict_arena = { 0 }; - static struct fixed_dict dict = { 0 }; - if (dict.buckets_count == 0) { - dict_arena = arena_alloc(GIGABYTE(64)); - dict = fixed_dict_init(&dict_arena, 4096); - } - - struct string manifold_key; - { - /* FIXME: Contact should be same regardless of entity order */ - u64 manifold_hash = hash_fnv64(HASH_FNV64_BASIS, BUFFER_FROM_STRUCT(&e0->handle)); - manifold_hash = hash_fnv64(manifold_hash, BUFFER_FROM_STRUCT(&e1->handle)); - manifold_key = STRING_FROM_BUFFER(BUFFER_FROM_STRUCT(&manifold_hash)); - } - - struct entity *manifold = fixed_dict_get(&dict, manifold_key); - - - - -#if 1 - const f32 remove_contact_threshold_global_dist_sq = 0.005 * 0.005; - const f32 insert_contact_threshold_global_dist_sq = 0.005 * 0.005; -#else - const f32 remove_contact_threshold_global_dist_sq = 0; - const f32 insert_contact_threshold_global_dist_sq = 0; -#endif - - - - struct gjk_contact_points_result res = gjk_contact_points(e0_poly, e1_poly); - if (res.num_pairs > 0) { - if (!manifold) { - manifold = entity_alloc(root); - manifold->manifold_e0 = e0->handle; - manifold->manifold_e1 = e1->handle; - /* TODO: Should we recalculate normal as more contact points are added? */ - manifold->manifold_normal = v2_norm(v2_sub(res.pairs[0].p1, res.pairs[0].p0)); - entity_enable_prop(manifold, ENTITY_PROP_MANIFOLD); - activate_now(manifold); - fixed_dict_set(&dict_arena, &dict, manifold_key, manifold); - } - - - - - /* TODO: Remove this (debugging) */ - manifold->prototype = res.prototype; - manifold->simplex = res.simplex; - - - - - - - - - /* 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; - b32 still_penetrating = v2_dot(old_normal, v2_sub(p1_world_cur, p0_world_cur)) >= 0; - - if (still_penetrating && p0_close_enough && p1_close_enough) { - /* Contact is still penetrating, mark persisted for warm starting */ - contact->persisted = true; - } 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 */ - - - /* Insert new contacts that aren't too close to original contacts */ - for (u32 i = 0; i < res.num_pairs; ++i) { - b32 should_insert = true; - - struct gjk_contact_pair new_pair = res.pairs[i]; - struct v2 p0_world = new_pair.p0; - struct v2 p1_world = new_pair.p1; - - 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; - break; - } - } - - 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++]; - } - - 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 = v2_len(v2_sub(p1_world, p0_world)); - } - } - - /* 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; - { - 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 i = 0; i < overflow_count; ++i) { - struct contact *c = &overflow[i]; - f32 depth = c->depth; - if (depth > deepest_depth) { - deepest = c; - deepest_depth = depth; - } - } - } - /* 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 */ - manifold->contacts[0] = *deepest; - manifold->contacts[1] = *furthest; - } - - - - - /* 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)); - - - - - - /* 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) { - /* No longer colliding, delete contact */ - manifold->num_contacts = 0; - fixed_dict_set(&dict_arena, &dict, manifold_key, NULL); - } - } - } + create_contact_manifolds(); /* ========================== * * Collision resolution * ========================== */ - 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; - - struct entity *e0 = entity_from_handle(store, manifold->manifold_e0); - struct entity *e1 = entity_from_handle(store, manifold->manifold_e1); - - if (manifold->num_contacts > 0 && e0->valid && e1->valid) { - struct xform e0_xf = entity_get_xform(e0); - struct xform e1_xf = entity_get_xform(e1); - - f32 scale0 = math_fabs(xform_get_determinant(e0_xf)); - f32 scale1 = math_fabs(xform_get_determinant(e1_xf)); - f32 m0 = e0->mass_unscaled * scale0; - f32 m1 = e1->mass_unscaled * scale1; - f32 i0 = e0->inertia_unscaled * scale0; - f32 i1 = e1->inertia_unscaled * scale1; - f32 inv_m0 = 1.f / m0; - f32 inv_m1 = 1.f / m1; - f32 inv_i0 = 1.f / i0; - f32 inv_i1 = 1.f / i1; - - struct v2 e0_accumulated_linear_impulse = { 0 }; - f32 e0_accumulated_angular_impulse = 0; - struct v2 e1_accumulated_linear_impulse = { 0 }; - f32 e1_accumulated_angular_impulse = 0; - - struct v2 normal = manifold->manifold_normal; - for (u32 contact_index = 0; contact_index < manifold->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); - f32 depth = contact->depth; - - #if 0 - f32 bias_factor = 0.2; - f32 bias_slop = 0.001; - //f32 bias_slop = 0.001; - //f32 bias_slop = 0.00; - - f32 bias = (bias_factor / dt) * max((depth - bias_slop), 0); - #else - (UNUSED)depth; - f32 bias = 0; - #endif - - - struct v2 vcp0 = v2_sub(p0, e0_xf.og); - struct v2 vcp1 = v2_sub(p1, 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 vrel = v2_sub(p1_vel, p0_vel); - - f32 vn = v2_dot(vrel, normal); - vn = max_f32(vn, 0); - - 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)); - - /* (to be applied along n) */ - f32 j = (vn + bias) / k; - //j = max_f32(j, 0); - - struct v2 imp = v2_mul(normal, j); - - if (!e0->test_collided) { - e0->test_collided = true; - } - - e0_accumulated_linear_impulse = v2_add(e0_accumulated_linear_impulse, imp); - e1_accumulated_linear_impulse = v2_add(e1_accumulated_linear_impulse, v2_neg(imp)); - - e0_accumulated_angular_impulse += v2_wedge(vcp0, imp); - e1_accumulated_angular_impulse += v2_wedge(vcp1, v2_neg(imp)); - - //entity_apply_linear_impulse(e0, imp, p0); - //entity_apply_linear_impulse(e1, v2_neg(imp), p1); - } - - //entity_apply_linear_impulse(e0, imp, p0); - //entity_apply_linear_impulse(e1, v2_neg(imp), p1); - - - /* TODO: Remove this (testing) */ -#if 1 - /* Divide impulses by number of contact points. */ - { - f32 inv_num_contacts = 1.f / manifold->num_contacts; - e0_accumulated_linear_impulse = v2_mul(e0_accumulated_linear_impulse, inv_num_contacts); - e1_accumulated_linear_impulse = v2_mul(e1_accumulated_linear_impulse, inv_num_contacts); - e0_accumulated_angular_impulse *= inv_num_contacts; - e1_accumulated_angular_impulse *= inv_num_contacts; - } -#endif - - entity_apply_linear_impulse_to_center(e0, e0_accumulated_linear_impulse); - entity_apply_angular_impulse(e0, e0_accumulated_angular_impulse); - entity_apply_linear_impulse_to_center(e1, e1_accumulated_linear_impulse); - entity_apply_angular_impulse(e1, e1_accumulated_angular_impulse); - } else { - entity_enable_prop(manifold, ENTITY_PROP_RELEASE); - } - } + solve_collisions(dt); /* ========================== * * Integrate positions from velocity * ========================== */ - 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; - if (!entity_has_prop(ent, ENTITY_PROP_PHYSICAL)) continue; + integrate_positions(dt); +#else + u32 iterations = 4; + for (u32 i = 0; i < iterations; ++i) { + f32 timestep = dt / iterations; - struct xform xf = entity_get_xform(ent); - struct v2 tick_linear_velocity = v2_mul(ent->linear_velocity, dt); - f32 tick_angular_velocity = ent->angular_velocity * dt; - xf.og = v2_add(xf.og, tick_linear_velocity); - xf = xform_rotated(xf, tick_angular_velocity); - entity_set_xform(ent, xf); +#if 1 + create_contact_manifolds(); + solve_collisions(timestep, true); + integrate_positions(timestep); + solve_collisions(timestep, false); +#else + create_contact_manifolds(); + solve_collisions(timestep, false); + integrate_positions(timestep); +#endif } +#endif /* ========================== * * Initialize bullet kinematics from sources diff --git a/src/gjk.c b/src/gjk.c index 25f52c4f..2cd50a06 100644 --- a/src/gjk.c +++ b/src/gjk.c @@ -156,7 +156,7 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru /* TODO: Verify epsilon */ /* FIXME: Infinite loop when epsilon too low and shapes is too far from 0 (precision issue) */ - const f32 epsilon = 0.00100; + const f32 epsilon = 0.0000100; struct gjk_simplex s = { 0 }; b32 colliding = false; struct gjk_menkowski_point *proto = NULL;