more physics testing (separate logic to functions & re-calculate constraint depth each iteration)
This commit is contained in:
parent
d14207959b
commit
41d56ac13c
873
src/game.c
873
src/game.c
@ -17,10 +17,12 @@ GLOBAL struct {
|
||||
struct sys_thread game_thread;
|
||||
|
||||
b32 paused;
|
||||
struct sprite_scope *sprite_frame_scope;
|
||||
|
||||
/* Game thread input */
|
||||
struct sys_mutex game_cmds_mutex;
|
||||
struct arena game_cmds_arena;
|
||||
struct entity *root;
|
||||
|
||||
/* Ticks */
|
||||
struct sys_mutex prev_tick_mutex;
|
||||
@ -127,10 +129,10 @@ INTERNAL void spawn_test_entities(void)
|
||||
//struct v2 pos = V2(0.374142020941, -0.246118023992); /* Touching glitch spot */
|
||||
//struct v2 size = V2(1, 1);
|
||||
struct v2 size = V2(0.5, 0.5);
|
||||
f32 r = PI / 4;
|
||||
//f32 r = PI / 4;
|
||||
//f32 r = PI / 3;
|
||||
//f32 r = PI / 2;
|
||||
//f32 r = 0;
|
||||
f32 r = 0;
|
||||
f32 skew = 0;
|
||||
|
||||
struct entity *e = entity_alloc(root);
|
||||
@ -153,8 +155,8 @@ INTERNAL void spawn_test_entities(void)
|
||||
|
||||
entity_enable_prop(e, ENTITY_PROP_PHYSICAL);
|
||||
e->mass_unscaled = 100;
|
||||
//e->inertia_unscaled = 1;
|
||||
e->inertia_unscaled = F32_INFINITY;
|
||||
e->inertia_unscaled = 1;
|
||||
//e->inertia_unscaled = F32_INFINITY;
|
||||
e->linear_ground_friction = 1000;
|
||||
e->angular_ground_friction = 100;
|
||||
|
||||
@ -224,6 +226,460 @@ INTERNAL void spawn_test_entities(void)
|
||||
e->camera_quad_xform = XFORM_TRS(.s = V2(width, height));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/* ========================== *
|
||||
* TESTING
|
||||
* ========================== */
|
||||
|
||||
INTERNAL void create_contact_manifolds(void)
|
||||
{
|
||||
struct entity_store *store = G.tick.entity_store;
|
||||
struct sprite_scope *sprite_frame_scope = G.sprite_frame_scope;
|
||||
struct entity *root = G.root;
|
||||
for (u64 e0_index = 0; e0_index < store->reserved; ++e0_index) {
|
||||
struct entity *e0 = &store->entities[e0_index];
|
||||
if (!(e0->valid && entity_has_prop(e0, ENTITY_PROP_ACTIVE))) continue;
|
||||
if (!entity_has_prop(e0, ENTITY_PROP_PHYSICAL)) continue;
|
||||
if (!entity_has_prop(e0, ENTITY_PROP_PLAYER_CONTROLLED) && !entity_has_prop(e0, ENTITY_PROP_BULLET)) continue;
|
||||
|
||||
struct xform e0_xf = entity_get_xform(e0);
|
||||
struct quad e0_quad;
|
||||
struct v2_array e0_poly;
|
||||
{
|
||||
struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, e0->sprite);
|
||||
struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("shape"), e0->animation_frame);
|
||||
e0_quad = xform_mul_quad(e0->sprite_local_xform, quad_from_rect(slice.rect));
|
||||
e0_quad = xform_mul_quad(e0_xf, e0_quad);
|
||||
e0_poly = (struct v2_array) {
|
||||
.count = ARRAY_COUNT(e0_quad.e),
|
||||
.points = e0_quad.e
|
||||
};
|
||||
}
|
||||
|
||||
for (u64 e1_index = 0; e1_index < store->reserved; ++e1_index) {
|
||||
struct entity *e1 = &store->entities[e1_index];
|
||||
if (e1 == e0) continue;
|
||||
if (!(e1->valid && entity_has_prop(e1, ENTITY_PROP_ACTIVE))) continue;
|
||||
if (!entity_has_prop(e1, ENTITY_PROP_PHYSICAL)) continue;
|
||||
if (entity_has_prop(e1, ENTITY_PROP_PLAYER_CONTROLLED)) continue;
|
||||
|
||||
struct xform e1_xf = entity_get_xform(e1);
|
||||
struct quad e1_quad;
|
||||
struct v2_array e1_poly;
|
||||
{
|
||||
struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, e1->sprite);
|
||||
struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("shape"), e1->animation_frame);
|
||||
e1_quad = xform_mul_quad(e1->sprite_local_xform, quad_from_rect(slice.rect));
|
||||
e1_quad = xform_mul_quad(e1_xf, e1_quad);
|
||||
e1_poly = (struct v2_array) {
|
||||
.count = ARRAY_COUNT(e1_quad.e),
|
||||
.points = e1_quad.e
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
/* TODO: Remove this */
|
||||
static struct arena dict_arena = { 0 };
|
||||
static struct fixed_dict dict = { 0 };
|
||||
if (dict.buckets_count == 0) {
|
||||
dict_arena = arena_alloc(GIGABYTE(64));
|
||||
dict = fixed_dict_init(&dict_arena, 4096);
|
||||
}
|
||||
|
||||
struct string manifold_key;
|
||||
{
|
||||
/* FIXME: Contact should be same regardless of entity order */
|
||||
u64 manifold_hash = hash_fnv64(HASH_FNV64_BASIS, BUFFER_FROM_STRUCT(&e0->handle));
|
||||
manifold_hash = hash_fnv64(manifold_hash, BUFFER_FROM_STRUCT(&e1->handle));
|
||||
manifold_key = STRING_FROM_BUFFER(BUFFER_FROM_STRUCT(&manifold_hash));
|
||||
}
|
||||
|
||||
struct entity *manifold = fixed_dict_get(&dict, manifold_key);
|
||||
|
||||
|
||||
|
||||
|
||||
#if 1
|
||||
const f32 remove_contact_threshold_global_dist_sq = 0.005 * 0.005;
|
||||
const f32 insert_contact_threshold_global_dist_sq = 0.005 * 0.005;
|
||||
#else
|
||||
const f32 remove_contact_threshold_global_dist_sq = 0;
|
||||
const f32 insert_contact_threshold_global_dist_sq = 0;
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
struct gjk_contact_points_result res = gjk_contact_points(e0_poly, e1_poly);
|
||||
if (res.num_pairs > 0) {
|
||||
if (!manifold) {
|
||||
manifold = entity_alloc(root);
|
||||
manifold->manifold_e0 = e0->handle;
|
||||
manifold->manifold_e1 = e1->handle;
|
||||
/* TODO: Should we recalculate normal as more contact points are added? */
|
||||
manifold->manifold_normal = v2_norm(v2_sub(res.pairs[0].p1, res.pairs[0].p0));
|
||||
entity_enable_prop(manifold, ENTITY_PROP_MANIFOLD);
|
||||
activate_now(manifold);
|
||||
fixed_dict_set(&dict_arena, &dict, manifold_key, manifold);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/* TODO: Remove this (debugging) */
|
||||
manifold->prototype = res.prototype;
|
||||
manifold->simplex = res.simplex;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/* Update contact depths, and remove contacts that are no longer penetrating or have drifted too far from original position */
|
||||
struct v2 old_normal = manifold->manifold_normal;
|
||||
for (i32 j = 0; j < (i32)manifold->num_contacts; ++j) {
|
||||
struct contact *contact = &manifold->contacts[j];
|
||||
|
||||
struct v2 p0_world_initial = contact->p0_initial_world;
|
||||
struct v2 p1_world_initial = contact->p1_initial_world;
|
||||
|
||||
struct v2 p0_world_cur = xform_mul_v2(e0_xf, contact->p0_local);
|
||||
struct v2 p1_world_cur = xform_mul_v2(e1_xf, contact->p1_local);
|
||||
|
||||
f32 p0_close_enough = v2_len_sq(v2_sub(p0_world_cur, p0_world_initial)) <= remove_contact_threshold_global_dist_sq;
|
||||
f32 p1_close_enough = v2_len_sq(v2_sub(p1_world_cur, p1_world_initial)) <= remove_contact_threshold_global_dist_sq;
|
||||
|
||||
f32 depth_cur = v2_dot(old_normal, v2_sub(p1_world_cur, p0_world_cur));
|
||||
b32 still_penetrating = depth_cur >= 0;
|
||||
|
||||
if (still_penetrating && p0_close_enough && p1_close_enough) {
|
||||
/* Contact is still penetrating, mark persisted for warm starting */
|
||||
contact->persisted = true;
|
||||
contact->depth = depth_cur;
|
||||
} else {
|
||||
/* Remove contact by swapping with last in array */
|
||||
manifold->contacts[j--] = manifold->contacts[--manifold->num_contacts];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/* FIXME: Test overflow */
|
||||
struct contact overflow[2];
|
||||
u32 overflow_count = 0;
|
||||
CT_ASSERT(ARRAY_COUNT(overflow) >= ARRAY_COUNT(res.pairs)); /* Overflow should be able to hold collision result */
|
||||
|
||||
|
||||
/* Insert new contacts that aren't too close to original contacts */
|
||||
for (u32 i = 0; i < res.num_pairs; ++i) {
|
||||
b32 should_insert = true;
|
||||
|
||||
struct gjk_contact_pair new_pair = res.pairs[i];
|
||||
struct v2 p0_world = new_pair.p0;
|
||||
struct v2 p1_world = new_pair.p1;
|
||||
|
||||
for (u32 j = 0; j < manifold->num_contacts; ++j) {
|
||||
struct contact *contact = &manifold->contacts[j];
|
||||
struct v2 old_p0_world = xform_mul_v2(e0_xf, contact->p0_local);
|
||||
struct v2 old_p1_world = xform_mul_v2(e1_xf, contact->p1_local);
|
||||
f32 p0_far_enough = v2_len_sq(v2_sub(p0_world, old_p0_world)) >= insert_contact_threshold_global_dist_sq;
|
||||
f32 p1_far_enough = v2_len_sq(v2_sub(p1_world, old_p1_world)) >= insert_contact_threshold_global_dist_sq;
|
||||
if (!p0_far_enough || !p1_far_enough) {
|
||||
should_insert = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (should_insert) {
|
||||
struct contact *c;
|
||||
if (manifold->num_contacts < ARRAY_COUNT(manifold->contacts)) {
|
||||
c = &manifold->contacts[manifold->num_contacts++];
|
||||
} else {
|
||||
c = &overflow[overflow_count++];
|
||||
}
|
||||
|
||||
MEMZERO_STRUCT(c);
|
||||
c->p0_local = xform_invert_mul_v2(e0_xf, p0_world);
|
||||
c->p0_initial_world = p0_world;
|
||||
c->p1_local = xform_invert_mul_v2(e1_xf, p1_world);
|
||||
c->p1_initial_world = p1_world;
|
||||
c->depth = v2_len(v2_sub(p1_world, p0_world));
|
||||
}
|
||||
}
|
||||
|
||||
/* Manifold already has max amount of contacts, decide which 2 to keep.
|
||||
* Logic: 1st contact to keep is deepest contact, 2nd contact is furthest point from 1st contact (in local space of first shape) */
|
||||
if (overflow_count > 0) {
|
||||
/* Find contact with greatest depth */
|
||||
struct contact *deepest = NULL;
|
||||
{
|
||||
f32 deepest_depth = -F32_INFINITY;
|
||||
for (u32 i = 0; i < manifold->num_contacts; ++i) {
|
||||
struct contact *c = &manifold->contacts[i];
|
||||
f32 depth = c->depth;
|
||||
if (depth > deepest_depth) {
|
||||
deepest = c;
|
||||
deepest_depth = depth;
|
||||
}
|
||||
}
|
||||
for (u32 i = 0; i < overflow_count; ++i) {
|
||||
struct contact *c = &overflow[i];
|
||||
f32 depth = c->depth;
|
||||
if (depth > deepest_depth) {
|
||||
deepest = c;
|
||||
deepest_depth = depth;
|
||||
}
|
||||
}
|
||||
}
|
||||
/* Find contact furthest from deepest point */
|
||||
struct contact *furthest = NULL;
|
||||
{
|
||||
struct v2 deepest_p0 = deepest->p0_local;
|
||||
f32 furthest_dist_sq = -F32_INFINITY;
|
||||
for (u32 i = 0; i < manifold->num_contacts; ++i) {
|
||||
struct contact *c = &manifold->contacts[i];
|
||||
f32 dist_sq = v2_len_sq(v2_sub(c->p0_local, deepest_p0));
|
||||
if (dist_sq > furthest_dist_sq) {
|
||||
furthest = c;
|
||||
furthest_dist_sq = dist_sq;
|
||||
}
|
||||
}
|
||||
for (u32 i = 0; i < overflow_count; ++i) {
|
||||
struct contact *c = &manifold->contacts[i];
|
||||
f32 dist_sq = v2_len_sq(v2_sub(c->p0_local, deepest_p0));
|
||||
if (dist_sq > furthest_dist_sq) {
|
||||
furthest = c;
|
||||
furthest_dist_sq = dist_sq;
|
||||
}
|
||||
}
|
||||
}
|
||||
/* Copy */
|
||||
/* TODO: Optimize */
|
||||
CT_ASSERT(ARRAY_COUNT(manifold->contacts) == 2); /* Logic assumes 2 contact points */
|
||||
manifold->contacts[0] = *deepest;
|
||||
manifold->contacts[1] = *furthest;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/* NOTE: This assumes that all contacts will share the same normal as first contact */
|
||||
manifold->manifold_normal = v2_norm(v2_sub(manifold->contacts[0].p1_initial_world, manifold->contacts[0].p0_initial_world));
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/* TODO: Remove this (testing) */
|
||||
#if 0
|
||||
if (manifold->num_contacts > 1) {
|
||||
if (manifold->contacts[1].depth > manifold->contacts[1].depth) {
|
||||
manifold->contacts[0] = manifold->contacts[1];
|
||||
}
|
||||
manifold->num_contacts = 1;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
} else if (manifold) {
|
||||
/* No longer colliding, delete contact */
|
||||
manifold->num_contacts = 0;
|
||||
fixed_dict_set(&dict_arena, &dict, manifold_key, NULL);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
INTERNAL void solve_collisions(f32 dt, b32 apply_bias)
|
||||
{
|
||||
struct entity_store *store = G.tick.entity_store;
|
||||
|
||||
for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
|
||||
struct entity *manifold = &store->entities[entity_index];
|
||||
if (!(manifold->valid && entity_has_prop(manifold, ENTITY_PROP_ACTIVE))) continue;
|
||||
if (!entity_has_prop(manifold, ENTITY_PROP_MANIFOLD)) continue;
|
||||
|
||||
struct entity *e0 = entity_from_handle(store, manifold->manifold_e0);
|
||||
struct entity *e1 = entity_from_handle(store, manifold->manifold_e1);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
if (manifold->num_contacts > 0 && e0->valid && e1->valid) {
|
||||
struct xform e0_xf = entity_get_xform(e0);
|
||||
struct xform e1_xf = entity_get_xform(e1);
|
||||
|
||||
f32 scale0 = math_fabs(xform_get_determinant(e0_xf));
|
||||
f32 scale1 = math_fabs(xform_get_determinant(e1_xf));
|
||||
f32 m0 = e0->mass_unscaled * scale0;
|
||||
f32 m1 = e1->mass_unscaled * scale1;
|
||||
f32 i0 = e0->inertia_unscaled * scale0;
|
||||
f32 i1 = e1->inertia_unscaled * scale1;
|
||||
f32 inv_m0 = 1.f / m0;
|
||||
f32 inv_m1 = 1.f / m1;
|
||||
f32 inv_i0 = 1.f / i0;
|
||||
f32 inv_i1 = 1.f / i1;
|
||||
|
||||
|
||||
struct v2 e0_accumulated_linear_impulse = { 0 };
|
||||
f32 e0_accumulated_angular_impulse = 0;
|
||||
struct v2 e1_accumulated_linear_impulse = { 0 };
|
||||
f32 e1_accumulated_angular_impulse = 0;
|
||||
|
||||
struct v2 normal = manifold->manifold_normal;
|
||||
for (u32 contact_index = 0; contact_index < manifold->num_contacts; ++contact_index) {
|
||||
struct contact *contact = &manifold->contacts[contact_index];
|
||||
|
||||
struct v2 p0 = xform_mul_v2(e0_xf, contact->p0_local);
|
||||
struct v2 p1 = xform_mul_v2(e1_xf, contact->p1_local);
|
||||
|
||||
f32 bias = 0;
|
||||
if (apply_bias) {
|
||||
//f32 bias_factor = 0.2;
|
||||
f32 bias_factor = 0.1;
|
||||
|
||||
f32 bias_slop = 0.001;
|
||||
//f32 bias_slop = 0.001;
|
||||
//f32 bias_slop = 0.00;
|
||||
|
||||
bias = (bias_factor / dt) * max((contact->depth - bias_slop), 0);
|
||||
}
|
||||
|
||||
|
||||
struct v2 vcp0 = v2_sub(p0, e0_xf.og);
|
||||
struct v2 vcp1 = v2_sub(p1, e1_xf.og);
|
||||
|
||||
struct v2 p0_vel = v2_add(e0->linear_velocity, v2_perp_mul(vcp0, e0->angular_velocity));
|
||||
struct v2 p1_vel = v2_add(e1->linear_velocity, v2_perp_mul(vcp1, e1->angular_velocity));
|
||||
|
||||
struct v2 vrel = v2_sub(p1_vel, p0_vel);
|
||||
|
||||
f32 vn = v2_dot(vrel, normal);
|
||||
/* FIXME: Clamp accumulated */
|
||||
vn = max_f32(vn, 0);
|
||||
|
||||
struct v2 idk0 = v2_perp_mul(vcp0, v2_wedge(vcp0, normal) * inv_i0);
|
||||
struct v2 idk1 = v2_perp_mul(vcp1, v2_wedge(vcp1, normal) * inv_i1);
|
||||
|
||||
f32 k = inv_m0 + inv_m1 + v2_dot(normal, v2_add(idk0, idk1));
|
||||
|
||||
/* (to be applied along n) */
|
||||
f32 j = (vn + bias) / k;
|
||||
//j = max_f32(j, 0);
|
||||
|
||||
struct v2 imp = v2_mul(normal, j);
|
||||
|
||||
if (!e0->test_collided) {
|
||||
e0->test_collided = true;
|
||||
}
|
||||
|
||||
e0_accumulated_linear_impulse = v2_add(e0_accumulated_linear_impulse, imp);
|
||||
e1_accumulated_linear_impulse = v2_add(e1_accumulated_linear_impulse, v2_neg(imp));
|
||||
|
||||
e0_accumulated_angular_impulse += v2_wedge(vcp0, imp);
|
||||
e1_accumulated_angular_impulse += v2_wedge(vcp1, v2_neg(imp));
|
||||
|
||||
//entity_apply_linear_impulse(e0, imp, p0);
|
||||
//entity_apply_linear_impulse(e1, v2_neg(imp), p1);
|
||||
}
|
||||
|
||||
//entity_apply_linear_impulse(e0, imp, p0);
|
||||
//entity_apply_linear_impulse(e1, v2_neg(imp), p1);
|
||||
|
||||
|
||||
/* TODO: Remove this (testing) */
|
||||
#if 1
|
||||
/* Divide impulses by number of contact points. */
|
||||
{
|
||||
f32 inv_num_contacts = 1.f / manifold->num_contacts;
|
||||
e0_accumulated_linear_impulse = v2_mul(e0_accumulated_linear_impulse, inv_num_contacts);
|
||||
e1_accumulated_linear_impulse = v2_mul(e1_accumulated_linear_impulse, inv_num_contacts);
|
||||
e0_accumulated_angular_impulse *= inv_num_contacts;
|
||||
e1_accumulated_angular_impulse *= inv_num_contacts;
|
||||
}
|
||||
#endif
|
||||
|
||||
entity_apply_linear_impulse_to_center(e0, e0_accumulated_linear_impulse);
|
||||
entity_apply_angular_impulse(e0, e0_accumulated_angular_impulse);
|
||||
entity_apply_linear_impulse_to_center(e1, e1_accumulated_linear_impulse);
|
||||
entity_apply_angular_impulse(e1, e1_accumulated_angular_impulse);
|
||||
} else {
|
||||
entity_enable_prop(manifold, ENTITY_PROP_RELEASE);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
INTERNAL void integrate_positions(f32 dt)
|
||||
{
|
||||
struct entity_store *store = G.tick.entity_store;
|
||||
|
||||
for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
|
||||
struct entity *ent = &store->entities[entity_index];
|
||||
if (!(ent->valid && entity_has_prop(ent, ENTITY_PROP_ACTIVE))) continue;
|
||||
if (!entity_has_prop(ent, ENTITY_PROP_PHYSICAL)) continue;
|
||||
|
||||
struct xform xf = entity_get_xform(ent);
|
||||
struct v2 tick_linear_velocity = v2_mul(ent->linear_velocity, dt);
|
||||
f32 tick_angular_velocity = ent->angular_velocity * dt;
|
||||
xf.og = v2_add(xf.og, tick_linear_velocity);
|
||||
xf = xform_rotated(xf, tick_angular_velocity);
|
||||
entity_set_xform(ent, xf);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/* ========================== *
|
||||
* Update
|
||||
@ -259,9 +715,12 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
|
||||
|
||||
f64 dt = G.tick.dt;
|
||||
f64 time = G.tick.time;
|
||||
G.sprite_frame_scope = sprite_scope_begin();
|
||||
G.root = entity_from_handle(G.tick.entity_store, G.tick.entity_store->root);
|
||||
|
||||
struct entity *root = G.root;
|
||||
struct entity_store *store = G.tick.entity_store;
|
||||
struct entity *root = entity_from_handle(store, store->root);
|
||||
struct sprite_scope *sprite_frame_scope = sprite_scope_begin();
|
||||
struct sprite_scope *sprite_frame_scope = G.sprite_frame_scope;
|
||||
|
||||
(UNUSED)dt;
|
||||
(UNUSED)time;
|
||||
@ -859,398 +1318,48 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
|
||||
ent->torque = 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#if 0
|
||||
/* ========================== *
|
||||
* Create contact manifolds
|
||||
* ========================== */
|
||||
|
||||
for (u64 e0_index = 0; e0_index < store->reserved; ++e0_index) {
|
||||
struct entity *e0 = &store->entities[e0_index];
|
||||
if (!(e0->valid && entity_has_prop(e0, ENTITY_PROP_ACTIVE))) continue;
|
||||
if (!entity_has_prop(e0, ENTITY_PROP_PHYSICAL)) continue;
|
||||
if (!entity_has_prop(e0, ENTITY_PROP_PLAYER_CONTROLLED) && !entity_has_prop(e0, ENTITY_PROP_BULLET)) continue;
|
||||
|
||||
struct xform e0_xf = entity_get_xform(e0);
|
||||
struct quad e0_quad;
|
||||
struct v2_array e0_poly;
|
||||
{
|
||||
struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, e0->sprite);
|
||||
struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("shape"), e0->animation_frame);
|
||||
e0_quad = xform_mul_quad(e0->sprite_local_xform, quad_from_rect(slice.rect));
|
||||
e0_quad = xform_mul_quad(e0_xf, e0_quad);
|
||||
e0_poly = (struct v2_array) {
|
||||
.count = ARRAY_COUNT(e0_quad.e),
|
||||
.points = e0_quad.e
|
||||
};
|
||||
}
|
||||
|
||||
for (u64 e1_index = 0; e1_index < store->reserved; ++e1_index) {
|
||||
struct entity *e1 = &store->entities[e1_index];
|
||||
if (e1 == e0) continue;
|
||||
if (!(e1->valid && entity_has_prop(e1, ENTITY_PROP_ACTIVE))) continue;
|
||||
if (!entity_has_prop(e1, ENTITY_PROP_PHYSICAL)) continue;
|
||||
if (entity_has_prop(e1, ENTITY_PROP_PLAYER_CONTROLLED)) continue;
|
||||
|
||||
struct xform e1_xf = entity_get_xform(e1);
|
||||
struct quad e1_quad;
|
||||
struct v2_array e1_poly;
|
||||
{
|
||||
struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, e1->sprite);
|
||||
struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("shape"), e1->animation_frame);
|
||||
e1_quad = xform_mul_quad(e1->sprite_local_xform, quad_from_rect(slice.rect));
|
||||
e1_quad = xform_mul_quad(e1_xf, e1_quad);
|
||||
e1_poly = (struct v2_array) {
|
||||
.count = ARRAY_COUNT(e1_quad.e),
|
||||
.points = e1_quad.e
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
/* TODO: Remove this */
|
||||
static struct arena dict_arena = { 0 };
|
||||
static struct fixed_dict dict = { 0 };
|
||||
if (dict.buckets_count == 0) {
|
||||
dict_arena = arena_alloc(GIGABYTE(64));
|
||||
dict = fixed_dict_init(&dict_arena, 4096);
|
||||
}
|
||||
|
||||
struct string manifold_key;
|
||||
{
|
||||
/* FIXME: Contact should be same regardless of entity order */
|
||||
u64 manifold_hash = hash_fnv64(HASH_FNV64_BASIS, BUFFER_FROM_STRUCT(&e0->handle));
|
||||
manifold_hash = hash_fnv64(manifold_hash, BUFFER_FROM_STRUCT(&e1->handle));
|
||||
manifold_key = STRING_FROM_BUFFER(BUFFER_FROM_STRUCT(&manifold_hash));
|
||||
}
|
||||
|
||||
struct entity *manifold = fixed_dict_get(&dict, manifold_key);
|
||||
|
||||
|
||||
|
||||
|
||||
#if 1
|
||||
const f32 remove_contact_threshold_global_dist_sq = 0.005 * 0.005;
|
||||
const f32 insert_contact_threshold_global_dist_sq = 0.005 * 0.005;
|
||||
#else
|
||||
const f32 remove_contact_threshold_global_dist_sq = 0;
|
||||
const f32 insert_contact_threshold_global_dist_sq = 0;
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
struct gjk_contact_points_result res = gjk_contact_points(e0_poly, e1_poly);
|
||||
if (res.num_pairs > 0) {
|
||||
if (!manifold) {
|
||||
manifold = entity_alloc(root);
|
||||
manifold->manifold_e0 = e0->handle;
|
||||
manifold->manifold_e1 = e1->handle;
|
||||
/* TODO: Should we recalculate normal as more contact points are added? */
|
||||
manifold->manifold_normal = v2_norm(v2_sub(res.pairs[0].p1, res.pairs[0].p0));
|
||||
entity_enable_prop(manifold, ENTITY_PROP_MANIFOLD);
|
||||
activate_now(manifold);
|
||||
fixed_dict_set(&dict_arena, &dict, manifold_key, manifold);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/* TODO: Remove this (debugging) */
|
||||
manifold->prototype = res.prototype;
|
||||
manifold->simplex = res.simplex;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/* Remove contacts that are no longer penetrating or have drifted too far from original position */
|
||||
struct v2 old_normal = manifold->manifold_normal;
|
||||
for (i32 j = 0; j < (i32)manifold->num_contacts; ++j) {
|
||||
struct contact *contact = &manifold->contacts[j];
|
||||
|
||||
struct v2 p0_world_initial = contact->p0_initial_world;
|
||||
struct v2 p1_world_initial = contact->p1_initial_world;
|
||||
|
||||
struct v2 p0_world_cur = xform_mul_v2(e0_xf, contact->p0_local);
|
||||
struct v2 p1_world_cur = xform_mul_v2(e1_xf, contact->p1_local);
|
||||
|
||||
f32 p0_close_enough = v2_len_sq(v2_sub(p0_world_cur, p0_world_initial)) <= remove_contact_threshold_global_dist_sq;
|
||||
f32 p1_close_enough = v2_len_sq(v2_sub(p1_world_cur, p1_world_initial)) <= remove_contact_threshold_global_dist_sq;
|
||||
b32 still_penetrating = v2_dot(old_normal, v2_sub(p1_world_cur, p0_world_cur)) >= 0;
|
||||
|
||||
if (still_penetrating && p0_close_enough && p1_close_enough) {
|
||||
/* Contact is still penetrating, mark persisted for warm starting */
|
||||
contact->persisted = true;
|
||||
} else {
|
||||
/* Remove contact by swapping with last in array */
|
||||
manifold->contacts[j--] = manifold->contacts[--manifold->num_contacts];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/* FIXME: Test overflow */
|
||||
struct contact overflow[2];
|
||||
u32 overflow_count = 0;
|
||||
CT_ASSERT(ARRAY_COUNT(overflow) >= ARRAY_COUNT(res.pairs)); /* Overflow should be able to hold collision result */
|
||||
|
||||
|
||||
/* Insert new contacts that aren't too close to original contacts */
|
||||
for (u32 i = 0; i < res.num_pairs; ++i) {
|
||||
b32 should_insert = true;
|
||||
|
||||
struct gjk_contact_pair new_pair = res.pairs[i];
|
||||
struct v2 p0_world = new_pair.p0;
|
||||
struct v2 p1_world = new_pair.p1;
|
||||
|
||||
for (u32 j = 0; j < manifold->num_contacts; ++j) {
|
||||
struct contact *contact = &manifold->contacts[j];
|
||||
struct v2 old_p0_world = xform_mul_v2(e0_xf, contact->p0_local);
|
||||
struct v2 old_p1_world = xform_mul_v2(e1_xf, contact->p1_local);
|
||||
f32 p0_far_enough = v2_len_sq(v2_sub(p0_world, old_p0_world)) >= insert_contact_threshold_global_dist_sq;
|
||||
f32 p1_far_enough = v2_len_sq(v2_sub(p1_world, old_p1_world)) >= insert_contact_threshold_global_dist_sq;
|
||||
if (!p0_far_enough || !p1_far_enough) {
|
||||
should_insert = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (should_insert) {
|
||||
struct contact *c;
|
||||
if (manifold->num_contacts < ARRAY_COUNT(manifold->contacts)) {
|
||||
c = &manifold->contacts[manifold->num_contacts++];
|
||||
} else {
|
||||
c = &overflow[overflow_count++];
|
||||
}
|
||||
|
||||
MEMZERO_STRUCT(c);
|
||||
c->p0_local = xform_invert_mul_v2(e0_xf, p0_world);
|
||||
c->p0_initial_world = p0_world;
|
||||
c->p1_local = xform_invert_mul_v2(e1_xf, p1_world);
|
||||
c->p1_initial_world = p1_world;
|
||||
c->depth = v2_len(v2_sub(p1_world, p0_world));
|
||||
}
|
||||
}
|
||||
|
||||
/* Manifold already has max amount of contacts, decide which 2 to keep.
|
||||
* Logic: 1st contact to keep is deepest contact, 2nd contact is furthest point from 1st contact (in local space of first shape) */
|
||||
if (overflow_count > 0) {
|
||||
/* Find contact with greatest depth */
|
||||
struct contact *deepest = NULL;
|
||||
{
|
||||
f32 deepest_depth = -F32_INFINITY;
|
||||
for (u32 i = 0; i < manifold->num_contacts; ++i) {
|
||||
struct contact *c = &manifold->contacts[i];
|
||||
f32 depth = c->depth;
|
||||
if (depth > deepest_depth) {
|
||||
deepest = c;
|
||||
deepest_depth = depth;
|
||||
}
|
||||
}
|
||||
for (u32 i = 0; i < overflow_count; ++i) {
|
||||
struct contact *c = &overflow[i];
|
||||
f32 depth = c->depth;
|
||||
if (depth > deepest_depth) {
|
||||
deepest = c;
|
||||
deepest_depth = depth;
|
||||
}
|
||||
}
|
||||
}
|
||||
/* Find contact furthest from deepest point */
|
||||
struct contact *furthest = NULL;
|
||||
{
|
||||
struct v2 deepest_p0 = deepest->p0_local;
|
||||
f32 furthest_dist_sq = -F32_INFINITY;
|
||||
for (u32 i = 0; i < manifold->num_contacts; ++i) {
|
||||
struct contact *c = &manifold->contacts[i];
|
||||
f32 dist_sq = v2_len_sq(v2_sub(c->p0_local, deepest_p0));
|
||||
if (dist_sq > furthest_dist_sq) {
|
||||
furthest = c;
|
||||
furthest_dist_sq = dist_sq;
|
||||
}
|
||||
}
|
||||
for (u32 i = 0; i < overflow_count; ++i) {
|
||||
struct contact *c = &manifold->contacts[i];
|
||||
f32 dist_sq = v2_len_sq(v2_sub(c->p0_local, deepest_p0));
|
||||
if (dist_sq > furthest_dist_sq) {
|
||||
furthest = c;
|
||||
furthest_dist_sq = dist_sq;
|
||||
}
|
||||
}
|
||||
}
|
||||
/* Copy */
|
||||
/* TODO: Optimize */
|
||||
CT_ASSERT(ARRAY_COUNT(manifold->contacts) == 2); /* Logic assumes 2 contact points */
|
||||
manifold->contacts[0] = *deepest;
|
||||
manifold->contacts[1] = *furthest;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/* NOTE: This assumes that all contacts will share the same normal as first contact */
|
||||
manifold->manifold_normal = v2_norm(v2_sub(manifold->contacts[0].p1_initial_world, manifold->contacts[0].p0_initial_world));
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/* TODO: Remove this (testing) */
|
||||
#if 0
|
||||
if (manifold->num_contacts > 1) {
|
||||
if (manifold->contacts[1].depth > manifold->contacts[1].depth) {
|
||||
manifold->contacts[0] = manifold->contacts[1];
|
||||
}
|
||||
manifold->num_contacts = 1;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
} else if (manifold) {
|
||||
/* No longer colliding, delete contact */
|
||||
manifold->num_contacts = 0;
|
||||
fixed_dict_set(&dict_arena, &dict, manifold_key, NULL);
|
||||
}
|
||||
}
|
||||
}
|
||||
create_contact_manifolds();
|
||||
|
||||
/* ========================== *
|
||||
* Collision resolution
|
||||
* ========================== */
|
||||
|
||||
for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
|
||||
struct entity *manifold = &store->entities[entity_index];
|
||||
if (!(manifold->valid && entity_has_prop(manifold, ENTITY_PROP_ACTIVE))) continue;
|
||||
if (!entity_has_prop(manifold, ENTITY_PROP_MANIFOLD)) continue;
|
||||
|
||||
struct entity *e0 = entity_from_handle(store, manifold->manifold_e0);
|
||||
struct entity *e1 = entity_from_handle(store, manifold->manifold_e1);
|
||||
|
||||
if (manifold->num_contacts > 0 && e0->valid && e1->valid) {
|
||||
struct xform e0_xf = entity_get_xform(e0);
|
||||
struct xform e1_xf = entity_get_xform(e1);
|
||||
|
||||
f32 scale0 = math_fabs(xform_get_determinant(e0_xf));
|
||||
f32 scale1 = math_fabs(xform_get_determinant(e1_xf));
|
||||
f32 m0 = e0->mass_unscaled * scale0;
|
||||
f32 m1 = e1->mass_unscaled * scale1;
|
||||
f32 i0 = e0->inertia_unscaled * scale0;
|
||||
f32 i1 = e1->inertia_unscaled * scale1;
|
||||
f32 inv_m0 = 1.f / m0;
|
||||
f32 inv_m1 = 1.f / m1;
|
||||
f32 inv_i0 = 1.f / i0;
|
||||
f32 inv_i1 = 1.f / i1;
|
||||
|
||||
struct v2 e0_accumulated_linear_impulse = { 0 };
|
||||
f32 e0_accumulated_angular_impulse = 0;
|
||||
struct v2 e1_accumulated_linear_impulse = { 0 };
|
||||
f32 e1_accumulated_angular_impulse = 0;
|
||||
|
||||
struct v2 normal = manifold->manifold_normal;
|
||||
for (u32 contact_index = 0; contact_index < manifold->num_contacts; ++contact_index) {
|
||||
struct contact *contact = &manifold->contacts[contact_index];
|
||||
|
||||
struct v2 p0 = xform_mul_v2(e0_xf, contact->p0_local);
|
||||
struct v2 p1 = xform_mul_v2(e1_xf, contact->p1_local);
|
||||
f32 depth = contact->depth;
|
||||
|
||||
#if 0
|
||||
f32 bias_factor = 0.2;
|
||||
f32 bias_slop = 0.001;
|
||||
//f32 bias_slop = 0.001;
|
||||
//f32 bias_slop = 0.00;
|
||||
|
||||
f32 bias = (bias_factor / dt) * max((depth - bias_slop), 0);
|
||||
#else
|
||||
(UNUSED)depth;
|
||||
f32 bias = 0;
|
||||
#endif
|
||||
|
||||
|
||||
struct v2 vcp0 = v2_sub(p0, e0_xf.og);
|
||||
struct v2 vcp1 = v2_sub(p1, e1_xf.og);
|
||||
|
||||
struct v2 p0_vel = v2_add(e0->linear_velocity, v2_perp_mul(vcp0, e0->angular_velocity));
|
||||
struct v2 p1_vel = v2_add(e1->linear_velocity, v2_perp_mul(vcp1, e1->angular_velocity));
|
||||
|
||||
struct v2 vrel = v2_sub(p1_vel, p0_vel);
|
||||
|
||||
f32 vn = v2_dot(vrel, normal);
|
||||
vn = max_f32(vn, 0);
|
||||
|
||||
struct v2 idk0 = v2_perp_mul(vcp0, v2_wedge(vcp0, normal) * inv_i0);
|
||||
struct v2 idk1 = v2_perp_mul(vcp1, v2_wedge(vcp1, normal) * inv_i1);
|
||||
|
||||
f32 k = inv_m0 + inv_m1 + v2_dot(normal, v2_add(idk0, idk1));
|
||||
|
||||
/* (to be applied along n) */
|
||||
f32 j = (vn + bias) / k;
|
||||
//j = max_f32(j, 0);
|
||||
|
||||
struct v2 imp = v2_mul(normal, j);
|
||||
|
||||
if (!e0->test_collided) {
|
||||
e0->test_collided = true;
|
||||
}
|
||||
|
||||
e0_accumulated_linear_impulse = v2_add(e0_accumulated_linear_impulse, imp);
|
||||
e1_accumulated_linear_impulse = v2_add(e1_accumulated_linear_impulse, v2_neg(imp));
|
||||
|
||||
e0_accumulated_angular_impulse += v2_wedge(vcp0, imp);
|
||||
e1_accumulated_angular_impulse += v2_wedge(vcp1, v2_neg(imp));
|
||||
|
||||
//entity_apply_linear_impulse(e0, imp, p0);
|
||||
//entity_apply_linear_impulse(e1, v2_neg(imp), p1);
|
||||
}
|
||||
|
||||
//entity_apply_linear_impulse(e0, imp, p0);
|
||||
//entity_apply_linear_impulse(e1, v2_neg(imp), p1);
|
||||
|
||||
|
||||
/* TODO: Remove this (testing) */
|
||||
#if 1
|
||||
/* Divide impulses by number of contact points. */
|
||||
{
|
||||
f32 inv_num_contacts = 1.f / manifold->num_contacts;
|
||||
e0_accumulated_linear_impulse = v2_mul(e0_accumulated_linear_impulse, inv_num_contacts);
|
||||
e1_accumulated_linear_impulse = v2_mul(e1_accumulated_linear_impulse, inv_num_contacts);
|
||||
e0_accumulated_angular_impulse *= inv_num_contacts;
|
||||
e1_accumulated_angular_impulse *= inv_num_contacts;
|
||||
}
|
||||
#endif
|
||||
|
||||
entity_apply_linear_impulse_to_center(e0, e0_accumulated_linear_impulse);
|
||||
entity_apply_angular_impulse(e0, e0_accumulated_angular_impulse);
|
||||
entity_apply_linear_impulse_to_center(e1, e1_accumulated_linear_impulse);
|
||||
entity_apply_angular_impulse(e1, e1_accumulated_angular_impulse);
|
||||
} else {
|
||||
entity_enable_prop(manifold, ENTITY_PROP_RELEASE);
|
||||
}
|
||||
}
|
||||
solve_collisions(dt);
|
||||
|
||||
/* ========================== *
|
||||
* Integrate positions from velocity
|
||||
* ========================== */
|
||||
|
||||
for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
|
||||
struct entity *ent = &store->entities[entity_index];
|
||||
if (!(ent->valid && entity_has_prop(ent, ENTITY_PROP_ACTIVE))) continue;
|
||||
if (!entity_has_prop(ent, ENTITY_PROP_PHYSICAL)) continue;
|
||||
integrate_positions(dt);
|
||||
#else
|
||||
u32 iterations = 4;
|
||||
for (u32 i = 0; i < iterations; ++i) {
|
||||
f32 timestep = dt / iterations;
|
||||
|
||||
struct xform xf = entity_get_xform(ent);
|
||||
struct v2 tick_linear_velocity = v2_mul(ent->linear_velocity, dt);
|
||||
f32 tick_angular_velocity = ent->angular_velocity * dt;
|
||||
xf.og = v2_add(xf.og, tick_linear_velocity);
|
||||
xf = xform_rotated(xf, tick_angular_velocity);
|
||||
entity_set_xform(ent, xf);
|
||||
#if 1
|
||||
create_contact_manifolds();
|
||||
solve_collisions(timestep, true);
|
||||
integrate_positions(timestep);
|
||||
solve_collisions(timestep, false);
|
||||
#else
|
||||
create_contact_manifolds();
|
||||
solve_collisions(timestep, false);
|
||||
integrate_positions(timestep);
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
/* ========================== *
|
||||
* Initialize bullet kinematics from sources
|
||||
|
||||
@ -156,7 +156,7 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru
|
||||
|
||||
/* TODO: Verify epsilon */
|
||||
/* FIXME: Infinite loop when epsilon too low and shapes is too far from 0 (precision issue) */
|
||||
const f32 epsilon = 0.00100;
|
||||
const f32 epsilon = 0.0000100;
|
||||
struct gjk_simplex s = { 0 };
|
||||
b32 colliding = false;
|
||||
struct gjk_menkowski_point *proto = NULL;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user