From b717a38b1378aff7f184d8d9439543fad38268c6 Mon Sep 17 00:00:00 2001 From: jacob Date: Tue, 17 Sep 2024 12:44:54 -0500 Subject: [PATCH] contact testing --- src/common.h | 2 +- src/entity.c | 2 +- src/entity.h | 23 +++-- src/game.c | 283 +++++++++++++++++++++++++++++++++++++++++++-------- src/user.c | 33 +++++- 5 files changed, 287 insertions(+), 56 deletions(-) diff --git a/src/common.h b/src/common.h index 2ab5dfa0..3b929e2f 100644 --- a/src/common.h +++ b/src/common.h @@ -526,7 +526,7 @@ struct sprite_tag { #define BUFFER_FROM_POINTERS(p1, p2) (CPPFRIENDLY_INITLIST_TYPE(struct buffer) { (u8 *)(p2) - (u8 *)(p1), (u8 *)p1 }) -#define BUFFER_FROM_STRUCT(ptr) (CPPFRIENDLY_INITLIST_TYPE(struct buffer) { sizeof(*(ptr)), (ptr) }) +#define BUFFER_FROM_STRUCT(ptr) (CPPFRIENDLY_INITLIST_TYPE(struct buffer) { sizeof(*(ptr)), (u8 *)(ptr) }) /* ========================== * * String utils diff --git a/src/entity.c b/src/entity.c index 1fe3777c..3c71eb36 100644 --- a/src/entity.c +++ b/src/entity.c @@ -312,7 +312,7 @@ void entity_apply_linear_impulse(struct entity *ent, struct v2 impulse, struct v struct v2 vcp = v2_sub(point, center); ent->linear_velocity = v2_add(ent->linear_velocity, v2_mul(impulse, inv_mass)); - ent->angular_velocity += -v2_wedge(impulse, vcp) * inv_inertia; + ent->angular_velocity += v2_wedge(vcp, impulse) * inv_inertia; } #if 0 diff --git a/src/entity.h b/src/entity.h index ed408e00..68626dd6 100644 --- a/src/entity.h +++ b/src/entity.h @@ -12,6 +12,7 @@ enum entity_prop { ENTITY_PROP_RELEASE, ENTITY_PROP_PHYSICAL, + ENTITY_PROP_CONTACT, ENTITY_PROP_PLAYER_CONTROLLED, ENTITY_PROP_CAMERA, @@ -60,6 +61,13 @@ struct entity_store { +struct entity_contact_point { + struct v2 local; +}; + +struct entity_contact_pair { + struct entity_contact_point p0, p1; +}; @@ -100,10 +108,6 @@ struct entity { struct v2 pendir; b32 solved; - struct v2 point0; - struct v2 point1; - u32 num_points; - b32 test_torque_applied; b32 test_collided; @@ -114,15 +118,16 @@ struct entity { -#if 0 /* ====================================================================== */ /* Contact */ - struct entity *contact_e0; - struct entity *contact_e1; - struct gjk_contact_pair contact_pairs[4]; + /* 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; -#endif + u32 next_pair_index; diff --git a/src/game.c b/src/game.c index bcc35727..2462e5c1 100644 --- a/src/game.c +++ b/src/game.c @@ -128,8 +128,8 @@ INTERNAL void spawn_test_entities(void) struct v2 size = V2(1, 1); //f32 r = PI / 4; //f32 r = PI / 3; - //f32 r = PI / 2; - f32 r = 0; + f32 r = PI / 2; + //f32 r = 0; f32 skew = 0; struct entity *e = entity_alloc(root); @@ -269,10 +269,12 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) /* TODO: remove this (testing) */ /* Initialize entities */ - static b32 run = 0; - if (!run) { - run = 1; - spawn_test_entities(); + { + static b32 run = 0; + if (!run) { + run = 1; + spawn_test_entities(); + } } /* ========================== * @@ -827,40 +829,9 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) #endif /* ========================== * - * Integrate forces + * Integrate forces into velocity * ========================== */ -#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; - if (!entity_has_prop(ent, ENTITY_PROP_PHYSICAL)) continue; - - struct xform xf = entity_get_xform(ent); - f32 det_abs = math_fabs(xform_get_determinant(xf)); - f32 mass = ent->mass_unscaled * det_abs; - f32 inertia = ent->inertia_unscaled * det_abs; - - /* Determine force & torque acceleration */ - struct v2 tick_force_accel = v2_mul(v2_div(ent->force, mass), dt); - f32 tick_torque_accel = (ent->torque / inertia) * dt; - - /* Integrate */ - /* TODO: Don't need to multiply each term by dt separately */ - struct v2 tick_linear_velocity = v2_add(v2_mul(ent->linear_velocity, dt), v2_mul(tick_force_accel, dt)); - f32 tick_angular_velocity = (ent->angular_velocity * dt) + (tick_torque_accel * dt); - - xf.og = v2_add(xf.og, tick_linear_velocity); - xf = xform_rotated(xf, tick_angular_velocity); - - ent->linear_velocity = v2_div(tick_linear_velocity, dt); - ent->angular_velocity = tick_angular_velocity / dt; - ent->force = V2(0, 0); - ent->torque = 0; - - 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; @@ -883,13 +854,243 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) ent->force = V2(0, 0); ent->torque = 0; } -#endif -#if 1 /* ========================== * - * Collision + * Create contact points * ========================== */ + 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 contact_key; + { + 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)); + } + + struct entity *contact = fixed_dict_get(&dict, contact_key); + + + + + const f32 min_new_contact_point_dist_sq = 0.05 * 0.05; + + 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); + } + /* TODO: Dont need to do insertion logic for newly created 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); + + 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; + 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; + } + } 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; + } + } + contact->contact_pairs[index] = (struct entity_contact_pair) { + .p0 = { + .local = new_p0_local + }, + .p1 = { + .local = new_p1_local + } + }; + } + } + /* TODO: Remove this (debugging) */ + contact->prototype = res.prototype; + contact->simplex = res.simplex; + } else if (contact) { + /* No longer colliding, delete contact */ + contact->num_contact_pairs = 0; + fixed_dict_set(&dict_arena, &dict, contact_key, NULL); + } + } + } + + /* ========================== * + * Collision resolution + * ========================== */ + + 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 *e0 = entity_from_handle(store, contact->contact_e0); + struct entity *e1 = entity_from_handle(store, contact->contact_e1); + + if (e0->valid && e1->valid && contact->num_contact_pairs > 0) { + 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; + + 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 = 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); + + struct v2 imp = v2_mul(pen_norm, j); + + if (!e0->test_collided) { + e0->test_collided = true; + } + + entity_apply_linear_impulse(e0, imp, p0); + entity_apply_linear_impulse(e1, v2_neg(imp), p1); + } + } else { + entity_enable_prop(contact, 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; diff --git a/src/user.c b/src/user.c index d2ff60d0..c6719f65 100644 --- a/src/user.c +++ b/src/user.c @@ -823,7 +823,7 @@ INTERNAL void user_update(void) __profscope(user_entity_iter); struct entity *ent = &store->entities[entity_index]; if (!(ent->valid && entity_has_prop(ent, ENTITY_PROP_ACTIVE))) continue; - if (sprite_tag_is_nil(ent->sprite)) continue; + //if (sprite_tag_is_nil(ent->sprite)) continue; struct sprite_tag sprite = ent->sprite; @@ -888,7 +888,8 @@ INTERNAL void user_update(void) /* Debug draw info */ if (G.debug_draw && !skip_debug_draw) { struct temp_arena temp = arena_temp_begin(scratch.arena); - b32 colliding = ent->num_points > 0; + + b32 colliding = false; #if 0 struct font *disp_font = font_load_async(STR("res/fonts/fixedsys.ttf"), 12.0f); @@ -987,13 +988,37 @@ INTERNAL void user_update(void) } } + /* Draw contact */ + if (entity_has_prop(ent, ENTITY_PROP_CONTACT)) { + f32 radius = 5; + struct entity *e0 = entity_from_handle(store, ent->contact_e0); + struct entity *e1 = entity_from_handle(store, ent->contact_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]; + { + 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); + 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); + point = xform_mul_v2(G.world_view, point); + draw_solid_circle(G.viewport_canvas, point, radius, color, 10); + } + } + } + +#if 0 /* Draw collision */ if (ent->is_top) { struct entity *e1 = entity_from_handle(store, ent->colliding_with); (UNUSED)e1; (UNUSED)colliding; -#if 1 /* Create shapes */ struct quad ent_quad; struct v2_array ent_poly; @@ -1111,7 +1136,6 @@ INTERNAL void user_update(void) draw_solid_poly_line(G.viewport_canvas, simplex_array, simplex.len > 2, thickness, line_color); } } -#endif /* Draw points */ { @@ -1128,6 +1152,7 @@ INTERNAL void user_update(void) } } } +#endif /* Draw hierarchy */ if (entity_has_prop(parent, ENTITY_PROP_ACTIVE) && !parent->is_root) {