jacob/power_play
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

move manifold data into 'contact_constraint' struct

Browse Source
This commit is contained in:
jacob 2024-10-23 16:35:09 -05:00
parent 59b48694e8
commit 68d80de75a
3 changed files with 390 additions and 166 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
src/entity.h
View File

@ -9,10 +9,10 @@ enum entity_prop {
ENTITY_PROP_ACTIVE, ENTITY_PROP_ACTIVE,
ENTITY_PROP_RELEASE, ENTITY_PROP_RELEASE_AT_END_OF_FRAME,
ENTITY_PROP_PHYSICAL, ENTITY_PROP_PHYSICAL,
ENTITY_PROP_MANIFOLD, ENTITY_PROP_CONTACT_CONSTRAINT,
ENTITY_PROP_PLAYER_CONTROLLED, ENTITY_PROP_PLAYER_CONTROLLED,
ENTITY_PROP_CAMERA, ENTITY_PROP_CAMERA,
@ -58,12 +58,7 @@ struct entity_store {
/* TODO: Remove this */ /* TODO: Remove this */
#include "collider.h" #include "collider.h"
struct contact_point {
struct contact {
/* Contact point in local space of each entity */ /* Contact point in local space of each entity */
struct v2 point_local_e0; struct v2 point_local_e0;
struct v2 point_local_e1; struct v2 point_local_e1;
@ -80,6 +75,26 @@ struct contact {
struct v2 dbg_pt; 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 { struct entity {
/* ====================================================================== */ /* ====================================================================== */
@ -114,35 +129,20 @@ struct entity {
/* TODO: Remove this (testing) */ /* TODO: Remove this (testing) */
i32 colliding; i32 colliding;
b32 test_torque_applied;
/* ====================================================================== */
/* Collider */
struct collider_shape local_collider; struct collider_shape local_collider;
/* ====================================================================== */ /* ====================================================================== */
/* Manifold */ /* Contact constraint */
/* 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;
/* ENTITY_PROP_CONSTRAINT_CONTACT */
struct contact_constraint contact_constraint;

466
src/game.c
View File

@ -136,7 +136,6 @@ INTERNAL void spawn_test_entities(f32 offset)
struct entity *e = entity_alloc(root); struct entity *e = entity_alloc(root);
#if 1
//struct v2 pos = V2(0.25, -10); //struct v2 pos = V2(0.25, -10);
//struct v2 pos = V2(0.25, -7); //struct v2 pos = V2(0.25, -7);
//struct v2 pos = V2(0.25, -5.27); //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); struct xform xf = XFORM_TRS(.t = pos, .r = r, .s = size);
//xf.bx.y = -1.f; //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); 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) INTERNAL void generate_contacts(void)
{ {
/* TODO: Remove this */ /* TODO: Remove this */
static u64 manifold_iteration = 0; /* FIXME: Dict has leaks: Entries never removed, even when constraint is no longer valid, or either entities are no longer valid. */
++manifold_iteration; 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_store *store = G.tick.entity_store;
struct entity *root = G.root; struct entity *root = G.root;
#if 0
for (u64 e0_index = 0; e0_index < store->reserved; ++e0_index) { for (u64 e0_index = 0; e0_index < store->reserved; ++e0_index) {
struct entity *e0 = &store->entities[e0_index]; struct entity *e0 = &store->entities[e0_index];
if (!entity_is_valid_and_active(e0)) continue; 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_is_valid_and_active(e1)) continue;
if (!entity_has_prop(e1, ENTITY_PROP_PHYSICAL)) 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) { if (e0_index >= e1_index) {
continue; continue;
} }
/* TODO: Remove this */ /* Retrieve constraint */
static struct arena dict_arena = ZI; u64 constraint_hash;
static struct fixed_dict dict = ZI; struct string constraint_key;
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;
{ {
struct entity_handle h0 = e0->handle; struct entity_handle h0 = e0->handle;
struct entity_handle h1 = e1->handle; struct entity_handle h1 = e1->handle;
manifold_hash = hash_fnv64(HASH_FNV64_BASIS, BUFFER_FROM_STRUCT(&h0)); constraint_hash = hash_fnv64(HASH_FNV64_BASIS, BUFFER_FROM_STRUCT(&h0));
manifold_hash = hash_fnv64(manifold_hash, BUFFER_FROM_STRUCT(&h1)); constraint_hash = hash_fnv64(constraint_hash, BUFFER_FROM_STRUCT(&h1));
manifold_key = STRING_FROM_BUFFER(BUFFER_FROM_STRUCT(&manifold_hash)); constraint_key = STRING_FROM_BUFFER(BUFFER_FROM_STRUCT(&constraint_hash));
} }
struct entity *manifold = NULL; struct entity *constraint_ent = NULL;
struct entity_handle *entry = fixed_dict_get(&dict, manifold_key); struct entity_handle *entry = fixed_dict_get(&dict, constraint_key);
if (entry) { if (entry) {
struct entity *t = entity_from_handle(store, *entry); struct entity *t = entity_from_handle(store, *entry);
if (entity_is_valid_and_active(t)) { if (entity_is_valid_and_active(t)) {
manifold = t; constraint_ent = t;
} }
} }
/* Ensure manifold hasn't already been computed this iteration */ /* Ensure constraint hasn't already been computed this iteration */
if (manifold) { if (constraint_ent) {
if (manifold->last_manifold_iteration == manifold_iteration) { if (constraint_ent->contact_constraint.last_iteration == constraint_iteration) {
/* Already iterated this manifold from The other entity's perspective, skip */ /* Already iterated this constraint from The other entity's perspective, skip */
continue; continue;
} }
manifold->last_manifold_iteration = manifold_iteration; constraint_ent->contact_constraint.last_iteration = constraint_iteration;
} }
/* Calculate entity 1 shape */ /* 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); 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 */ /* 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); CT_ASSERT(ARRAY_COUNT(res.points) == 2);
/* TODO: Move this down */ /* TODO: Move this down */
if (res.num_points > 0 || COLLIDER_DEBUG) { if (res.num_points > 0 || COLLIDER_DEBUG) {
if (!manifold) { if (!constraint_ent) {
manifold = entity_alloc(root); constraint_ent = entity_alloc(root);
manifold->manifold_e0 = e0->handle; constraint_ent->contact_constraint.e1 = e1->handle;
manifold->manifold_e1 = e1->handle; constraint_ent->contact_constraint.e0 = e0->handle;
/* TODO: Should we recalculate normal as more contact points are added? */ /* TODO: Should we recalculate normal as more contact points are added? */
entity_enable_prop(manifold, ENTITY_PROP_MANIFOLD); entity_enable_prop(constraint_ent, ENTITY_PROP_CONTACT_CONSTRAINT);
activate_now(manifold); activate_now(constraint_ent);
if (entry) { if (entry) {
*entry = manifold->handle; *entry = constraint_ent->handle;
} else { } else {
entry = arena_push(&dict_arena, struct entity_handle); entry = arena_push(&dict_arena, struct entity_handle);
*entry = manifold->handle; *entry = constraint_ent->handle;
fixed_dict_set(&dict_arena, &dict, manifold_key, entry); fixed_dict_set(&dict_arena, &dict, constraint_key, entry);
} }
} }
manifold->manifold_normal = res.normal; data->normal = res.normal;
/* TODO: Remove this (debugging) */ /* TODO: Remove this (debugging) */
#if COLLIDER_DEBUG #if COLLIDER_DEBUG
{ {
manifold->res = res; constraint->res = res;
manifold->dbg_xf0 = e0_xf; constraint->dbg_xf0 = e0_xf;
manifold->dbg_xf1 = e1_xf; constraint->dbg_xf1 = e1_xf;
if (manifold->num_contacts == 0) { if (constraint->num_points == 0) {
if (res.num_points > 0) { if (res.num_points > 0) {
++e0->colliding; ++e0->colliding;
++e1->colliding; ++e1->colliding;
@ -474,7 +469,7 @@ INTERNAL void generate_contacts(void)
struct v2 tangent = v2_perp(normal); struct v2 tangent = v2_perp(normal);
/* TODO: Cache this */ /* TODO: Cache this */
/* Prepare manifold masses */ /* Prepare constraint masses */
f32 inv_m0; f32 inv_m0;
f32 inv_m1; f32 inv_m1;
f32 inv_i0; f32 inv_i0;
@ -486,15 +481,15 @@ INTERNAL void generate_contacts(void)
inv_m1 = 1.f / (e1->mass_unscaled * scale1); inv_m1 = 1.f / (e1->mass_unscaled * scale1);
inv_i0 = 1.f / (e0->inertia_unscaled * scale0); inv_i0 = 1.f / (e0->inertia_unscaled * scale0);
inv_i1 = 1.f / (e1->inertia_unscaled * scale1); inv_i1 = 1.f / (e1->inertia_unscaled * scale1);
manifold->manifold_inv_m0 = inv_m0; data->inv_m0 = inv_m0;
manifold->manifold_inv_m1 = inv_m1; data->inv_m1 = inv_m1;
manifold->manifold_inv_i0 = inv_i0; data->inv_i0 = inv_i0;
manifold->manifold_inv_i1 = inv_i1; data->inv_i1 = inv_i1;
} }
/* Delete old contacts that are no longer present */ /* Delete old contacts that are no longer present */
for (u32 i = 0; i < manifold->num_contacts; ++i) { for (u32 i = 0; i < constraint->num_points; ++i) {
struct contact *old = &manifold->contacts[i]; struct contact *old = &constraint->contacts[i];
u32 id = old->id; u32 id = old->id;
b32 found = false; b32 found = false;
for (u32 j = 0; j < res.num_points; ++j) { for (u32 j = 0; j < res.num_points; ++j) {
@ -505,7 +500,7 @@ INTERNAL void generate_contacts(void)
} }
if (!found) { if (!found) {
/* Delete contact by replacing with last in array */ /* Delete contact by replacing with last in array */
*old = manifold->contacts[--manifold->num_contacts]; *old = constraint->contacts[--constraint->num_points];
--i; --i;
} }
} }
@ -518,8 +513,8 @@ INTERNAL void generate_contacts(void)
u32 id = res_point->id; u32 id = res_point->id;
struct contact *contact = NULL; struct contact *contact = NULL;
/* Match */ /* Match */
for (u32 j = 0; j < manifold->num_contacts; ++j) { for (u32 j = 0; j < constraint->num_points; ++j) {
struct contact *t = &manifold->contacts[j]; struct contact *t = &constraint->contacts[j];
if (t->id == id) { if (t->id == id) {
contact = t; contact = t;
break; break;
@ -533,7 +528,7 @@ INTERNAL void generate_contacts(void)
#endif #endif
} else { } else {
/* Insert new */ /* Insert new */
contact = &manifold->contacts[manifold->num_contacts++]; contact = &constraint->contacts[constraint->num_points++];
MEMZERO_STRUCT(contact); MEMZERO_STRUCT(contact);
contact->id = id; contact->id = id;
} }
@ -568,46 +563,259 @@ INTERNAL void generate_contacts(void)
contact->inv_tangent_mass = k > 0.0f ? 1.0f / k : 0.0f; contact->inv_tangent_mass = k > 0.0f ? 1.0f / k : 0.0f;
} }
} }
} }
} else if (manifold) { } else if (constraint) {
#if COLLIDER_DEBUG #if COLLIDER_DEBUG
manifold->num_contacts = 0; constraint->num_points = 0;
#else #else
/* No longer colliding, delete manifold */ /* No longer colliding, delete constraint */
manifold->num_contacts = 0; constraint->num_points = 0;
entity_enable_prop(manifold, ENTITY_PROP_RELEASE); entity_enable_prop(constraint, ENTITY_PROP_RELEASE_AT_END_OF_FRAME);
#endif #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) INTERNAL void warm_start_contacts(void)
{ {
struct entity_store *store = G.tick.entity_store; struct entity_store *store = G.tick.entity_store;
for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
struct entity *manifold = &store->entities[entity_index]; struct entity *constraint = &store->entities[entity_index];
if (!entity_is_valid_and_active(manifold)) continue; if (!entity_is_valid_and_active(constraint)) continue;
if (!entity_has_prop(manifold, ENTITY_PROP_MANIFOLD)) continue; if (!entity_has_prop(constraint, ENTITY_PROP_CONTACT_CONSTRAINT)) continue;
u32 num_contacts = manifold->num_contacts; struct contact_constraint *data = &constraint->contact_constraint;
struct entity *e0 = entity_from_handle(store, manifold->manifold_e0);
struct entity *e1 = entity_from_handle(store, manifold->manifold_e1); u32 num_points = data->num_points;
if (num_contacts > 0 && entity_is_valid_and_active(e0) && entity_is_valid_and_active(e1)) { 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 e0_xf = entity_get_xform(e0);
struct xform e1_xf = entity_get_xform(e1); struct xform e1_xf = entity_get_xform(e1);
f32 inv_m0 = manifold->manifold_inv_m0; f32 inv_m0 = data->inv_m0;
f32 inv_m1 = manifold->manifold_inv_m1; f32 inv_m1 = data->inv_m1;
f32 inv_i0 = manifold->manifold_inv_i0; f32 inv_i0 = data->inv_i0;
f32 inv_i1 = manifold->manifold_inv_i1; f32 inv_i1 = data->inv_i1;
struct v2 v0 = e0->linear_velocity; struct v2 v0 = e0->linear_velocity;
struct v2 v1 = e1->linear_velocity; struct v2 v1 = e1->linear_velocity;
@ -615,18 +823,18 @@ INTERNAL void warm_start_contacts(void)
f32 w1 = e1->angular_velocity; f32 w1 = e1->angular_velocity;
/* Warm start */ /* Warm start */
struct v2 normal = manifold->manifold_normal; struct v2 normal = data->normal;
struct v2 tangent = v2_perp(normal); struct v2 tangent = v2_perp(normal);
f32 inv_num_contacts = 1.f / num_contacts; f32 inv_num_points = 1.f / num_points;
for (u32 i = 0; i < num_contacts; ++i) { for (u32 i = 0; i < num_points; ++i) {
struct contact *contact = &manifold->contacts[i]; struct contact_point *point = &data->points[i];
struct v2 p0 = xform_mul_v2(e0_xf, contact->point_local_e0); struct v2 p0 = xform_mul_v2(e0_xf, point->point_local_e0);
struct v2 p1 = xform_mul_v2(e1_xf, contact->point_local_e1); struct v2 p1 = xform_mul_v2(e1_xf, point->point_local_e1);
struct v2 vcp0 = v2_sub(p0, e0_xf.og); struct v2 vcp0 = v2_sub(p0, e0_xf.og);
struct v2 vcp1 = v2_sub(p1, e1_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)); struct v2 impulse = v2_add(v2_mul(normal, point->normal_impulse), v2_mul(tangent, point->tangent_impulse));
impulse = v2_mul(impulse, inv_num_contacts); impulse = v2_mul(impulse, inv_num_points);
v0 = v2_sub(v0, v2_mul(impulse, inv_m0)); v0 = v2_sub(v0, v2_mul(impulse, inv_m0));
v1 = v2_add(v1, v2_mul(impulse, inv_m1)); 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; }; struct soft_result { f32 bias_rate; f32 mass_scale; f32 impulse_scale; };
INTERNAL struct soft_result make_soft(f32 hertz, f32 zeta, f32 h) 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; struct entity_store *store = G.tick.entity_store;
for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
struct entity *manifold = &store->entities[entity_index]; struct entity *constraint = &store->entities[entity_index];
if (!entity_is_valid_and_active(manifold)) continue; if (!entity_is_valid_and_active(constraint)) continue;
if (!entity_has_prop(manifold, ENTITY_PROP_MANIFOLD)) continue; if (!entity_has_prop(constraint, ENTITY_PROP_CONTACT_CONSTRAINT)) continue;
struct entity *e0 = entity_from_handle(store, manifold->manifold_e0); struct contact_constraint *data = &constraint->contact_constraint;
struct entity *e1 = entity_from_handle(store, manifold->manifold_e1);
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 v0 = e0->linear_velocity;
struct v2 v1 = e1->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 w0 = e0->angular_velocity;
f32 w1 = e1->angular_velocity; f32 w1 = e1->angular_velocity;
u32 num_contacts = manifold->num_contacts; u32 num_points = data->num_points;
if (num_contacts > 0 && entity_is_valid_and_active(e0) && entity_is_valid_and_active(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 e0_xf = entity_get_xform(e0);
struct xform e1_xf = entity_get_xform(e1); struct xform e1_xf = entity_get_xform(e1);
f32 inv_m0 = manifold->manifold_inv_m0; f32 inv_m0 = data->inv_m0;
f32 inv_m1 = manifold->manifold_inv_m1; f32 inv_m1 = data->inv_m1;
f32 inv_i0 = manifold->manifold_inv_i0; f32 inv_i0 = data->inv_i0;
f32 inv_i1 = manifold->manifold_inv_i1; f32 inv_i1 = data->inv_i1;
/* Normal impulse */ /* Normal impulse */
struct v2 normal = manifold->manifold_normal; struct v2 normal = data->normal;
for (u32 contact_index = 0; contact_index < num_contacts; ++contact_index) { for (u32 point_index = 0; point_index < num_points; ++point_index) {
struct contact *contact = &manifold->contacts[contact_index]; struct contact_point *point = &data->points[point_index];
struct v2 p0 = xform_mul_v2(e0_xf, contact->point_local_e0); struct v2 p0 = xform_mul_v2(e0_xf, point->point_local_e0);
struct v2 p1 = xform_mul_v2(e1_xf, contact->point_local_e1); struct v2 p1 = xform_mul_v2(e1_xf, point->point_local_e1);
struct v2 vcp0 = v2_sub(p0, e0_xf.og); struct v2 vcp0 = v2_sub(p0, e0_xf.og);
struct v2 vcp1 = v2_sub(p1, e1_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 velocity_bias = 0.0f;
f32 mass_scale = 1.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 vel1 = v2_add(v1, v2_perp_mul(vcp1, w1));
struct v2 vrel = v2_sub(vel0, vel1); struct v2 vrel = v2_sub(vel0, vel1);
f32 k = contact->inv_normal_mass; f32 k = point->inv_normal_mass;
/* (to be applied along n) */ /* (to be applied along n) */
f32 vn = v2_dot(vrel, normal); 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 new_impulse = max_f32(old_impulse + j, 0);
f32 delta = new_impulse - old_impulse; f32 delta = new_impulse - old_impulse;
contact->normal_impulse = new_impulse; point->normal_impulse = new_impulse;
struct v2 impulse = v2_mul(normal, delta); struct v2 impulse = v2_mul(normal, delta);
v0 = v2_sub(v0, v2_mul(impulse, inv_m0)); v0 = v2_sub(v0, v2_mul(impulse, inv_m0));
@ -749,10 +954,10 @@ INTERNAL void solve_contacts(f32 dt, b32 apply_bias)
/* Tangent impulse */ /* Tangent impulse */
struct v2 tangent = v2_perp(normal); struct v2 tangent = v2_perp(normal);
for (u32 contact_index = 0; contact_index < num_contacts; ++contact_index) { for (u32 point_index = 0; point_index < num_points; ++point_index) {
struct contact *contact = &manifold->contacts[contact_index]; struct contact_point *point = &data->points[point_index];
struct v2 p0 = xform_mul_v2(e0_xf, contact->point_local_e0); struct v2 p0 = xform_mul_v2(e0_xf, point->point_local_e0);
struct v2 p1 = xform_mul_v2(e1_xf, contact->point_local_e1); struct v2 p1 = xform_mul_v2(e1_xf, point->point_local_e1);
struct v2 vcp0 = v2_sub(p0, e0_xf.og); struct v2 vcp0 = v2_sub(p0, e0_xf.og);
struct v2 vcp1 = v2_sub(p1, e1_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 vel1 = v2_add(v1, v2_perp_mul(vcp1, w1));
struct v2 vrel = v2_sub(vel0, vel1); struct v2 vrel = v2_sub(vel0, vel1);
f32 k = contact->inv_tangent_mass; f32 k = point->inv_tangent_mass;
/* (to be applied along t) */ /* (to be applied along t) */
f32 vt = v2_dot(vrel, tangent); 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 = 0.6f;
//f32 friction = 1.0f; //f32 friction = 1.0f;
//f32 friction = F32_INFINITY; //f32 friction = F32_INFINITY;
f32 max_friction = friction * contact->normal_impulse; f32 max_friction = friction * point->normal_impulse;
f32 old_impulse = contact->tangent_impulse; f32 old_impulse = point->tangent_impulse;
f32 new_impulse = clamp_f32(old_impulse + j, -max_friction, max_friction); f32 new_impulse = clamp_f32(old_impulse + j, -max_friction, max_friction);
f32 delta = new_impulse - old_impulse; f32 delta = new_impulse - old_impulse;
contact->tangent_impulse = new_impulse; point->tangent_impulse = new_impulse;
struct v2 impulse = v2_mul(tangent, delta); struct v2 impulse = v2_mul(tangent, delta);
v0 = v2_sub(v0, v2_mul(impulse, inv_m0)); 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) INTERNAL void integrate_velocities_from_forces(f32 dt)
{ {
struct entity_store *store = G.tick.entity_store; 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]; struct entity *ent = &store->entities[entity_index];
if (!ent->valid) continue; 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; *arena_push(temp.arena, struct entity *) = ent;
++ents_to_release_count; ++ents_to_release_count;
} }

32
src/user.c
View File

@ -1034,9 +1034,10 @@ INTERNAL void user_update(void)
/* Draw collision */ /* Draw collision */
#if 1 #if 1
if (entity_has_prop(ent, ENTITY_PROP_MANIFOLD)) { if (entity_has_prop(ent, ENTITY_PROP_CONTACT_CONSTRAINT)) {
struct entity *e0 = entity_from_handle(store, ent->manifold_e0); struct contact_constraint *data = &ent->contact_constraint;
struct entity *e1 = entity_from_handle(store, ent->manifold_e1); 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 e0_collider = e0->local_collider;
struct collider_shape e1_collider = e1->local_collider; struct collider_shape e1_collider = e1->local_collider;
(UNUSED)e0_collider; (UNUSED)e0_collider;
@ -1044,8 +1045,8 @@ INTERNAL void user_update(void)
//struct xform e0_xf = entity_get_xform(e0); //struct xform e0_xf = entity_get_xform(e0);
//struct xform e1_xf = entity_get_xform(e1); //struct xform e1_xf = entity_get_xform(e1);
struct xform e0_xf = ent->dbg_xf0; struct xform e0_xf = data->dbg_xf0;
struct xform e1_xf = ent->dbg_xf1; struct xform e1_xf = data->dbg_xf1;
(UNUSED)e0_xf; (UNUSED)e0_xf;
(UNUSED)e1_xf; (UNUSED)e1_xf;
@ -1177,14 +1178,14 @@ INTERNAL void user_update(void)
/* Draw contacts */ /* Draw contacts */
{ {
f32 radius = 5; f32 radius = 5;
for (u32 i = 0; i < ent->num_contacts; ++i) { for (u32 i = 0; i < ent->contact_constraint.num_points; ++i) {
struct contact contact = ent->contacts[i]; struct contact_point point = ent->contact_constraint.points[i];
#if 0 #if 0
struct v2 p0 = xform_mul_v2(e0_xf, contact.point_local_e0); 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 p1 = xform_mul_v2(e1_xf, contact.point_local_e1);
struct v2 point = v2_add(p0, v2_mul(v2_sub(p1, p0), 0.5f)); struct v2 point = v2_add(p0, v2_mul(v2_sub(p1, p0), 0.5f));
#else #else
struct v2 point = contact.dbg_pt; struct v2 dbg_pt = point.dbg_pt;
#endif #endif
/* Draw point */ /* Draw point */
{ {
@ -1192,7 +1193,7 @@ INTERNAL void user_update(void)
u32 color = RGBA_32_F(1, 1, 0, 0.50); u32 color = RGBA_32_F(1, 1, 0, 0.50);
//struct v2 point = xform_mul_v2(e0_xf, contact.p0_local); //struct v2 point = xform_mul_v2(e0_xf, contact.p0_local);
//struct v2 point = contact.p0_initial_world; //struct v2 point = contact.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 */ /* Draw normal */
{ {
@ -1200,8 +1201,8 @@ INTERNAL void user_update(void)
f32 len = 0.1f; f32 len = 0.1f;
f32 arrow_thickness = 2; f32 arrow_thickness = 2;
f32 arrow_height = 5; f32 arrow_height = 5;
struct v2 start = xform_mul_v2(G.world_view, point); struct v2 start = xform_mul_v2(G.world_view, dbg_pt);
struct v2 end = xform_mul_v2(G.world_view, v2_add(point, v2_mul(v2_norm(ent->manifold_normal), len))); 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); draw_solid_arrow_line(G.viewport_canvas, start, end, arrow_thickness, arrow_height, color);
} }
#if 0 #if 0
@ -1262,16 +1263,17 @@ INTERNAL void user_update(void)
u32 color_line = RGBA_32_F(1, 0, 1, 0.5); u32 color_line = RGBA_32_F(1, 0, 1, 0.5);
u32 color_a = RGBA_32_F(1, 0, 0, 0.5); u32 color_a = RGBA_32_F(1, 0, 0, 0.5);
u32 color_b = RGBA_32_F(0, 1, 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 a = xform_mul_v2(G.world_view, res.a0);
struct v2 b = xform_mul_v2(G.world_view, ent->res.b0); struct v2 b = xform_mul_v2(G.world_view, res.b0);
draw_solid_line(G.viewport_canvas, a, b, thickness, color_line); 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, a, radius, color_a, 10);
draw_solid_circle(G.viewport_canvas, b, radius, color_b, 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 a = xform_mul_v2(G.world_view, res.a1);
struct v2 b = xform_mul_v2(G.world_view, ent->res.b1); struct v2 b = xform_mul_v2(G.world_view, res.b1);
draw_solid_line(G.viewport_canvas, a, b, thickness, color_line); 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, a, radius, color_a, 10);
draw_solid_circle(G.viewport_canvas, b, radius, color_b, 10); draw_solid_circle(G.viewport_canvas, b, radius, color_b, 10);