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

fix wild collisions at low precision

Browse Source
This commit is contained in:
jacob 2024-10-05 00:09:50 -05:00
parent aac6acd18f
commit 1dde27d31b
6 changed files with 182 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
.natvis
View File

@ -1,6 +1,10 @@
<?xml version="1.0" encoding="utf-8"?> <?xml version="1.0" encoding="utf-8"?>
<AutoVisualizer xmlns="http://schemas.microsoft.com/vstudio/debugger/natvis/2010"> <AutoVisualizer xmlns="http://schemas.microsoft.com/vstudio/debugger/natvis/2010">
<Type Name = "v2">
<DisplayString>({x}, {y})</DisplayString>
</Type>
<Type Name = "string"> <Type Name = "string">
<DisplayString>({len}) {text, [len] s}</DisplayString> <DisplayString>({len}) {text, [len] s}</DisplayString>
<StringView>text, [len] s</StringView> <StringView>text, [len] s</StringView>

5
src/config.h
View File

@ -35,12 +35,15 @@
#define GAME_PHYSICS_SUBSTEPS 4 #define GAME_PHYSICS_SUBSTEPS 4
#define GAME_PHYSICS_ENABLE_WARM_STARTING 1 #define GAME_PHYSICS_ENABLE_WARM_STARTING 1
#define GAME_MAX_LINEAR_VELOCITY 100
#define GAME_MAX_ANGULAR_VELOCITY ((2 * PI) * 5)
/* How many ticks back in time should the user blend between? /* How many ticks back in time should the user blend between?
* <Delay ms> = <USER_INTERP_OFFSET_TICK_RATIO> * <Game tick rate> * <Delay ms> = <USER_INTERP_OFFSET_TICK_RATIO> * <Game tick rate>
* E.g: At 1.5, the user thread will render 75ms back in time (if game thread runs at 50FPS) * E.g: At 1.5, the user thread will render 75ms back in time (if game thread runs at 50FPS)
*/ */
#define USER_INTERP_OFFSET_TICK_RATIO 1.1 #define USER_INTERP_OFFSET_TICK_RATIO 1.1
#define USER_INTERP_ENABLED 0 #define USER_INTERP_ENABLED 1
/* ========================== * /* ========================== *
* Settings * Settings

3
src/entity.c
View File

@ -279,6 +279,9 @@ struct xform entity_get_local_xform(struct entity *ent)
void entity_set_xform(struct entity *ent, struct xform xf) void entity_set_xform(struct entity *ent, struct xform xf)
{ {
/* TODO: Remove this (debugging) */
if (v2_len_sq(xf.bx) == 0 || v2_len_sq(xf.by) == 0) DEBUGBREAK;
if (!xform_eq(xf, ent->cached_global_xform)) { if (!xform_eq(xf, ent->cached_global_xform)) {
struct entity_store *store = entity_get_store(ent); struct entity_store *store = entity_get_store(ent);
/* Update local xform */ /* Update local xform */

157
src/game.c
View File

@ -123,6 +123,10 @@ INTERNAL void spawn_test_entities(f32 offset)
{ {
struct entity *root = entity_from_handle(G.tick.entity_store, G.tick.entity_store->root); struct entity *root = entity_from_handle(G.tick.entity_store, G.tick.entity_store->root);
//const f32 offset_all = -20000;
//const f32 offset_all = -5000;
const f32 offset_all = 0;
/* Player */ /* Player */
struct entity *player_ent; struct entity *player_ent;
{ {
@ -134,12 +138,13 @@ INTERNAL void spawn_test_entities(f32 offset)
//struct v2 pos = V2(1.1230469346046448864129274625156 - 0.0001, -1); /* Touching right side of box */ //struct v2 pos = V2(1.1230469346046448864129274625156 - 0.0001, -1); /* Touching right side of box */
//struct v2 pos = V2(0.374142020941, -0.246118023992); /* Touching glitch spot */ //struct v2 pos = V2(0.374142020941, -0.246118023992); /* Touching glitch spot */
pos = v2_add(pos, V2(0, -offset)); pos = v2_add(pos, V2(0, offset));
pos = v2_add(pos, V2(0, offset_all));
//struct v2 size = V2(1, 1); //struct v2 size = V2(1, 1);
struct v2 size = V2(0.5, 0.5); struct v2 size = V2(0.5, 0.5);
f32 r = PI; //f32 r = PI;
//f32 r = PI / 4; f32 r = PI / 4;
//f32 r = PI / 3; //f32 r = PI / 3;
//f32 r = 0.05; //f32 r = 0.05;
//f32 r = PI / 2; //f32 r = PI / 2;
@ -167,8 +172,8 @@ INTERNAL void spawn_test_entities(f32 offset)
entity_enable_prop(e, ENTITY_PROP_PHYSICAL); entity_enable_prop(e, ENTITY_PROP_PHYSICAL);
e->mass_unscaled = 100; e->mass_unscaled = 100;
//e->inertia_unscaled = F32_INFINITY; e->inertia_unscaled = F32_INFINITY;
e->inertia_unscaled = 25; //e->inertia_unscaled = 25;
e->linear_ground_friction = 1000; e->linear_ground_friction = 1000;
e->angular_ground_friction = 100; e->angular_ground_friction = 100;
@ -211,6 +216,8 @@ INTERNAL void spawn_test_entities(f32 offset)
f32 rot = 0; f32 rot = 0;
struct entity *e = entity_alloc(root); struct entity *e = entity_alloc(root);
pos = v2_add(pos, V2(0, offset_all));
e->sprite = sprite_tag_from_path(STR("res/graphics/box.ase")); e->sprite = sprite_tag_from_path(STR("res/graphics/box.ase"));
entity_enable_prop(e, ENTITY_PROP_PHYSICAL); entity_enable_prop(e, ENTITY_PROP_PHYSICAL);
@ -466,6 +473,22 @@ INTERNAL void create_contact_manifolds(void)
/* TODO: Remove this (debugging) */
if (F32_IS_NAN(scale0) ||
F32_IS_NAN(scale1) ||
F32_IS_NAN(m0) ||
F32_IS_NAN(m1) ||
F32_IS_NAN(i0) ||
F32_IS_NAN(i1) ||
F32_IS_NAN(inv_m0) ||
F32_IS_NAN(inv_m1) ||
F32_IS_NAN(inv_i0) ||
F32_IS_NAN(inv_i1)) {
DEBUGBREAK;
}
@ -559,6 +582,9 @@ INTERNAL void warm_start_contacts(void)
v1 = v2_add(v1, v2_mul(impulse, contact->inv_m1)); v1 = v2_add(v1, v2_mul(impulse, contact->inv_m1));
w0 -= v2_wedge(vcp0, impulse) * contact->inv_i0; w0 -= v2_wedge(vcp0, impulse) * contact->inv_i0;
w1 += v2_wedge(vcp1, impulse) * contact->inv_i1; w1 += v2_wedge(vcp1, impulse) * contact->inv_i1;
/* TODO: Remove this (debugging) */
if (F32_IS_NAN(v0.x) || F32_IS_NAN(v0.y) || F32_IS_NAN(v1.x) || F32_IS_NAN(v1.y) || F32_IS_NAN(w0) || F32_IS_NAN(w1)) DEBUGBREAK;
} }
e0->linear_velocity = v0; e0->linear_velocity = v0;
@ -650,8 +676,6 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias)
/* Soft constraint */ /* Soft constraint */
f32 contact_pushout_velocity = 3.0f; f32 contact_pushout_velocity = 3.0f;
f32 contact_damping_ratio = 10.0f; f32 contact_damping_ratio = 10.0f;
f32 contact_hertz = (GAME_FPS * GAME_PHYSICS_SUBSTEPS) / 8.f; f32 contact_hertz = (GAME_FPS * GAME_PHYSICS_SUBSTEPS) / 8.f;
@ -678,15 +702,40 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias)
f32 delta = new_impulse - old_impulse; f32 delta = new_impulse - old_impulse;
contact->normal_impulse = new_impulse; contact->normal_impulse = new_impulse;
/* TODO: Remove this (debugging) */
//if (math_fabs(delta) > 10) DEBUGBREAK;
struct v2 impulse = v2_mul(normal, delta); struct v2 impulse = v2_mul(normal, delta);
v0 = v2_sub(v0, v2_mul(impulse, contact->inv_m0)); v0 = v2_sub(v0, v2_mul(impulse, contact->inv_m0));
v1 = v2_add(v1, v2_mul(impulse, contact->inv_m1)); v1 = v2_add(v1, v2_mul(impulse, contact->inv_m1));
w0 -= v2_wedge(vcp0, impulse) * contact->inv_i0; w0 -= v2_wedge(vcp0, impulse) * contact->inv_i0;
w1 += v2_wedge(vcp1, impulse) * contact->inv_i1; w1 += v2_wedge(vcp1, impulse) * contact->inv_i1;
#if 1
v0 = v2_clamp_len(v0, GAME_MAX_LINEAR_VELOCITY);
v1 = v2_clamp_len(v1, GAME_MAX_LINEAR_VELOCITY);
w0 = clamp_f32(w0, -GAME_MAX_ANGULAR_VELOCITY, GAME_MAX_ANGULAR_VELOCITY);
w1 = clamp_f32(w1, -GAME_MAX_ANGULAR_VELOCITY, GAME_MAX_ANGULAR_VELOCITY);
#endif
/* TODO: Remove this (debugging) */
if ((math_fabs(v0.x) > 10000000) || (math_fabs(v0.y) > 10000000) || (math_fabs(v1.x) > 10000000) || (math_fabs(v1.y) > 10000000) || (math_fabs(w0) > 10000000) || (math_fabs(w1) > 10000000)) DEBUGBREAK;
/* TODO: Remove this (debugging) */
if (F32_IS_NAN(v0.x) || F32_IS_NAN(v0.y) || F32_IS_NAN(v1.x) || F32_IS_NAN(v1.y) || F32_IS_NAN(w0) || F32_IS_NAN(w1)) DEBUGBREAK;
} }
/* Tangent impulse */ /* Tangent impulse */
#if 1
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 contact_index = 0; contact_index < num_contacts; ++contact_index) {
struct contact *contact = &manifold->contacts[contact_index]; struct contact *contact = &manifold->contacts[contact_index];
@ -706,8 +755,8 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias)
f32 j = vt * k; f32 j = vt * k;
//j *= inv_num_contacts; //j *= inv_num_contacts;
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 * contact->normal_impulse;
f32 old_impulse = contact->tangent_impulse; f32 old_impulse = contact->tangent_impulse;
@ -720,8 +769,13 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias)
v1 = v2_add(v1, v2_mul(impulse, contact->inv_m1)); v1 = v2_add(v1, v2_mul(impulse, contact->inv_m1));
w0 -= v2_wedge(vcp0, impulse) * contact->inv_i0; w0 -= v2_wedge(vcp0, impulse) * contact->inv_i0;
w1 += v2_wedge(vcp1, impulse) * contact->inv_i1; w1 += v2_wedge(vcp1, impulse) * contact->inv_i1;
/* TODO: Remove this (debugging) */
if (F32_IS_NAN(v0.x) || F32_IS_NAN(v0.y) || F32_IS_NAN(v1.x) || F32_IS_NAN(v1.y) || F32_IS_NAN(w0) || F32_IS_NAN(w1)) DEBUGBREAK;
} }
#endif
/* TODO: Remove this (debugging) */
if (F32_IS_NAN(v0.x) || F32_IS_NAN(v0.y) || F32_IS_NAN(v1.x) || F32_IS_NAN(v1.y) || F32_IS_NAN(w0) || F32_IS_NAN(w1)) DEBUGBREAK;
e0->linear_velocity = v0; e0->linear_velocity = v0;
e0->angular_velocity = w0; e0->angular_velocity = w0;
@ -737,9 +791,41 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias)
} }
INTERNAL void integrate_velocities_from_forces(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 (!entity_is_valid_and_active(ent)) continue;
if (!entity_has_prop(ent, ENTITY_PROP_PHYSICAL)) continue;
struct xform xf = entity_get_xform(ent);
f32 det_abs = math_fabs(xform_get_determinant(xf));
f32 mass = ent->mass_unscaled * det_abs;
f32 inertia = ent->inertia_unscaled * det_abs;
INTERNAL void integrate_positions(f32 dt) /* Determine force & torque acceleration */
struct v2 force_accel = v2_mul(v2_div(ent->force, mass), dt);
f32 torque_accel = (ent->torque / inertia) * dt;
/* Integrate */
ent->linear_velocity = v2_add(ent->linear_velocity, force_accel);
ent->angular_velocity += torque_accel;
/* Clamp velocities */
ent->linear_velocity = v2_clamp_len(ent->linear_velocity, GAME_MAX_LINEAR_VELOCITY);
ent->angular_velocity = clamp_f32(ent->angular_velocity, -GAME_MAX_ANGULAR_VELOCITY, GAME_MAX_ANGULAR_VELOCITY);
/* TODO: Remove this (debugging) */
if (F32_IS_NAN(ent->linear_velocity.x) || F32_IS_NAN(ent->linear_velocity.y) || F32_IS_NAN(ent->angular_velocity)) DEBUGBREAK;
/* Reset forces */
ent->force = V2(0, 0);
ent->torque = 0;
}
}
INTERNAL void integrate_positions_from_velocities(f32 dt)
{ {
struct entity_store *store = G.tick.entity_store; struct entity_store *store = G.tick.entity_store;
@ -748,12 +834,21 @@ INTERNAL void integrate_positions(f32 dt)
if (!entity_is_valid_and_active(ent)) continue; if (!entity_is_valid_and_active(ent)) continue;
if (!entity_has_prop(ent, ENTITY_PROP_PHYSICAL)) continue; if (!entity_has_prop(ent, ENTITY_PROP_PHYSICAL)) continue;
/* Clamp velocities */
ent->linear_velocity = v2_clamp_len(ent->linear_velocity, GAME_MAX_LINEAR_VELOCITY);
ent->angular_velocity = clamp_f32(ent->angular_velocity, -GAME_MAX_ANGULAR_VELOCITY, GAME_MAX_ANGULAR_VELOCITY);
struct xform xf = entity_get_xform(ent); struct xform xf = entity_get_xform(ent);
struct v2 tick_linear_velocity = v2_mul(ent->linear_velocity, dt); struct v2 tick_linear_velocity = v2_mul(ent->linear_velocity, dt);
f32 tick_angular_velocity = ent->angular_velocity * dt; f32 tick_angular_velocity = ent->angular_velocity * dt;
xf.og = v2_add(xf.og, tick_linear_velocity); xf.og = v2_add(xf.og, tick_linear_velocity);
xf = xform_rotated(xf, tick_angular_velocity); xf = xform_rotated(xf, tick_angular_velocity);
entity_set_xform(ent, xf); entity_set_xform(ent, xf);
/* TODO: Remove this (debugging) */
if (F32_IS_NAN(xf.bx.x) || F32_IS_NAN(xf.bx.y) || F32_IS_NAN(xf.by.x) || F32_IS_NAN(xf.by.y) || F32_IS_NAN(xf.og.x) || F32_IS_NAN(xf.og.y)) {
DEBUGBREAK;
}
} }
} }
@ -858,9 +953,9 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
case GAME_CMD_KIND_SPAWN_TEST: case GAME_CMD_KIND_SPAWN_TEST:
{ {
logf_info("Spawning (test)"); logf_info("Spawning (test)");
#if 1 #if 0
for (u32 i = 0; i < 50; ++i) { for (u32 i = 0; i < 50; ++i) {
spawn_test_entities(i); spawn_test_entities(-i);
} }
#else #else
spawn_test_entities(0); spawn_test_entities(0);
@ -1399,43 +1494,17 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
} }
#endif #endif
/* ========================== *
* Integrate velocities from forces
* ========================== */
for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
struct entity *ent = &store->entities[entity_index];
if (!entity_is_valid_and_active(ent)) continue;
if (!entity_has_prop(ent, ENTITY_PROP_PHYSICAL)) continue;
struct xform xf = entity_get_xform(ent);
f32 det_abs = math_fabs(xform_get_determinant(xf));
f32 mass = ent->mass_unscaled * det_abs;
f32 inertia = ent->inertia_unscaled * det_abs;
/* Determine force & torque acceleration */
struct v2 force_accel = v2_mul(v2_div(ent->force, mass), dt);
f32 torque_accel = (ent->torque / inertia) * dt;
/* Integrate */
ent->linear_velocity = v2_add(ent->linear_velocity, force_accel);
ent->angular_velocity += torque_accel;
/* Reset forces */
ent->force = V2(0, 0);
ent->torque = 0;
}
/* ========================== * /* ========================== *
* Physics * Physics
* ========================== */ * ========================== */
{ {
integrate_velocities_from_forces(dt);
create_contact_manifolds(); create_contact_manifolds();
(UNUSED)create_contact_manifolds; (UNUSED)create_contact_manifolds;
(UNUSED)solve_collisions; (UNUSED)solve_collisions;
(UNUSED)integrate_positions; (UNUSED)integrate_positions_from_velocities;
(UNUSED)warm_start_contacts; (UNUSED)warm_start_contacts;
f32 substep_dt = dt / GAME_PHYSICS_SUBSTEPS; f32 substep_dt = dt / GAME_PHYSICS_SUBSTEPS;
@ -1445,12 +1514,12 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
warm_start_contacts(); warm_start_contacts();
#endif #endif
solve_collisions(substep_dt, true); solve_collisions(substep_dt, true);
integrate_positions(substep_dt); integrate_positions_from_velocities(substep_dt);
solve_collisions(substep_dt, false); /* Relaxation */ solve_collisions(substep_dt, false); /* Relaxation */
#else #else
//solve_collisions(substep_dt, true); //solve_collisions(substep_dt, true);
solve_collisions(substep_dt, false); solve_collisions(substep_dt, false);
integrate_positions(substep_dt); integrate_positions_from_velocities(substep_dt);
#endif #endif
} }
} }

93
src/gjk.c
View File

@ -597,9 +597,10 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru
/* TODO: Parameterize */ /* TODO: Parameterize */
const f32 tolerance = 0.001f; const f32 tolerance = 0.001f;
const f32 min_unique_pt_dist_sq = 0.0001f * 0.0001f; const f32 min_unique_pt_dist_sq = 0.001f * 0.001f;
b32 colliding = false; b32 colliding = false;
b32 simplex_is_closest_edge = false;
struct gjk_simplex s = ZI; struct gjk_simplex s = ZI;
struct v2 *proto = NULL; struct v2 *proto = NULL;
@ -609,7 +610,6 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru
struct gjk_contact_point points[2] = ZI; struct gjk_contact_point points[2] = ZI;
u32 num_points = 0; u32 num_points = 0;
/* Used by GJK & EPA */
struct v2 dir = ZI; struct v2 dir = ZI;
struct v2 m = ZI; struct v2 m = ZI;
@ -628,23 +628,19 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru
s.a = menkowski_point(shape0, shape1, v2_sub(shape1.points[0], shape0.points[0])); s.a = menkowski_point(shape0, shape1, v2_sub(shape1.points[0], shape0.points[0]));
s.len = 1; s.len = 1;
f32 dist_test = 0;
struct v2 removed_a = ZI; struct v2 removed_a = ZI;
struct v2 removed_b = ZI; struct v2 removed_b = ZI;
u32 num_removed = 0; u32 num_removed = 0;
b32 done = false;
while (!colliding) { while (!done) {
if (s.len == 1) { if (s.len == 1) {
/* Second point is support point towards origin */ /* Second point is support point towards origin */
dir = v2_neg(s.a); dir = v2_neg(s.a);
DBGSTEP; DBGSTEP;
m = menkowski_point(shape0, shape1, dir); m = menkowski_point(shape0, shape1, dir);
dist_test = v2_len_sq(v2_sub(m, s.a)); /* Check that new point is far enough away from existing point */
if (dist_test < min_unique_pt_dist_sq) { if (v2_len_sq(v2_sub(m, s.a)) < min_unique_pt_dist_sq) break;
break;
}
s.b = s.a; s.b = s.a;
s.a = m; s.a = m;
s.len = 2; s.len = 2;
@ -657,11 +653,17 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru
DBGSTEP; DBGSTEP;
m = menkowski_point(shape0, shape1, dir); m = menkowski_point(shape0, shape1, dir);
/* Check that new point is far enough away from existing points */ /* Check that new point is far enough away from existing points */
if (v2_len_sq(v2_sub(m, s.a)) < min_unique_pt_dist_sq) break; if ((v2_len_sq(v2_sub(m, s.a)) < min_unique_pt_dist_sq) ||
if (v2_len_sq(v2_sub(m, s.b)) < min_unique_pt_dist_sq) break; (v2_len_sq(v2_sub(m, s.b)) < min_unique_pt_dist_sq) ||
if (num_removed >= 1) { ((num_removed >= 1) && (
if (v2_len_sq(v2_sub(m, removed_a)) < min_unique_pt_dist_sq) break; (v2_len_sq(v2_sub(m, removed_a)) < min_unique_pt_dist_sq) ||
if (num_removed >= 2 && v2_len_sq(v2_sub(m, removed_b)) < min_unique_pt_dist_sq) break; (num_removed >= 2 && v2_len_sq(v2_sub(m, removed_b)) < min_unique_pt_dist_sq)
)) ||
(math_fabs(v2_wedge(v2_sub(s.b, s.a), v2_sub(m, s.a))) < min_unique_pt_dist_sq)
) {
simplex_is_closest_edge = true;
done = true;
break;
} }
s.c = s.b; s.c = s.b;
s.b = s.a; s.b = s.a;
@ -682,9 +684,10 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru
voronoi_mask |= (v2_dot(rbc_dir, v2_neg(s.b)) > 0) << 2; /* Regions bc, b, and c */ voronoi_mask |= (v2_dot(rbc_dir, v2_neg(s.b)) > 0) << 2; /* Regions bc, b, and c */
/* Remove point or edge and determine next direction based on voronoi region */ /* Remove point or edge and determine next direction based on voronoi region */
switch (voronoi_mask) { switch (voronoi_mask) {
default: case 0:
{ /* No region, must be in simplex */ { /* No region, must be in simplex */
colliding = true; colliding = true;
done = true;
} break; } break;
case 1: case 1:
{ /* Region ab, remove c */ { /* Region ab, remove c */
@ -692,7 +695,6 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru
removed_a = s.c; removed_a = s.c;
s.len = 2; s.len = 2;
dir = rab_dir; /* Next third point is in direction of region ab */ dir = rab_dir; /* Next third point is in direction of region ab */
} break; } break;
case 2: case 2:
{ /* Region ac, remove b */ { /* Region ac, remove b */
@ -734,10 +736,21 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru
s.len = 1; s.len = 1;
s.a = s.c; s.a = s.c;
} break; } break;
default:
{
/* Unknown region (should be impossible) */
ASSERT(false);
done = true;
} break;
} }
} }
} }
/* TODO: Remove this (debugging) */
if (F32_IS_NAN(s.a.x) || F32_IS_NAN(s.a.y) || F32_IS_NAN(s.b.x) || F32_IS_NAN(s.b.y) || F32_IS_NAN(s.c.x) || F32_IS_NAN(s.c.y)) {
DEBUGBREAK;
}
if (colliding) { if (colliding) {
/* ========================== * /* ========================== *
* Epa (to find collision normal from inside shape) * Epa (to find collision normal from inside shape)
@ -849,62 +862,23 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru
} }
proto[pen_pe_index] = m; proto[pen_pe_index] = m;
} }
} else { } else if (simplex_is_closest_edge) {
if (s.len == 1) { if (s.len == 1) {
/* TODO? */ /* TODO? */
} else { } else {
ASSERT(s.len == 2); ASSERT(s.len == 2);
#if 0
struct v2 vab = v2_sub(s.b, s.a); struct v2 vab = v2_sub(s.b, s.a);
struct v2 vao = v2_neg(s.a); struct v2 vao = v2_neg(s.a);
f32 ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1); f32 ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1);
struct v2 p = v2_add(s.a, v2_mul(vab, ratio)); struct v2 p = v2_add(s.a, v2_mul(vab, ratio));
if (v2_len_sq(p) <= (tolerance * tolerance)) { if (v2_len_sq(p) <= (tolerance * tolerance)) {
normal = v2_norm(v2_perp_towards_dir(vab, vao)); #if 1
colliding = true;
}
#elif 0
struct v2 vab = v2_sub(s.b, s.a);
struct v2 vao = v2_neg(s.a);
f32 ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1);
struct v2 p = v2_add(s.a, v2_mul(vab, ratio));
if (v2_len_sq(p) <= (tolerance * tolerance)) {
struct v2_64 a64 = V2_64_FROM_V2(s.a);
struct v2_64 b64 = V2_64_FROM_V2(s.b);
struct v2_64 vab64 = v2_sub64(b64, a64);
//struct v2_64 vao64 = v2_neg64(a64);
//struct v2_64 normal64 = v2_norm64(v2_perp_towards_dir64(vab64, vao64));
struct v2_64 normal64 = v2_norm64(v2_perp_towards_dir64(vab64, V2_64_FROM_V2(dir)));
normal = V2(normal64.x, normal64.y);
colliding = true;
}
#elif 0
struct v2 vab = v2_sub(s.b, s.a);
struct v2 vao = v2_neg(s.a);
f32 ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1);
struct v2 p = v2_add(s.a, v2_mul(vab, ratio));
if (v2_len_sq(p) <= (tolerance * tolerance)) {
normal = v2_norm(v2_perp_towards_dir(vab, dir));
colliding = true;
}
#else
struct v2 vab = v2_sub(s.b, s.a);
struct v2 vao = v2_neg(s.a);
f32 ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1);
struct v2 p = v2_add(s.a, v2_mul(vab, ratio));
if (v2_len_sq(p) <= (tolerance * tolerance)) {
#if 0
normal = v2_norm(v2_perp_towards_dir(vab, dir)); normal = v2_norm(v2_perp_towards_dir(vab, dir));
#else #else
normal = v2_norm(dir); normal = v2_norm(dir);
#endif #endif
colliding = true; colliding = true;
} }
#endif
} }
} }
@ -1084,6 +1058,9 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru
point->point = v2_add(b0_clipped, v2_mul(vb0b1_clipped, 0.5f)); point->point = v2_add(b0_clipped, v2_mul(vb0b1_clipped, 0.5f));
} }
#else #else
/* TODO: Remove this (debugging) */
if (a_sep < -3 || b_sep < -3) DEBUGBREAK;
if (a_sep < tolerance) { if (a_sep < tolerance) {
struct gjk_contact_point *point = &points[num_points++]; struct gjk_contact_point *point = &points[num_points++];
point->id = id_a0 | (id_a1 << 4); point->id = id_a0 | (id_a1 << 4);

26
src/math.h
View File

@ -673,10 +673,21 @@ INLINE struct v2 v2_perp_towards_dir(struct v2 v, struct v2 dir)
INLINE struct v2 v2_norm(struct v2 a) INLINE struct v2 v2_norm(struct v2 a)
{ {
f32 l = a.x * a.x + a.y * a.y; f32 l_sq = a.x * a.x + a.y * a.y;
if (l != 0) { if (l_sq != 0) {
/* TODO: Benchmark vs math_rqsrt(l) */ /* TODO: Benchmark vs math_rqsrt(l) */
f32 denom = 1.f / math_sqrt(l); f32 denom = 1.f / math_sqrt(l_sq);
a.x *= denom;
a.y *= denom;
}
return a;
}
INLINE struct v2 v2_clamp_len(struct v2 a, f32 max)
{
f32 l_sq = a.x * a.x + a.y * a.y;
if (l_sq > (max * max) && l_sq != 0) {
f32 denom = max / math_sqrt(l_sq);
a.x *= denom; a.x *= denom;
a.y *= denom; a.y *= denom;
} }
@ -887,6 +898,15 @@ INLINE struct xform xform_rotated(struct xform xf, f32 angle)
res.bx.y = xf.bx.y * c + xf.by.y * s; res.bx.y = xf.bx.y * c + xf.by.y * s;
res.by.x = xf.bx.x * -s + xf.by.x * c; res.by.x = xf.bx.x * -s + xf.by.x * c;
res.by.y = xf.bx.y * -s + xf.by.y * c; res.by.y = xf.bx.y * -s + xf.by.y * c;
/* TODO: Remove this (debugging) */
if (v2_len_sq(res.bx) == 0 || v2_len_sq(res.by) == 0) DEBUGBREAK;
return res; return res;
} }