From d14207959b925437b4e1851700476189670f2087 Mon Sep 17 00:00:00 2001 From: jacob Date: Wed, 18 Sep 2024 14:46:40 -0500 Subject: [PATCH] more physics testing --- res/graphics/box.ase | 4 +- res/graphics/bullet.ase | 2 +- src/entity.h | 39 ++- src/game.c | 539 ++++++++++++++++++++-------------------- src/gjk.c | 9 +- src/math.h | 1 + src/user.c | 22 +- 7 files changed, 305 insertions(+), 311 deletions(-) diff --git a/res/graphics/box.ase b/res/graphics/box.ase index f11bbe39..1613cbe1 100644 --- a/res/graphics/box.ase +++ b/res/graphics/box.ase @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:1818fa9630b3221692c334df3a2b4054efaddd841f231a0f620820461ab65594 -size 10002 +oid sha256:fc6935736f09575dfcbcdaaa55a8fd025777d5427eb15d8ed5a76f755e0c952c +size 623 diff --git a/res/graphics/bullet.ase b/res/graphics/bullet.ase index a7641e98..4de314a3 100644 --- a/res/graphics/bullet.ase +++ b/res/graphics/bullet.ase @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:36b70469900e1316f2a10e8e500ec0f8d6e87ddfd35d52d4d21d24f3a3b7a023 +oid sha256:a96e0621cb4a5948d2ac01a5c3481e1540d28c1e820a34ede2f4f2a1d53a53ad size 478 diff --git a/src/entity.h b/src/entity.h index 58236eec..91780be9 100644 --- a/src/entity.h +++ b/src/entity.h @@ -12,7 +12,7 @@ enum entity_prop { ENTITY_PROP_RELEASE, ENTITY_PROP_PHYSICAL, - ENTITY_PROP_CONTACT, + ENTITY_PROP_MANIFOLD, ENTITY_PROP_PLAYER_CONTROLLED, ENTITY_PROP_CAMERA, @@ -62,15 +62,18 @@ struct entity_store { -struct entity_contact_point { - struct v2 local; -}; -struct entity_contact_pair { - struct entity_contact_point p0, p1; - struct v2 pen; -}; +struct contact { + struct v2 p0_local; + struct v2 p0_initial_world; + struct v2 p1_local; + struct v2 p1_initial_world; + + f32 depth; /* How far is p0 from p1 along normal */ + + b32 persisted; +}; struct entity { @@ -121,20 +124,14 @@ struct entity { /* ====================================================================== */ - /* Contact */ - - /* ENTITY_PROP_CONTACT */ - struct entity_handle contact_e0; - struct entity_handle contact_e1; - //struct entity_contact_point contact_points[4]; - struct entity_contact_pair contact_pairs[4]; - u32 num_contact_pairs; - u32 next_pair_index; - - - - + /* Manifold */ + /* ENTITY_PROP_MANIFOLD */ + struct entity_handle manifold_e0; + struct entity_handle manifold_e1; + struct v2 manifold_normal; /* Normal vector of collision from e0 -> e1 */ + struct contact contacts[2]; + u32 num_contacts; diff --git a/src/game.c b/src/game.c index 0e7d091e..a97751e3 100644 --- a/src/game.c +++ b/src/game.c @@ -125,10 +125,11 @@ INTERNAL void spawn_test_entities(void) //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); - //f32 r = PI / 4; + //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 / 2; + //f32 r = PI / 2; //f32 r = 0; f32 skew = 0; @@ -138,20 +139,22 @@ INTERNAL void spawn_test_entities(void) xf = xform_skewed_to(xf, skew); entity_set_xform(e, xf); - e->sprite = sprite_tag_from_path(STR("res/graphics/tim.ase")); + //e->sprite = sprite_tag_from_path(STR("res/graphics/tim.ase")); + e->sprite = sprite_tag_from_path(STR("res/graphics/box.ase")); //e->sprite_span_name = STR("idle.unarmed"); //e->sprite_span_name = STR("idle.one_handed"); e->sprite_span_name = STR("idle.two_handed"); entity_enable_prop(e, ENTITY_PROP_PLAYER_CONTROLLED); - e->control_force = 4500; - //e->control_force = 1200; + //e->control_force = 4500; + e->control_force = 1200; e->control_torque = 10; e->control.focus = V2(0, -1); entity_enable_prop(e, ENTITY_PROP_PHYSICAL); e->mass_unscaled = 100; - e->inertia_unscaled = 25; + //e->inertia_unscaled = 1; + e->inertia_unscaled = F32_INFINITY; e->linear_ground_friction = 1000; e->angular_ground_friction = 100; @@ -577,6 +580,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) bullet->bullet_impulse = 0.25; //bullet->bullet_impulse = 1; bullet->mass_unscaled = 0.04; + bullet->inertia_unscaled = 0.00001; entity_enable_prop(bullet, ENTITY_PROP_BULLET); } @@ -770,7 +774,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]; @@ -829,7 +833,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) #endif /* ========================== * - * Integrate forces into velocity + * Integrate velocities from forces * ========================== */ for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { @@ -856,7 +860,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) } /* ========================== * - * Create contact points + * Create contact manifolds * ========================== */ for (u64 e0_index = 0; e0_index < store->reserved; ++e0_index) { @@ -909,94 +913,210 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) dict = fixed_dict_init(&dict_arena, 4096); } - struct string contact_key; + struct string manifold_key; { /* FIXME: Contact should be same regardless of entity order */ - u64 contact_hash = hash_fnv64(HASH_FNV64_BASIS, BUFFER_FROM_STRUCT(&e0->handle)); - contact_hash = hash_fnv64(contact_hash, BUFFER_FROM_STRUCT(&e1->handle)); - contact_key = STRING_FROM_BUFFER(BUFFER_FROM_STRUCT(&contact_hash)); + 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 *contact = fixed_dict_get(&dict, contact_key); + struct entity *manifold = fixed_dict_get(&dict, manifold_key); - const f32 min_new_contact_point_dist_sq = 0.05 * 0.05; +#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 (!contact) { - contact = entity_alloc(root); - contact->contact_e0 = e0->handle; - contact->contact_e1 = e1->handle; - entity_enable_prop(contact, ENTITY_PROP_CONTACT); - activate_now(contact); - fixed_dict_set(&dict_arena, &dict, contact_key, contact); + 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: Dont need to do insertion logic for newly created contacts */ + + + + + /* 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 new_p0_world = new_pair.p0; - struct v2 new_p1_world = new_pair.p1; - struct v2 new_p0_local = xform_invert_mul_v2(e0_xf, new_p0_world); - struct v2 new_p1_local = xform_invert_mul_v2(e1_xf, new_p1_world); + struct v2 p0_world = new_pair.p0; + struct v2 p1_world = new_pair.p1; - struct v2 pen = v2_sub(new_p1_world, new_p0_world); - - i32 replace_index = -1; - for (u32 j = 0; j < contact->num_contact_pairs; ++j) { - struct entity_contact_pair old_pair = contact->contact_pairs[j]; - - struct v2 old_p0_local = old_pair.p0.local; - struct v2 old_p1_local = old_pair.p1.local; - - f32 p0_dist_sq = v2_len_sq(v2_sub(new_p0_local, old_p0_local)); - f32 p1_dist_sq = v2_len_sq(v2_sub(new_p1_local, old_p1_local)); - - if (p0_dist_sq < min_new_contact_point_dist_sq || p1_dist_sq < min_new_contact_point_dist_sq) { - replace_index = j; - //should_insert = false; + 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) { - u32 index; - if (replace_index >= 0) { - index = replace_index; - contact->next_pair_index = index + 1; - if (contact->next_pair_index >= ARRAY_COUNT(contact->contact_pairs)) { - contact->next_pair_index = 0; - } + struct contact *c; + if (manifold->num_contacts < ARRAY_COUNT(manifold->contacts)) { + c = &manifold->contacts[manifold->num_contacts++]; } else { - index = contact->num_contact_pairs; - if (index >= ARRAY_COUNT(contact->contact_pairs)) { - index = contact->next_pair_index; - if (index >= (ARRAY_COUNT(contact->contact_pairs) - 1)) { - contact->next_pair_index = 0; - } else { - ++contact->next_pair_index; - } - } else { - ++contact->num_contact_pairs; - } + c = &overflow[overflow_count++]; } - contact->contact_pairs[index] = (struct entity_contact_pair) { - .p0 = { .local = new_p0_local }, - .p1 = { .local = new_p1_local }, - .pen = pen - }; + + 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)); } } - /* TODO: Remove this (debugging) */ - contact->prototype = res.prototype; - contact->simplex = res.simplex; - } else if (contact) { + + /* 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 */ - contact->num_contact_pairs = 0; - fixed_dict_set(&dict_arena, &dict, contact_key, NULL); + manifold->num_contacts = 0; + fixed_dict_set(&dict_arena, &dict, manifold_key, NULL); } } } @@ -1006,14 +1126,14 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) * ========================== */ for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { - struct entity *contact = &store->entities[entity_index]; - if (!(contact->valid && entity_has_prop(contact, ENTITY_PROP_ACTIVE))) continue; - if (!entity_has_prop(contact, ENTITY_PROP_CONTACT)) continue; + 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, contact->contact_e0); - struct entity *e1 = entity_from_handle(store, contact->contact_e1); + struct entity *e0 = entity_from_handle(store, manifold->manifold_e0); + struct entity *e1 = entity_from_handle(store, manifold->manifold_e1); - if (contact->num_contact_pairs > 0 && e0->valid && e1->valid) { + 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); @@ -1033,24 +1153,23 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) struct v2 e1_accumulated_linear_impulse = { 0 }; f32 e1_accumulated_angular_impulse = 0; - //f32 e1_accumulated_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]; - for (u32 pair_index = 0; pair_index < contact->num_contact_pairs; ++pair_index) { - struct entity_contact_pair pair = contact->contact_pairs[pair_index]; - struct v2 p0 = xform_mul_v2(e0_xf, pair.p0.local); - struct v2 p1 = xform_mul_v2(e1_xf, pair.p1.local); - - struct v2 pen = pair.pen; - struct v2 pen_norm = v2_norm(pen); + 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.1; f32 bias_slop = 0.001; + //f32 bias_slop = 0.001; //f32 bias_slop = 0.00; - f32 bias = (bias_factor / dt) * max((v2_len(pen) - bias_slop), 0); + f32 bias = (bias_factor / dt) * max((depth - bias_slop), 0); #else + (UNUSED)depth; f32 bias = 0; #endif @@ -1063,20 +1182,19 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) struct v2 vrel = v2_sub(p1_vel, p0_vel); - f32 vn = v2_dot(vrel, pen_norm); + 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); - struct v2 idk0 = v2_perp_mul(vcp0, v2_wedge(vcp0, pen_norm) * inv_i0); - struct v2 idk1 = v2_perp_mul(vcp1, v2_wedge(vcp1, pen_norm) * inv_i1); - - f32 k = inv_m0 + inv_m1 + v2_dot(pen_norm, v2_add(idk0, idk1)); + 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(pen_norm, j); + struct v2 imp = v2_mul(normal, j); if (!e0->test_collided) { e0->test_collided = true; @@ -1088,8 +1206,6 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) 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); } @@ -1097,194 +1213,30 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) //entity_apply_linear_impulse(e0, imp, p0); //entity_apply_linear_impulse(e1, v2_neg(imp), p1); - /* FIXME: Clamp */ + + /* 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(contact, ENTITY_PROP_RELEASE); + entity_enable_prop(manifold, ENTITY_PROP_RELEASE); } } - - - - - - - - - - -#if 0 - 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 - }; - } - - b32 colliding = false; - struct gjk_contact_pair pair0 = { 0 }; - struct gjk_contact_pair pair1 = { 0 }; - u32 num_pairs = 0; - struct entity *colliding_with = entity_nil(); - struct gjk_simplex simplex = { 0 }; - struct gjk_prototype prototype = { 0 }; - struct v2 velocity = V2(0, 0); - b32 solved = false; - 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 - }; - } - - struct gjk_contact_points_result res = gjk_contact_points(e0_poly, e1_poly); - - num_pairs = res.num_pairs; - colliding = num_pairs > 0; - pair0 = res.pairs[0]; - pair1 = res.pairs[1]; - colliding_with = e1; - simplex = res.simplex; - prototype = res.prototype; - solved = res.solved; - } - - e0->colliding_with = colliding_with->handle; - e0->point0 = pair0.p0; - e0->point1 = pair1.p0; - e0->num_points = num_pairs; - e0->simplex = simplex; - e0->prototype = prototype; - e0->pendir = velocity; - e0->solved = solved; - - if (colliding_with->valid) { - colliding_with->colliding_with = e0->handle; - colliding_with->point0 = pair0.p1; - colliding_with->point1 = pair1.p1; - colliding_with->num_points = num_pairs; - } - - (UNUSED)colliding; - { -#if 0 - if (colliding) { - struct entity *e1 = colliding_with; - 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; - (UNUSED)inv_m0; - (UNUSED)inv_m1; - (UNUSED)inv_i0; - (UNUSED)inv_i1; - - struct v2 pen = v2_sub(p1, p0); - struct v2 pen_norm = v2_norm(pen); - -#if 1 - f32 bias_factor = 0.2; - f32 bias_slop = 0.001; - - f32 bias = (bias_factor / dt) * max((v2_len(pen) - bias_slop), 0); -#else - 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, pen_norm); - vn = max_f32(vn, 0); - - - struct v2 idk0 = v2_perp_mul(vcp0, v2_wedge(vcp0, pen_norm) * inv_i0); - struct v2 idk1 = v2_perp_mul(vcp1, v2_wedge(vcp1, pen_norm) * inv_i1); - - f32 k = inv_m0 + inv_m1 + v2_dot(pen_norm, v2_add(idk0, idk1)); - - /* (to be applied along n) */ - f32 j = (vn + bias) / k; - //j = max_f32(j, 0); - - - - if (j > 0) { - DEBUGBREAKABLE; - } - - if (!e0->test_collided) { - e0->test_collided = true; - } - - - - struct v2 imp = v2_mul(pen_norm, j); - (UNUSED)imp; - - //imp = v2_mul(imp, dt); - - -#if 1 - entity_apply_linear_impulse(e0, imp, p0); - entity_apply_linear_impulse(e1, v2_neg(imp), p1); -#endif - - } -#endif - } - } -#endif - /* ========================== * - * Update positions + * Integrate positions from velocity * ========================== */ for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { @@ -1330,6 +1282,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) * Update camera position * ========================== */ +#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; @@ -1366,6 +1319,44 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) ent->camera_applied_lerp_continuity_gen_plus_one = ent->camera_lerp_continuity_gen + 1; entity_set_xform(ent, xf); } +#else + 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; + + /* Camera follow */ + if (entity_has_prop(ent, ENTITY_PROP_CAMERA)) { + struct entity *follow = entity_from_handle(store, ent->camera_follow); + + struct xform xf = entity_get_xform(ent); + + f32 aspect_ratio = 1.0; + { + struct xform quad_xf = xform_mul(entity_get_xform(ent), ent->camera_quad_xform); + struct v2 camera_size = xform_get_scale(quad_xf); + if (!v2_is_zero(camera_size)) { + aspect_ratio = camera_size.x / camera_size.y; + } + } + f32 ratio_y = 0.33f; + f32 ratio_x = ratio_y / aspect_ratio; + struct v2 camera_focus_dir = v2_mul_v2(follow->control.focus, V2(ratio_x, ratio_y)); + struct v2 camera_focus_pos = v2_add(entity_get_xform(follow).og, camera_focus_dir); + ent->camera_xform_target = xf; + ent->camera_xform_target.og = camera_focus_pos; + + /* Lerp camera */ + if (ent->camera_applied_lerp_continuity_gen_plus_one == ent->camera_lerp_continuity_gen + 1) { + f32 t = 1 - math_pow(2.f, -20.f * (f32)G.tick.dt); + xf = xform_lerp(xf, ent->camera_xform_target, t); + } else { + /* Skip lerp */ + xf = ent->camera_xform_target; + } + ent->camera_applied_lerp_continuity_gen_plus_one = ent->camera_lerp_continuity_gen + 1; + entity_set_xform(ent, xf); + } +#endif } /* ========================== * diff --git a/src/gjk.c b/src/gjk.c index 85f87e69..25f52c4f 100644 --- a/src/gjk.c +++ b/src/gjk.c @@ -155,7 +155,8 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru struct gjk_contact_points_result res = { 0 }; /* TODO: Verify epsilon */ - const f32 epsilon = 0.0000100; + /* FIXME: Infinite loop when epsilon too low and shapes is too far from 0 (precision issue) */ + const f32 epsilon = 0.00100; struct gjk_simplex s = { 0 }; b32 colliding = false; struct gjk_menkowski_point *proto = NULL; @@ -236,9 +237,9 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru struct v2 rab_dir = v2_perp_towards_dir(vab, v2_neg(vac)); struct v2 rac_dir = v2_perp_towards_dir(vac, v2_neg(vab)); struct v2 rbc_dir = v2_perp_towards_dir(vbc, vab); - voronoi_mask |= (v2_dot(rab_dir, v2_neg(s.a.p)) >= 0) << 0; /* Regions ab, a, and b*/ - voronoi_mask |= (v2_dot(rac_dir, v2_neg(s.a.p)) >= 0) << 1; /* Regions ac, a, and c */ - voronoi_mask |= (v2_dot(rbc_dir, v2_neg(s.b.p)) >= 0) << 2; /* Regions bc, b, and c */ + voronoi_mask |= (v2_dot(rab_dir, v2_neg(s.a.p)) > 0) << 0; /* Regions ab, a, and b*/ + voronoi_mask |= (v2_dot(rac_dir, v2_neg(s.a.p)) > 0) << 1; /* Regions ac, a, and c */ + voronoi_mask |= (v2_dot(rbc_dir, v2_neg(s.b.p)) > 0) << 2; /* Regions bc, b, and c */ /* Remove point or edge and determine next direction based on voronoi region */ switch (voronoi_mask) { default: diff --git a/src/math.h b/src/math.h index be44d4fc..c9215a8a 100644 --- a/src/math.h +++ b/src/math.h @@ -1000,6 +1000,7 @@ INLINE struct v2 xform_basis_invert_mul_v2(struct xform xf, struct v2 v) return res; } +/* TODO: Get rid of this? Just force caller to use invert manually since it's expensive. */ INLINE struct v2 xform_invert_mul_v2(struct xform xf, struct v2 v) { struct xform inv = xform_invert(xf); diff --git a/src/user.c b/src/user.c index c6719f65..d2604192 100644 --- a/src/user.c +++ b/src/user.c @@ -989,23 +989,27 @@ INTERNAL void user_update(void) } /* Draw contact */ - if (entity_has_prop(ent, ENTITY_PROP_CONTACT)) { + if (entity_has_prop(ent, ENTITY_PROP_MANIFOLD)) { f32 radius = 5; - struct entity *e0 = entity_from_handle(store, ent->contact_e0); - struct entity *e1 = entity_from_handle(store, ent->contact_e1); + struct entity *e0 = entity_from_handle(store, ent->manifold_e0); + struct entity *e1 = entity_from_handle(store, ent->manifold_e1); struct xform e0_xf = entity_get_xform(e0); struct xform e1_xf = entity_get_xform(e1); - for (u32 i = 0; i < ent->num_contact_pairs; ++i) { - struct entity_contact_pair pair = ent->contact_pairs[i]; + (UNUSED)e0_xf; + (UNUSED)e1_xf; + for (u32 i = 0; i < ent->num_contacts; ++i) { + struct contact contact = ent->contacts[i]; { 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, pair.p0.local); + //struct v2 point = xform_mul_v2(e0_xf, contact.p0_local); + struct v2 point = contact.p0_initial_world; point = xform_mul_v2(G.world_view, point); draw_solid_circle(G.viewport_canvas, point, radius, color, 10); } { 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, pair.p1.local); + //struct v2 point = xform_mul_v2(e1_xf, contact.p1_local); + struct v2 point = contact.p1_initial_world; point = xform_mul_v2(G.world_view, point); draw_solid_circle(G.viewport_canvas, point, radius, color, 10); } @@ -1214,8 +1218,8 @@ INTERNAL void user_update(void) /* Queue player move cmd */ f32 move_speed = 1.0f; if (G.bind_states[USER_BIND_KIND_WALK].is_held) { - const f32 walk_ratio = 0.25f; - //const f32 walk_ratio = 0.01f; + //const f32 walk_ratio = 0.25f; + const f32 walk_ratio = 0.05f; move_speed *= walk_ratio; }