diff --git a/src/entity.h b/src/entity.h index df9fa71a..fd079a80 100644 --- a/src/entity.h +++ b/src/entity.h @@ -9,10 +9,10 @@ enum entity_prop { ENTITY_PROP_ACTIVE, - ENTITY_PROP_RELEASE, + ENTITY_PROP_RELEASE_AT_END_OF_FRAME, ENTITY_PROP_PHYSICAL, - ENTITY_PROP_MANIFOLD, + ENTITY_PROP_CONTACT_CONSTRAINT, ENTITY_PROP_PLAYER_CONTROLLED, ENTITY_PROP_CAMERA, @@ -58,12 +58,7 @@ struct entity_store { /* TODO: Remove this */ #include "collider.h" - - - - - -struct contact { +struct contact_point { /* Contact point in local space of each entity */ struct v2 point_local_e0; struct v2 point_local_e1; @@ -80,6 +75,26 @@ struct contact { struct v2 dbg_pt; }; +struct contact_constraint { + struct entity_handle e0; + struct entity_handle e1; + + f32 inv_m0; + f32 inv_m1; + f32 inv_i0; + f32 inv_i1; + + struct v2 normal; /* Normal vector of collision from e0 -> e1 */ + u64 last_iteration; + struct contact_point points[2]; + u32 num_points; + + /* TODO: Remove this (debugging) */ + struct collider_collision_points_result res; + struct xform dbg_xf0; + struct xform dbg_xf1; +}; + struct entity { /* ====================================================================== */ @@ -114,35 +129,20 @@ struct entity { /* TODO: Remove this (testing) */ i32 colliding; - b32 test_torque_applied; + + /* ====================================================================== */ + /* Collider */ struct collider_shape local_collider; - /* ====================================================================== */ - /* 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 */ - u64 last_manifold_iteration; - struct contact contacts[2]; - u32 num_contacts; - - f32 manifold_inv_m0; - f32 manifold_inv_m1; - f32 manifold_inv_i0; - f32 manifold_inv_i1; - - /* TODO: Remove this (debugging) */ - struct collider_collision_points_result res; - struct xform dbg_xf0; - struct xform dbg_xf1; + /* Contact constraint */ + /* ENTITY_PROP_CONSTRAINT_CONTACT */ + struct contact_constraint contact_constraint; diff --git a/src/game.c b/src/game.c index e498c112..c0ae2dc0 100644 --- a/src/game.c +++ b/src/game.c @@ -136,7 +136,6 @@ INTERNAL void spawn_test_entities(f32 offset) struct entity *e = entity_alloc(root); -#if 1 //struct v2 pos = V2(0.25, -10); //struct v2 pos = V2(0.25, -7); //struct v2 pos = V2(0.25, -5.27); @@ -163,9 +162,6 @@ INTERNAL void spawn_test_entities(f32 offset) struct xform xf = XFORM_TRS(.t = pos, .r = r, .s = size); //xf.bx.y = -1.f; -#else - struct xform xf = { .bx = {0.0382978655, -0.498547733}, .by = {0.498547733, 0.0382978655}, .og = {2.01672602, -1.06180537}, }; -#endif entity_set_xform(e, xf); @@ -347,21 +343,28 @@ INTERNAL void spawn_test_entities(f32 offset) /* ========================== * - * TESTING + * TESTING MANIFOLDS / CONTACTS * ========================== */ - - INTERNAL void generate_contacts(void) { /* TODO: Remove this */ - static u64 manifold_iteration = 0; - ++manifold_iteration; + /* FIXME: Dict has leaks: Entries never removed, even when constraint is no longer valid, or either entities are no longer valid. */ + static u64 constraint_iteration = 0; + ++constraint_iteration; + static struct arena dict_arena = ZI; + static struct fixed_dict dict = ZI; + if (dict.buckets_count == 0) { + dict_arena = arena_alloc(GIGABYTE(64)); + dict = fixed_dict_init(&dict_arena, 4096); + } - /* FIXME: I think it's technically possible for manifold entities to swap between iterations */ + /* FIXME: I think it's technically possible for constraint entities to swap between iterations */ struct entity_store *store = G.tick.entity_store; struct entity *root = G.root; + +#if 0 for (u64 e0_index = 0; e0_index < store->reserved; ++e0_index) { struct entity *e0 = &store->entities[e0_index]; if (!entity_is_valid_and_active(e0)) continue; @@ -376,46 +379,38 @@ INTERNAL void generate_contacts(void) if (!entity_is_valid_and_active(e1)) continue; if (!entity_has_prop(e1, ENTITY_PROP_PHYSICAL)) continue; - /* TODO: Remove this (temporary stop to prevent double-manifold creation) */ + /* TODO: Remove this (temporary stop to prevent double-constraint creation) */ if (e0_index >= e1_index) { continue; } - /* TODO: Remove this */ - static struct arena dict_arena = ZI; - static struct fixed_dict dict = ZI; - if (dict.buckets_count == 0) { - dict_arena = arena_alloc(GIGABYTE(64)); - dict = fixed_dict_init(&dict_arena, 4096); - } - - /* Retrieve manifold */ - u64 manifold_hash; - struct string manifold_key; + /* Retrieve constraint */ + u64 constraint_hash; + struct string constraint_key; { struct entity_handle h0 = e0->handle; struct entity_handle h1 = e1->handle; - manifold_hash = hash_fnv64(HASH_FNV64_BASIS, BUFFER_FROM_STRUCT(&h0)); - manifold_hash = hash_fnv64(manifold_hash, BUFFER_FROM_STRUCT(&h1)); - manifold_key = STRING_FROM_BUFFER(BUFFER_FROM_STRUCT(&manifold_hash)); + constraint_hash = hash_fnv64(HASH_FNV64_BASIS, BUFFER_FROM_STRUCT(&h0)); + constraint_hash = hash_fnv64(constraint_hash, BUFFER_FROM_STRUCT(&h1)); + constraint_key = STRING_FROM_BUFFER(BUFFER_FROM_STRUCT(&constraint_hash)); } - struct entity *manifold = NULL; - struct entity_handle *entry = fixed_dict_get(&dict, manifold_key); + struct entity *constraint_ent = NULL; + struct entity_handle *entry = fixed_dict_get(&dict, constraint_key); if (entry) { struct entity *t = entity_from_handle(store, *entry); if (entity_is_valid_and_active(t)) { - manifold = t; + constraint_ent = t; } } - /* Ensure manifold hasn't already been computed this iteration */ - if (manifold) { - if (manifold->last_manifold_iteration == manifold_iteration) { - /* Already iterated this manifold from The other entity's perspective, skip */ + /* Ensure constraint hasn't already been computed this iteration */ + if (constraint_ent) { + if (constraint_ent->contact_constraint.last_iteration == constraint_iteration) { + /* Already iterated this constraint from The other entity's perspective, skip */ continue; } - manifold->last_manifold_iteration = manifold_iteration; + constraint_ent->contact_constraint.last_iteration = constraint_iteration; } /* Calculate entity 1 shape */ @@ -425,36 +420,36 @@ INTERNAL void generate_contacts(void) struct collider_collision_points_result res = collider_collision_points(&e0_collider, &e1_collider, e0_xf, e1_xf); /* Parts of algorithm are hard-coded to support 2 contact points */ - CT_ASSERT(ARRAY_COUNT(manifold->contacts) == 2); + CT_ASSERT(ARRAY_COUNT(constraint_ent->contact_constraint.points) == 2); CT_ASSERT(ARRAY_COUNT(res.points) == 2); /* TODO: Move this down */ if (res.num_points > 0 || COLLIDER_DEBUG) { - if (!manifold) { - manifold = entity_alloc(root); - manifold->manifold_e0 = e0->handle; - manifold->manifold_e1 = e1->handle; + if (!constraint_ent) { + constraint_ent = entity_alloc(root); + constraint_ent->contact_constraint.e1 = e1->handle; + constraint_ent->contact_constraint.e0 = e0->handle; /* TODO: Should we recalculate normal as more contact points are added? */ - entity_enable_prop(manifold, ENTITY_PROP_MANIFOLD); - activate_now(manifold); + entity_enable_prop(constraint_ent, ENTITY_PROP_CONTACT_CONSTRAINT); + activate_now(constraint_ent); if (entry) { - *entry = manifold->handle; + *entry = constraint_ent->handle; } else { entry = arena_push(&dict_arena, struct entity_handle); - *entry = manifold->handle; - fixed_dict_set(&dict_arena, &dict, manifold_key, entry); + *entry = constraint_ent->handle; + fixed_dict_set(&dict_arena, &dict, constraint_key, entry); } } - manifold->manifold_normal = res.normal; + data->normal = res.normal; /* TODO: Remove this (debugging) */ #if COLLIDER_DEBUG { - manifold->res = res; - manifold->dbg_xf0 = e0_xf; - manifold->dbg_xf1 = e1_xf; - if (manifold->num_contacts == 0) { + constraint->res = res; + constraint->dbg_xf0 = e0_xf; + constraint->dbg_xf1 = e1_xf; + if (constraint->num_points == 0) { if (res.num_points > 0) { ++e0->colliding; ++e1->colliding; @@ -474,7 +469,7 @@ INTERNAL void generate_contacts(void) struct v2 tangent = v2_perp(normal); /* TODO: Cache this */ - /* Prepare manifold masses */ + /* Prepare constraint masses */ f32 inv_m0; f32 inv_m1; f32 inv_i0; @@ -486,15 +481,15 @@ INTERNAL void generate_contacts(void) inv_m1 = 1.f / (e1->mass_unscaled * scale1); inv_i0 = 1.f / (e0->inertia_unscaled * scale0); inv_i1 = 1.f / (e1->inertia_unscaled * scale1); - manifold->manifold_inv_m0 = inv_m0; - manifold->manifold_inv_m1 = inv_m1; - manifold->manifold_inv_i0 = inv_i0; - manifold->manifold_inv_i1 = inv_i1; + data->inv_m0 = inv_m0; + data->inv_m1 = inv_m1; + data->inv_i0 = inv_i0; + data->inv_i1 = inv_i1; } /* Delete old contacts that are no longer present */ - for (u32 i = 0; i < manifold->num_contacts; ++i) { - struct contact *old = &manifold->contacts[i]; + for (u32 i = 0; i < constraint->num_points; ++i) { + struct contact *old = &constraint->contacts[i]; u32 id = old->id; b32 found = false; for (u32 j = 0; j < res.num_points; ++j) { @@ -505,7 +500,7 @@ INTERNAL void generate_contacts(void) } if (!found) { /* Delete contact by replacing with last in array */ - *old = manifold->contacts[--manifold->num_contacts]; + *old = constraint->contacts[--constraint->num_points]; --i; } } @@ -518,8 +513,8 @@ INTERNAL void generate_contacts(void) u32 id = res_point->id; struct contact *contact = NULL; /* Match */ - for (u32 j = 0; j < manifold->num_contacts; ++j) { - struct contact *t = &manifold->contacts[j]; + for (u32 j = 0; j < constraint->num_points; ++j) { + struct contact *t = &constraint->contacts[j]; if (t->id == id) { contact = t; break; @@ -533,7 +528,7 @@ INTERNAL void generate_contacts(void) #endif } else { /* Insert new */ - contact = &manifold->contacts[manifold->num_contacts++]; + contact = &constraint->contacts[constraint->num_points++]; MEMZERO_STRUCT(contact); contact->id = id; } @@ -568,46 +563,259 @@ INTERNAL void generate_contacts(void) contact->inv_tangent_mass = k > 0.0f ? 1.0f / k : 0.0f; } } - - } - } else if (manifold) { + } else if (constraint) { #if COLLIDER_DEBUG - manifold->num_contacts = 0; + constraint->num_points = 0; #else - /* No longer colliding, delete manifold */ - manifold->num_contacts = 0; - entity_enable_prop(manifold, ENTITY_PROP_RELEASE); + /* No longer colliding, delete constraint */ + constraint->num_points = 0; + entity_enable_prop(constraint, ENTITY_PROP_RELEASE_AT_END_OF_FRAME); #endif } } } +#else + for (u64 e0_index = 0; e0_index < store->reserved; ++e0_index) { + struct entity *e0 = &store->entities[e0_index]; + if (!entity_is_valid_and_active(e0)) continue; + if (!entity_has_prop(e0, ENTITY_PROP_PHYSICAL)) continue; + + struct xform e0_xf = entity_get_xform(e0); + struct collider_shape e0_collider = e0->local_collider; + + for (u64 e1_index = 0; e1_index < store->reserved; ++e1_index) { + struct entity *e1 = &store->entities[e1_index]; + if (e1 == e0) continue; + if (!entity_is_valid_and_active(e1)) continue; + if (!entity_has_prop(e1, ENTITY_PROP_PHYSICAL)) continue; + + /* TODO: Remove this (temporary stop to prevent double-constraint creation) */ + if (e0_index >= e1_index) { + continue; + } + + u64 constraint_hash; + struct string constraint_key; + struct entity_handle *entry; + struct entity *constraint = NULL; + { + { + struct entity_handle h0 = e0->handle; + struct entity_handle h1 = e1->handle; + constraint_hash = hash_fnv64(HASH_FNV64_BASIS, BUFFER_FROM_STRUCT(&h0)); + constraint_hash = hash_fnv64(constraint_hash, BUFFER_FROM_STRUCT(&h1)); + constraint_key = STRING_FROM_BUFFER(BUFFER_FROM_STRUCT(&constraint_hash)); + } + + entry = fixed_dict_get(&dict, constraint_key); + if (entry) { + struct entity *t = entity_from_handle(store, *entry); + if (entity_is_valid_and_active(t)) { + if (t->contact_constraint.last_iteration == constraint_iteration) { + /* Constraint has already been computed this iteration */ + continue; + } else { + t->contact_constraint.last_iteration = constraint_iteration; + constraint = t; + } + } else { + /* Constraint entity no longer valid */ + continue; + } + } + } + + /* Calculate collision */ + struct xform e1_xf = entity_get_xform(e1); + struct collider_collision_points_result res = collider_collision_points(&e0_collider, &e1->local_collider, e0_xf, e1_xf); + + /* Parts of algorithm are hard-coded to support 2 contact points */ + CT_ASSERT(ARRAY_COUNT(constraint->contact_constraint.points) == 2); + CT_ASSERT(ARRAY_COUNT(res.points) == 2); + + + /* TODO: Move this down */ + if (res.num_points > 0 || COLLIDER_DEBUG) { + if (!constraint) { + constraint = entity_alloc(root); + constraint->contact_constraint.e1 = e1->handle; + constraint->contact_constraint.e0 = e0->handle; + /* TODO: Should we recalculate normal as more contact points are added? */ + entity_enable_prop(constraint, ENTITY_PROP_CONTACT_CONSTRAINT); + activate_now(constraint); + if (entry) { + *entry = constraint->handle; + } else { + entry = arena_push(&dict_arena, struct entity_handle); + *entry = constraint->handle; + fixed_dict_set(&dict_arena, &dict, constraint_key, entry); + } + } + struct contact_constraint *data = &constraint->contact_constraint; + data->normal = res.normal; + + /* TODO: Remove this (debugging) */ +#if COLLIDER_DEBUG + { + data->res = res; + data->dbg_xf0 = e0_xf; + data->dbg_xf1 = e1_xf; + if (data->num_points == 0) { + if (res.num_points > 0) { + ++e0->colliding; + ++e1->colliding; + } + } else { + if (res.num_points == 0) { + --e0->colliding; + --e1->colliding; + } + } + } +#endif + } + + if (res.num_points > 0) { + struct contact_constraint *data = &constraint->contact_constraint; + + struct v2 normal = res.normal; + struct v2 tangent = v2_perp(normal); + + /* TODO: Cache this */ + /* Prepare constraint masses */ + f32 inv_m0; + f32 inv_m1; + f32 inv_i0; + f32 inv_i1; + { + f32 scale0 = math_fabs(xform_get_determinant(e0_xf)); + f32 scale1 = math_fabs(xform_get_determinant(e1_xf)); + inv_m0 = 1.f / (e0->mass_unscaled * scale0); + inv_m1 = 1.f / (e1->mass_unscaled * scale1); + inv_i0 = 1.f / (e0->inertia_unscaled * scale0); + inv_i1 = 1.f / (e1->inertia_unscaled * scale1); + data->inv_m0 = inv_m0; + data->inv_m1 = inv_m1; + data->inv_i0 = inv_i0; + data->inv_i1 = inv_i1; + } + + /* Delete old contacts that are no longer present */ + for (u32 i = 0; i < data->num_points; ++i) { + struct contact_point *old = &data->points[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; + } + } + if (!found) { + /* Delete contact by replacing with last in array */ + *old = data->points[--data->num_points]; + --i; + } + } + + /* Update / insert returned contacts */ + for (u32 i = 0; i < res.num_points; ++i) { + struct collider_collision_point *res_point = &res.points[i]; + struct v2 point = res_point->point; + f32 sep = res_point->separation; + u32 id = res_point->id; + struct contact_point *contact = NULL; + /* Match */ + for (u32 j = 0; j < data->num_points; ++j) { + struct contact_point *t = &data->points[j]; + if (t->id == id) { + contact = t; + break; + } + } + if (contact) { + /* Update existing */ +#if !GAME_PHYSICS_ENABLE_WARM_STARTING + contact->normal_impulse = 0; + contact->tangent_impulse = 0; +#endif + } else { + /* Insert new */ + contact = &data->points[data->num_points++]; + MEMZERO_STRUCT(contact); + contact->id = id; + } + contact->point_local_e0 = xform_invert_mul_v2(e0_xf, point); + contact->point_local_e1 = xform_invert_mul_v2(e1_xf, point); + contact->starting_separation = sep; + + /* TODO: Remove this (debugging) */ +#if COLLIDER_DEBUG + { + contact->dbg_pt = point; + } +#endif + + { + struct v2 vcp0 = v2_sub(point, e0_xf.og); + struct v2 vcp1 = v2_sub(point, e1_xf.og); + + /* Normal mass */ + { + f32 vcp0_wedge = v2_wedge(vcp0, normal); + f32 vcp1_wedge = v2_wedge(vcp1, normal); + f32 k = (inv_m0 + inv_m1) + (inv_i0 * vcp0_wedge * vcp0_wedge) + (inv_i1 * vcp1_wedge * vcp1_wedge); + contact->inv_normal_mass = k > 0.0f ? 1.0f / k : 0.0f; + } + + /* Tangent mass */ + { + f32 vcp0_wedge = v2_wedge(vcp0, tangent); + f32 vcp1_wedge = v2_wedge(vcp1, tangent); + f32 k = (inv_m0 + inv_m1) + (inv_i0 * vcp0_wedge * vcp0_wedge) + (inv_i1 * vcp1_wedge * vcp1_wedge); + contact->inv_tangent_mass = k > 0.0f ? 1.0f / k : 0.0f; + } + } + } + + + } else if (constraint) { + constraint->contact_constraint.num_points= 0; +#if !COLLIDER_DEBUG + /* No longer colliding, delete constraint */ + entity_enable_prop(constraint, ENTITY_PROP_RELEASE_AT_END_OF_FRAME); +#endif + } + } + } +#endif } - - INTERNAL void warm_start_contacts(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 (!entity_is_valid_and_active(manifold)) continue; - if (!entity_has_prop(manifold, ENTITY_PROP_MANIFOLD)) continue; + struct entity *constraint = &store->entities[entity_index]; + if (!entity_is_valid_and_active(constraint)) continue; + if (!entity_has_prop(constraint, ENTITY_PROP_CONTACT_CONSTRAINT)) continue; - u32 num_contacts = manifold->num_contacts; - struct entity *e0 = entity_from_handle(store, manifold->manifold_e0); - struct entity *e1 = entity_from_handle(store, manifold->manifold_e1); - if (num_contacts > 0 && entity_is_valid_and_active(e0) && entity_is_valid_and_active(e1)) { + struct contact_constraint *data = &constraint->contact_constraint; + + u32 num_points = data->num_points; + struct entity *e0 = entity_from_handle(store, data->e0); + struct entity *e1 = entity_from_handle(store, data->e1); + if (num_points > 0 && entity_is_valid_and_active(e0) && entity_is_valid_and_active(e1)) { struct xform e0_xf = entity_get_xform(e0); struct xform e1_xf = entity_get_xform(e1); - f32 inv_m0 = manifold->manifold_inv_m0; - f32 inv_m1 = manifold->manifold_inv_m1; - f32 inv_i0 = manifold->manifold_inv_i0; - f32 inv_i1 = manifold->manifold_inv_i1; + f32 inv_m0 = data->inv_m0; + f32 inv_m1 = data->inv_m1; + f32 inv_i0 = data->inv_i0; + f32 inv_i1 = data->inv_i1; struct v2 v0 = e0->linear_velocity; struct v2 v1 = e1->linear_velocity; @@ -615,18 +823,18 @@ INTERNAL void warm_start_contacts(void) f32 w1 = e1->angular_velocity; /* Warm start */ - struct v2 normal = manifold->manifold_normal; + struct v2 normal = data->normal; struct v2 tangent = v2_perp(normal); - f32 inv_num_contacts = 1.f / num_contacts; - for (u32 i = 0; i < num_contacts; ++i) { - struct contact *contact = &manifold->contacts[i]; - struct v2 p0 = xform_mul_v2(e0_xf, contact->point_local_e0); - struct v2 p1 = xform_mul_v2(e1_xf, contact->point_local_e1); + f32 inv_num_points = 1.f / num_points; + for (u32 i = 0; i < num_points; ++i) { + struct contact_point *point = &data->points[i]; + struct v2 p0 = xform_mul_v2(e0_xf, point->point_local_e0); + struct v2 p1 = xform_mul_v2(e1_xf, point->point_local_e1); struct v2 vcp0 = v2_sub(p0, e0_xf.og); struct v2 vcp1 = v2_sub(p1, e1_xf.og); - struct v2 impulse = v2_add(v2_mul(normal, contact->normal_impulse), v2_mul(tangent, contact->tangent_impulse)); - impulse = v2_mul(impulse, inv_num_contacts); + struct v2 impulse = v2_add(v2_mul(normal, point->normal_impulse), v2_mul(tangent, point->tangent_impulse)); + impulse = v2_mul(impulse, inv_num_points); v0 = v2_sub(v0, v2_mul(impulse, inv_m0)); v1 = v2_add(v1, v2_mul(impulse, inv_m1)); @@ -642,11 +850,6 @@ INTERNAL void warm_start_contacts(void) } } - - - - - struct soft_result { f32 bias_rate; f32 mass_scale; f32 impulse_scale; }; INTERNAL struct soft_result make_soft(f32 hertz, f32 zeta, f32 h) { @@ -669,12 +872,14 @@ INTERNAL void solve_contacts(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 (!entity_is_valid_and_active(manifold)) continue; - if (!entity_has_prop(manifold, ENTITY_PROP_MANIFOLD)) continue; + struct entity *constraint = &store->entities[entity_index]; + if (!entity_is_valid_and_active(constraint)) continue; + if (!entity_has_prop(constraint, ENTITY_PROP_CONTACT_CONSTRAINT)) continue; - struct entity *e0 = entity_from_handle(store, manifold->manifold_e0); - struct entity *e1 = entity_from_handle(store, manifold->manifold_e1); + struct contact_constraint *data = &constraint->contact_constraint; + + struct entity *e0 = entity_from_handle(store, data->e0); + struct entity *e1 = entity_from_handle(store, data->e1); struct v2 v0 = e0->linear_velocity; struct v2 v1 = e1->linear_velocity; @@ -682,26 +887,26 @@ INTERNAL void solve_contacts(f32 dt, b32 apply_bias) f32 w0 = e0->angular_velocity; f32 w1 = e1->angular_velocity; - u32 num_contacts = manifold->num_contacts; - if (num_contacts > 0 && entity_is_valid_and_active(e0) && entity_is_valid_and_active(e1)) { + u32 num_points = data->num_points; + if (num_points > 0 && entity_is_valid_and_active(e0) && entity_is_valid_and_active(e1)) { struct xform e0_xf = entity_get_xform(e0); struct xform e1_xf = entity_get_xform(e1); - f32 inv_m0 = manifold->manifold_inv_m0; - f32 inv_m1 = manifold->manifold_inv_m1; - f32 inv_i0 = manifold->manifold_inv_i0; - f32 inv_i1 = manifold->manifold_inv_i1; + f32 inv_m0 = data->inv_m0; + f32 inv_m1 = data->inv_m1; + f32 inv_i0 = data->inv_i0; + f32 inv_i1 = data->inv_i1; /* Normal impulse */ - struct v2 normal = manifold->manifold_normal; - 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->point_local_e0); - struct v2 p1 = xform_mul_v2(e1_xf, contact->point_local_e1); + struct v2 normal = data->normal; + for (u32 point_index = 0; point_index < num_points; ++point_index) { + struct contact_point *point = &data->points[point_index]; + struct v2 p0 = xform_mul_v2(e0_xf, point->point_local_e0); + struct v2 p1 = xform_mul_v2(e1_xf, point->point_local_e1); struct v2 vcp0 = v2_sub(p0, e0_xf.og); struct v2 vcp1 = v2_sub(p1, e1_xf.og); - f32 separation = v2_dot(v2_sub(p1, p0), normal) + contact->starting_separation; + f32 separation = v2_dot(v2_sub(p1, p0), normal) + point->starting_separation; f32 velocity_bias = 0.0f; f32 mass_scale = 1.0f; @@ -729,16 +934,16 @@ INTERNAL void solve_contacts(f32 dt, b32 apply_bias) struct v2 vel1 = v2_add(v1, v2_perp_mul(vcp1, w1)); struct v2 vrel = v2_sub(vel0, vel1); - f32 k = contact->inv_normal_mass; + f32 k = point->inv_normal_mass; /* (to be applied along n) */ f32 vn = v2_dot(vrel, normal); - f32 j = ((k * mass_scale) * (vn - velocity_bias)) - (contact->normal_impulse * impulse_scale); + f32 j = ((k * mass_scale) * (vn - velocity_bias)) - (point->normal_impulse * impulse_scale); - f32 old_impulse = contact->normal_impulse; + f32 old_impulse = point->normal_impulse; f32 new_impulse = max_f32(old_impulse + j, 0); f32 delta = new_impulse - old_impulse; - contact->normal_impulse = new_impulse; + point->normal_impulse = new_impulse; struct v2 impulse = v2_mul(normal, delta); v0 = v2_sub(v0, v2_mul(impulse, inv_m0)); @@ -749,10 +954,10 @@ INTERNAL void solve_contacts(f32 dt, b32 apply_bias) /* Tangent impulse */ struct v2 tangent = v2_perp(normal); - 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->point_local_e0); - struct v2 p1 = xform_mul_v2(e1_xf, contact->point_local_e1); + for (u32 point_index = 0; point_index < num_points; ++point_index) { + struct contact_point *point = &data->points[point_index]; + struct v2 p0 = xform_mul_v2(e0_xf, point->point_local_e0); + struct v2 p1 = xform_mul_v2(e1_xf, point->point_local_e1); struct v2 vcp0 = v2_sub(p0, e0_xf.og); struct v2 vcp1 = v2_sub(p1, e1_xf.og); @@ -760,7 +965,7 @@ INTERNAL void solve_contacts(f32 dt, b32 apply_bias) struct v2 vel1 = v2_add(v1, v2_perp_mul(vcp1, w1)); struct v2 vrel = v2_sub(vel0, vel1); - f32 k = contact->inv_tangent_mass; + f32 k = point->inv_tangent_mass; /* (to be applied along t) */ f32 vt = v2_dot(vrel, tangent); @@ -769,11 +974,11 @@ INTERNAL void solve_contacts(f32 dt, b32 apply_bias) f32 friction = 0.6f; //f32 friction = 1.0f; //f32 friction = F32_INFINITY; - f32 max_friction = friction * contact->normal_impulse; - f32 old_impulse = contact->tangent_impulse; + f32 max_friction = friction * point->normal_impulse; + f32 old_impulse = point->tangent_impulse; f32 new_impulse = clamp_f32(old_impulse + j, -max_friction, max_friction); f32 delta = new_impulse - old_impulse; - contact->tangent_impulse = new_impulse; + point->tangent_impulse = new_impulse; struct v2 impulse = v2_mul(tangent, delta); v0 = v2_sub(v0, v2_mul(impulse, inv_m0)); @@ -791,6 +996,23 @@ INTERNAL void solve_contacts(f32 dt, b32 apply_bias) } + +/* ========================== * + * TESTING MOTOR JOINT + * ========================== */ + + + + + + + + + +/* ========================== * + * TESTING PHYSICS INTEGRATION + * ========================== */ + INTERNAL void integrate_velocities_from_forces(f32 dt) { struct entity_store *store = G.tick.entity_store; @@ -1644,7 +1866,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) struct entity *ent = &store->entities[entity_index]; if (!ent->valid) continue; - if (entity_has_prop(ent, ENTITY_PROP_RELEASE)) { + if (entity_has_prop(ent, ENTITY_PROP_RELEASE_AT_END_OF_FRAME)) { *arena_push(temp.arena, struct entity *) = ent; ++ents_to_release_count; } diff --git a/src/user.c b/src/user.c index 702f901c..875fcbbe 100644 --- a/src/user.c +++ b/src/user.c @@ -1034,9 +1034,10 @@ INTERNAL void user_update(void) /* Draw collision */ #if 1 - if (entity_has_prop(ent, ENTITY_PROP_MANIFOLD)) { - struct entity *e0 = entity_from_handle(store, ent->manifold_e0); - struct entity *e1 = entity_from_handle(store, ent->manifold_e1); + if (entity_has_prop(ent, ENTITY_PROP_CONTACT_CONSTRAINT)) { + struct contact_constraint *data = &ent->contact_constraint; + struct entity *e0 = entity_from_handle(store, data->e0); + struct entity *e1 = entity_from_handle(store, data->e1); struct collider_shape e0_collider = e0->local_collider; struct collider_shape e1_collider = e1->local_collider; (UNUSED)e0_collider; @@ -1044,8 +1045,8 @@ INTERNAL void user_update(void) //struct xform e0_xf = entity_get_xform(e0); //struct xform e1_xf = entity_get_xform(e1); - struct xform e0_xf = ent->dbg_xf0; - struct xform e1_xf = ent->dbg_xf1; + struct xform e0_xf = data->dbg_xf0; + struct xform e1_xf = data->dbg_xf1; (UNUSED)e0_xf; (UNUSED)e1_xf; @@ -1177,14 +1178,14 @@ INTERNAL void user_update(void) /* Draw contacts */ { f32 radius = 5; - for (u32 i = 0; i < ent->num_contacts; ++i) { - struct contact contact = ent->contacts[i]; + for (u32 i = 0; i < ent->contact_constraint.num_points; ++i) { + struct contact_point point = ent->contact_constraint.points[i]; #if 0 struct v2 p0 = xform_mul_v2(e0_xf, contact.point_local_e0); struct v2 p1 = xform_mul_v2(e1_xf, contact.point_local_e1); struct v2 point = v2_add(p0, v2_mul(v2_sub(p1, p0), 0.5f)); #else - struct v2 point = contact.dbg_pt; + struct v2 dbg_pt = point.dbg_pt; #endif /* Draw point */ { @@ -1192,7 +1193,7 @@ INTERNAL void user_update(void) u32 color = RGBA_32_F(1, 1, 0, 0.50); //struct v2 point = xform_mul_v2(e0_xf, contact.p0_local); //struct v2 point = contact.p0_initial_world; - draw_solid_circle(G.viewport_canvas, xform_mul_v2(G.world_view, point), radius, color, 10); + draw_solid_circle(G.viewport_canvas, xform_mul_v2(G.world_view, dbg_pt), radius, color, 10); } /* Draw normal */ { @@ -1200,8 +1201,8 @@ INTERNAL void user_update(void) f32 len = 0.1f; f32 arrow_thickness = 2; f32 arrow_height = 5; - struct v2 start = xform_mul_v2(G.world_view, point); - struct v2 end = xform_mul_v2(G.world_view, v2_add(point, v2_mul(v2_norm(ent->manifold_normal), len))); + struct v2 start = xform_mul_v2(G.world_view, dbg_pt); + struct v2 end = xform_mul_v2(G.world_view, v2_add(dbg_pt, v2_mul(v2_norm(ent->contact_constraint.normal), len))); draw_solid_arrow_line(G.viewport_canvas, start, end, arrow_thickness, arrow_height, color); } #if 0 @@ -1262,16 +1263,17 @@ INTERNAL void user_update(void) u32 color_line = RGBA_32_F(1, 0, 1, 0.5); u32 color_a = RGBA_32_F(1, 0, 0, 0.5); u32 color_b = RGBA_32_F(0, 1, 0, 0.5); + struct collider_collision_points_result res = ent->contact_constraint.res; { - struct v2 a = xform_mul_v2(G.world_view, ent->res.a0); - struct v2 b = xform_mul_v2(G.world_view, ent->res.b0); + struct v2 a = xform_mul_v2(G.world_view, res.a0); + struct v2 b = xform_mul_v2(G.world_view, res.b0); draw_solid_line(G.viewport_canvas, a, b, thickness, color_line); draw_solid_circle(G.viewport_canvas, a, radius, color_a, 10); draw_solid_circle(G.viewport_canvas, b, radius, color_b, 10); } { - struct v2 a = xform_mul_v2(G.world_view, ent->res.a1); - struct v2 b = xform_mul_v2(G.world_view, ent->res.b1); + struct v2 a = xform_mul_v2(G.world_view, res.a1); + struct v2 b = xform_mul_v2(G.world_view, res.b1); draw_solid_line(G.viewport_canvas, a, b, thickness, color_line); draw_solid_circle(G.viewport_canvas, a, radius, color_a, 10); draw_solid_circle(G.viewport_canvas, b, radius, color_b, 10);