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

apparent stability for now

Browse Source
This commit is contained in:
jacob 2024-10-04 20:15:44 -05:00
parent cf911f27b7
commit aac6acd18f
7 changed files with 712 additions and 92 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/config.h
View File

@ -31,7 +31,9 @@
#define GAME_FPS 50.0
#define GAME_TIMESCALE 1.0
#define GAME_PHYSICS_SUBSTEPS 4
#define GAME_PHYSICS_ENABLE_WARM_STARTING 1
/* How many ticks back in time should the user blend between?
* <Delay ms> = <USER_INTERP_OFFSET_TICK_RATIO> * <Game tick rate>

7
src/entity.h
View File

@ -73,6 +73,13 @@ struct contact {
f32 normal_impulse; /* Accumulated impulse along normal */
f32 tangent_impulse; /* Accumulated impulse along tangent */
f32 inv_normal_mass;
f32 inv_tangent_mass;
f32 inv_m0;
f32 inv_m1;
f32 inv_i0;
f32 inv_i1;
b32 persisted;
};

191
src/game.c
View File

@ -20,7 +20,7 @@ GLOBAL struct {
struct sprite_scope *sprite_frame_scope;
/* TODO: Remove this (testing) */
b32 box_spawned;
b32 first_spawn;
/* Game thread input */
struct sys_mutex game_cmds_mutex;
@ -119,7 +119,7 @@ INTERNAL void activate_now(struct entity *ent)
/* TODO: Remove this */
INTERNAL void spawn_test_entities(void)
INTERNAL void spawn_test_entities(f32 offset)
{
struct entity *root = entity_from_handle(G.tick.entity_store, G.tick.entity_store->root);
@ -133,10 +133,13 @@ INTERNAL void spawn_test_entities(void)
//struct v2 pos = V2(1.1230469346046448864129274625156, -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 */
pos = v2_add(pos, V2(0, -offset));
//struct v2 size = V2(1, 1);
struct v2 size = V2(0.5, 0.5);
//f32 r = PI;
f32 r = PI / 4;
f32 r = PI;
//f32 r = PI / 4;
//f32 r = PI / 3;
//f32 r = 0.05;
//f32 r = PI / 2;
@ -164,8 +167,8 @@ INTERNAL void spawn_test_entities(void)
entity_enable_prop(e, ENTITY_PROP_PHYSICAL);
e->mass_unscaled = 100;
e->inertia_unscaled = F32_INFINITY;
//e->inertia_unscaled = 25;
//e->inertia_unscaled = F32_INFINITY;
e->inertia_unscaled = 25;
e->linear_ground_friction = 1000;
e->angular_ground_friction = 100;
@ -197,14 +200,13 @@ INTERNAL void spawn_test_entities(void)
}
/* Box */
if (!G.box_spawned) {
G.box_spawned = true;
if (!G.first_spawn) {
//struct v2 pos = V2(0.5, -1);
struct v2 pos = V2(0.5, 20);
//struct v2 pos = V2(1, -1);
//struct v2 size = V2(1, 1);
struct v2 size = V2(50, 50);
struct v2 size = V2(500, 50);
//f32 rot = PI / 4;
f32 rot = 0;
struct entity *e = entity_alloc(root);
@ -226,7 +228,7 @@ INTERNAL void spawn_test_entities(void)
}
/* Camera */
{
if (!G.first_spawn) {
struct entity *e = entity_alloc(root);
entity_set_xform(e, XFORM_IDENT);
@ -238,6 +240,8 @@ INTERNAL void spawn_test_entities(void)
f32 height = (f32)DEFAULT_CAMERA_HEIGHT;
e->camera_quad_xform = XFORM_TRS(.s = V2(width, height));
}
G.first_spawn = true;
}
@ -391,7 +395,10 @@ INTERNAL void create_contact_manifolds(void)
manifold->solved = res.solved;
}
}
manifold->manifold_normal = res.normal;
struct v2 normal = res.normal;
struct v2 tangent = v2_perp(normal);
manifold->manifold_normal = normal;
/* Delete old contacts that are no longer present */
for (u32 i = 0; i < manifold->num_contacts; ++i) {
@ -428,7 +435,7 @@ INTERNAL void create_contact_manifolds(void)
}
if (contact) {
/* Update existing */
#if 1
#if !GAME_PHYSICS_ENABLE_WARM_STARTING
contact->normal_impulse = 0;
contact->tangent_impulse = 0;
#endif
@ -441,6 +448,54 @@ INTERNAL void create_contact_manifolds(void)
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;
{
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 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);
//f32 k = ((inv_m0 + inv_m1) + (inv_i0 * vcp0_wedge * vcp0_wedge) + (inv_i1 * vcp1_wedge * vcp1_wedge)) * manifold->num_contacts;
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);
//f32 k = ((inv_m0 + inv_m1) + (inv_i0 * vcp0_wedge * vcp0_wedge) + (inv_i1 * vcp1_wedge * vcp1_wedge)) * manifold->num_contacts;
contact->inv_tangent_mass = k > 0.0f ? 1.0f / k : 0.0f;
}
contact->inv_m0 = inv_m0;
contact->inv_m1 = inv_m1;
contact->inv_i0 = inv_i0;
contact->inv_i1 = inv_i1;
}
}
@ -484,21 +539,11 @@ INTERNAL void warm_start_contacts(void)
f32 w0 = e0->angular_velocity;
f32 w1 = e1->angular_velocity;
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;
/* Warm start */
f32 inv_num_contacts = 1.f / num_contacts;
struct v2 normal = manifold->manifold_normal;
struct v2 tangent = v2_perp(normal);
for (u32 i = 0; i < manifold->num_contacts; ++i) {
for (u32 i = 0; i < num_contacts; ++i) {
struct contact *contact = &manifold->contacts[i];
struct v2 p0 = xform_mul_v2(e0_xf, contact->point_local_e0);
struct v2 p1 = xform_mul_v2(e1_xf, contact->point_local_e1);
@ -506,10 +551,14 @@ INTERNAL void warm_start_contacts(void)
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));
v0 = v2_sub(v0, v2_mul(impulse, inv_m0));
v1 = v2_add(v1, v2_mul(impulse, inv_m1));
w0 -= v2_wedge(vcp0, impulse) * inv_i0;
w1 += v2_wedge(vcp1, impulse) * inv_i1;
(UNUSED)inv_num_contacts;
//impulse = v2_mul(impulse, inv_num_contacts);
v0 = v2_sub(v0, v2_mul(impulse, contact->inv_m0));
v1 = v2_add(v1, v2_mul(impulse, contact->inv_m1));
w0 -= v2_wedge(vcp0, impulse) * contact->inv_i0;
w1 += v2_wedge(vcp1, impulse) * contact->inv_i1;
}
e0->linear_velocity = v0;
@ -568,13 +617,9 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias)
struct v2 v0 = e0->linear_velocity;
struct v2 v1 = e1->linear_velocity;
//struct v2 v0_start = v0;
//struct v2 v1_start = v1;
f32 w0 = e0->angular_velocity;
f32 w1 = e1->angular_velocity;
//f32 w0_start = w0;
//f32 w1_start = w1;
u32 num_contacts = manifold->num_contacts;
f32 inv_num_contacts = 1.f / num_contacts;
@ -583,17 +628,6 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias)
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;
/* Normal impulse */
struct v2 normal = manifold->manifold_normal;
for (u32 contact_index = 0; contact_index < num_contacts; ++contact_index) {
@ -603,52 +637,40 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias)
struct v2 vcp0 = v2_sub(p0, e0_xf.og);
struct v2 vcp1 = v2_sub(p1, e1_xf.og);
f32 penetration = -(v2_dot(v2_sub(p1, p0), normal) + contact->starting_separation);
f32 separation = v2_dot(v2_sub(p1, p0), normal) + contact->starting_separation;
f32 velocity_bias = 0.0f;
f32 mass_scale = 1.0f;
f32 impulse_scale = 0.0f;
if (penetration <= 0.0f) {
velocity_bias = penetration / dt;
if (separation > 0.0f) {
velocity_bias = separation / dt;
} else if (apply_bias) {
#if 0
/* Baumgarte Stabilization */
const f32 bias_factor = 0.2f;
const f32 bias_slop = 0.005f;
velocity_bias = (bias_factor / dt) * max_f32(-separation - bias_slop, 0);
#else
/* Soft constraint */
f32 contact_pushout_velocity = 3.0f;
f32 contact_damping_ratio = 10.0f;
f32 contact_hertz = 25.0f;
contact_hertz = min_f32(contact_hertz, 0.25f / dt);
f32 contact_hertz = (GAME_FPS * GAME_PHYSICS_SUBSTEPS) / 8.f;
struct soft_result softness = make_soft(contact_hertz, contact_damping_ratio, dt);
velocity_bias = max_f32(softness.bias_rate * penetration, -contact_pushout_velocity);
velocity_bias = max_f32(softness.bias_rate * separation, -contact_pushout_velocity);
mass_scale = softness.mass_scale;
impulse_scale = softness.impulse_scale;
#endif
}
struct v2 vel0 = v2_add(v0, v2_perp_mul(vcp0, w0));
struct v2 vel1 = v2_add(v1, v2_perp_mul(vcp1, w1));
struct v2 vrel = v2_sub(vel0, vel1);
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);
k = k > 0.0f ? 1.0f / k : 0.0f;
f32 k = contact->inv_normal_mass;
/* (to be applied along n) */
f32 vn = v2_dot(vrel, normal);
f32 j = ((k * mass_scale) * (vn + velocity_bias)) - (contact->normal_impulse * impulse_scale) ;
f32 j = ((k * mass_scale) * (vn - velocity_bias)) - (contact->normal_impulse * impulse_scale);
//j *= inv_num_contacts;
f32 old_impulse = contact->normal_impulse;
@ -657,13 +679,14 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias)
contact->normal_impulse = new_impulse;
struct v2 impulse = v2_mul(normal, delta);
v0 = v2_sub(v0, v2_mul(impulse, inv_m0));
v1 = v2_add(v1, v2_mul(impulse, inv_m1));
w0 -= v2_wedge(vcp0, impulse) * inv_i0;
w1 += v2_wedge(vcp1, impulse) * inv_i1;
v0 = v2_sub(v0, v2_mul(impulse, contact->inv_m0));
v1 = v2_add(v1, v2_mul(impulse, contact->inv_m1));
w0 -= v2_wedge(vcp0, impulse) * contact->inv_i0;
w1 += v2_wedge(vcp1, impulse) * contact->inv_i1;
}
/* Tangent impulse */
#if 1
struct v2 tangent = v2_perp(normal);
for (u32 contact_index = 0; contact_index < num_contacts; ++contact_index) {
struct contact *contact = &manifold->contacts[contact_index];
@ -676,19 +699,16 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias)
struct v2 vel1 = v2_add(v1, v2_perp_mul(vcp1, w1));
struct v2 vrel = v2_sub(vel0, vel1);
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);
k = k > 0.0f ? 1.0f / k : 0.0f;
f32 k = contact->inv_tangent_mass;
/* (to be applied along t) */
f32 vt = v2_dot(vrel, tangent);
f32 j = vt * k;
//j *= inv_num_contacts;
//f32 friction = 0.6f;
f32 friction = 0.6f;
//f32 friction = 1.0f;
f32 friction = 999999.f;
//f32 friction = F32_INFINITY;
f32 max_friction = friction * contact->normal_impulse;
f32 old_impulse = contact->tangent_impulse;
f32 new_impulse = clamp_f32(old_impulse + j, -max_friction, max_friction);
@ -696,11 +716,12 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias)
contact->tangent_impulse = new_impulse;
struct v2 impulse = v2_mul(tangent, delta);
v0 = v2_sub(v0, v2_mul(impulse, inv_m0));
v1 = v2_add(v1, v2_mul(impulse, inv_m1));
w0 -= v2_wedge(vcp0, impulse) * inv_i0;
w1 += v2_wedge(vcp1, impulse) * inv_i1;
v0 = v2_sub(v0, v2_mul(impulse, contact->inv_m0));
v1 = v2_add(v1, v2_mul(impulse, contact->inv_m1));
w0 -= v2_wedge(vcp0, impulse) * contact->inv_i0;
w1 += v2_wedge(vcp1, impulse) * contact->inv_i1;
}
#endif
e0->linear_velocity = v0;
e0->angular_velocity = w0;
@ -813,7 +834,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
static b32 run = 0;
if (!run) {
run = 1;
spawn_test_entities();
spawn_test_entities(0);
}
}
@ -830,14 +851,20 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
{
logf_info("Clearing level");
entity_store_release_all_entities(store);
G.box_spawned = false;
G.first_spawn = false;
} break;
/* Spawn test */
case GAME_CMD_KIND_SPAWN_TEST:
{
logf_info("Spawning (test)");
spawn_test_entities();
#if 1
for (u32 i = 0; i < 50; ++i) {
spawn_test_entities(i);
}
#else
spawn_test_entities(0);
#endif
} break;
default: break;
@ -1405,18 +1432,18 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
{
create_contact_manifolds();
warm_start_contacts();
(UNUSED)create_contact_manifolds;
(UNUSED)solve_collisions;
(UNUSED)integrate_positions;
(UNUSED)warm_start_contacts;
f32 substep_dt = dt / GAME_PHYSICS_SUBSTEPS;
for (u32 i = 0; i < GAME_PHYSICS_SUBSTEPS; ++i) {
#if 1
//warm_start_contacts();
#if GAME_PHYSICS_ENABLE_WARM_STARTING
warm_start_contacts();
#endif
solve_collisions(substep_dt, true);
integrate_positions(substep_dt);
solve_collisions(substep_dt, false); /* Relaxation */

586
src/gjk.c
View File

@ -164,6 +164,7 @@ struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_arra
#if 0
struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, struct v2_array shape1)
{
struct temp_arena scratch = scratch_begin_no_conflict(); /* TODO: Only begin scratch for EPA */
@ -171,8 +172,9 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru
/* TODO: Set all epsilons used in this function to 0.005 */
struct gjk_simplex s = ZI;
b32 colliding = false;
struct gjk_simplex s = ZI;
struct v2 *proto = NULL;
u32 proto_count = 0;
@ -193,8 +195,6 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru
* ========================== */
{
/* First point is support point in shape's general directions to eachother */
dir = v2_sub(shape1.points[0], shape0.points[0]);
if (v2_is_zero(dir)) dir = V2(1, 0);
@ -253,13 +253,9 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru
if (colliding) {
/* ========================== *
* Epa (to find collision normal)
* Epa (to find collision normal from inside shape)
* ========================== */
/* If penetration is less than this, collision will return false (to
* prevent wacky extrapolated normals at small penetration values) */
const f32 min_pen_len = 0.0001f;
proto = arena_dry_push(scratch.arena, struct v2);
proto_count = 0;
{
@ -331,6 +327,7 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru
}
}
if (!unique) {
#if 0
f32 len = v2_len(normal);
if (len < min_pen_len) {
colliding = false;
@ -338,6 +335,18 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru
normal = v2_mul(normal, 1.f / len);
}
break;
#else
/* Re-do normal calculation in high precision (for accuracy with small separation values) */
struct v2_64 ps = V2_64_FROM_V2(proto[pen_ps_index]);
struct v2_64 pe = V2_64_FROM_V2(proto[pen_pe_index]);
struct v2_64 vse = v2_sub64(pe, ps);
struct v2_64 vso = v2_neg64(ps);
struct v2_64 vsd = v2_mul64(vse, (v2_dot64(vso, vse) / v2_len_sq64(vse)));
struct v2_64 normal64 = v2_add64(ps, vsd);
normal64 = v2_norm64(normal64);
normal = V2(normal64.x, normal64.y);
break;
#endif
}
}
@ -516,6 +525,7 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru
f32 a_sep = v2_dot(va0a1_clipped, normal);
f32 b_sep = v2_dot(vb0b1_clipped, normal);
#if 0
if (a_sep < 0) {
struct gjk_contact_point *point = &points[num_points++];
point->id = id_a0 | (id_a1 << 4);
@ -528,6 +538,21 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru
point->separation = b_sep;
point->point = v2_add(b0_clipped, v2_mul(vb0b1_clipped, 0.5f));
}
#else
f32 sticky = 0.05f;
if (a_sep < sticky) {
struct gjk_contact_point *point = &points[num_points++];
point->id = id_a0 | (id_a1 << 4);
point->separation = a_sep;
point->point = v2_add(a0_clipped, v2_mul(va0a1_clipped, 0.5f));
}
if (b_sep < sticky) {
struct gjk_contact_point *point = &points[num_points++];
point->id = id_b0 | (id_b1 << 4);
point->separation = b_sep;
point->point = v2_add(b0_clipped, v2_mul(vb0b1_clipped, 0.5f));
}
#endif
}
}
@ -563,6 +588,551 @@ abort:
return res;
}
#else
struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, struct v2_array shape1)
{
struct temp_arena scratch = scratch_begin_no_conflict(); /* TODO: Only begin scratch for EPA */
struct gjk_contact_points_result res = ZI;
/* TODO: Parameterize */
const f32 tolerance = 0.001f;
const f32 min_unique_pt_dist_sq = 0.0001f * 0.0001f;
b32 colliding = false;
struct gjk_simplex s = ZI;
struct v2 *proto = NULL;
u32 proto_count = 0;
struct v2 normal = ZI;
struct gjk_contact_point points[2] = ZI;
u32 num_points = 0;
/* Used by GJK & EPA */
struct v2 dir = ZI;
struct v2 m = ZI;
#if GJK_DEBUG
u32 dbg_step = 0;
#endif
/* ========================== *
* GJK
*
* Determine encapsulating simplex if colliding, or closest edge / point to
* origin on simplex (for check if shape distances are within tolerance)
* ========================== */
{
/* First point is support point in shape's general directions to eachother */
s.a = menkowski_point(shape0, shape1, v2_sub(shape1.points[0], shape0.points[0]));
s.len = 1;
f32 dist_test = 0;
struct v2 removed_a = ZI;
struct v2 removed_b = ZI;
u32 num_removed = 0;
while (!colliding) {
if (s.len == 1) {
/* Second point is support point towards origin */
dir = v2_neg(s.a);
DBGSTEP;
m = menkowski_point(shape0, shape1, dir);
dist_test = v2_len_sq(v2_sub(m, s.a));
if (dist_test < min_unique_pt_dist_sq) {
break;
}
s.b = s.a;
s.a = m;
s.len = 2;
/* Third point is support point in direction of line normal towards origin */
dir = v2_perp_towards_dir(v2_sub(s.b, s.a), v2_neg(s.a));
}
{
DBGSTEP;
m = menkowski_point(shape0, shape1, dir);
/* 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.b)) < min_unique_pt_dist_sq) break;
if (num_removed >= 1) {
if (v2_len_sq(v2_sub(m, removed_a)) < min_unique_pt_dist_sq) break;
if (num_removed >= 2 && v2_len_sq(v2_sub(m, removed_b)) < min_unique_pt_dist_sq) break;
}
s.c = s.b;
s.b = s.a;
s.a = m;
s.len = 3;
}
/* Determine voronoi region of the simplex in which the origin lies */
i32 voronoi_mask = 0;
struct v2 vab = v2_sub(s.b, s.a);
struct v2 vac = v2_sub(s.c, s.a);
struct v2 vbc = v2_sub(s.c, s.b);
struct v2 rab_dir = v2_perp_towards_dir(vab, v2_neg(vac));
struct v2 rac_dir = v2_perp_towards_dir(vac, v2_neg(vab));
struct v2 rbc_dir = v2_perp_towards_dir(vbc, vab);
voronoi_mask |= (v2_dot(rab_dir, v2_neg(s.a)) > 0) << 0; /* Regions ab, a, and b*/
voronoi_mask |= (v2_dot(rac_dir, v2_neg(s.a)) > 0) << 1; /* Regions ac, a, 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 */
switch (voronoi_mask) {
default:
{ /* No region, must be in simplex */
colliding = true;
} break;
case 1:
{ /* Region ab, remove c */
num_removed = 1;
removed_a = s.c;
s.len = 2;
dir = rab_dir; /* Next third point is in direction of region ab */
} break;
case 2:
{ /* Region ac, remove b */
num_removed = 1;
removed_a = s.b;
s.len = 2;
s.b = s.c;
dir = rac_dir; /* Next third point is in direction of region ac */
} break;
case 4:
{ /* Region bc, remove a */
num_removed = 1;
removed_a = s.a;
s.len = 2;
s.a = s.b;
s.b = s.c;
dir = rbc_dir; /* Next third point is in direction of region bc */
} break;
case 3:
{ /* Region a, remove bc */
num_removed = 2;
removed_a = s.b;
removed_b = s.c;
s.len = 1;
} break;
case 5:
{ /* Region b, remove ac */
num_removed = 2;
removed_a = s.a;
removed_b = s.c;
s.len = 1;
s.a = s.b;
} break;
case 6:
{ /* Region c, remove ab */
num_removed = 2;
removed_a = s.a;
removed_b = s.b;
s.len = 1;
s.a = s.c;
} break;
}
}
}
if (colliding) {
/* ========================== *
* Epa (to find collision normal from inside shape)
* ========================== */
proto = arena_dry_push(scratch.arena, struct v2);
proto_count = 0;
{
ASSERT(s.len == 3);
struct v2 *tmp = arena_push_array(scratch.arena, struct v2, 3);
tmp[0] = s.a;
tmp[1] = s.b;
tmp[2] = s.c;
proto_count = 3;
}
while (colliding) {
f32 pen_len_sq = F32_INFINITY;
/* Find dir from origin to closest edge */
/* FIXME: Winding order of ps & pe index */
u32 pen_ps_index = 0;
u32 pen_pe_index = 0;
for (u32 i = 0; i < proto_count; ++i) {
u32 ps_index = i;
u32 pe_index = (i < proto_count - 1) ? (i + 1) : 0;
struct v2 ps = proto[ps_index];
struct v2 pe = proto[pe_index];
struct v2 vse = v2_sub(pe, ps);
struct v2 vso = v2_neg(ps);
struct v2 vsd = v2_mul(vse, (v2_dot(vso, vse) / v2_len_sq(vse)));
struct v2 pd = v2_add(ps, vsd);
f32 pd_len_sq = v2_len_sq(pd);
if (pd_len_sq < pen_len_sq) {
pen_ps_index = ps_index;
pen_pe_index = pe_index;
pen_len_sq = pd_len_sq;
}
}
/* TODO: Remove this (debugging) */
s.a = proto[pen_ps_index];
s.b = proto[pen_pe_index];
s.len = 2;
/* Find new point in dir */
DBGSTEP;
{
/* Next point is in direction of line normal pointing outwards from shape */
/* TODO: If winding order is guaranteed then this can become v2_perp_left/right */
struct v2 a = proto[pen_ps_index];
struct v2 b = proto[pen_pe_index];
struct v2 n = proto[(pen_pe_index < proto_count - 1) ? (pen_pe_index + 1) : 0]; /* Next point along prototype after edge */
struct v2 vab = v2_sub(b, a);
struct v2 vna = v2_sub(a, n);
dir = v2_perp_towards_dir(vab, vna);
}
m = menkowski_point(shape0, shape1, dir);
/* Check unique */
/* TODO: Better */
{
b32 unique = true;
for (u32 i = 0; i < proto_count; ++i) {
struct v2 p = proto[i];
if (v2_len_sq(v2_sub(p, m)) < min_unique_pt_dist_sq) {
unique = false;
break;
}
}
if (!unique) {
#if 0
f32 len = v2_len(normal);
if (len < min_pen_len) {
colliding = false;
} else {
normal = v2_mul(normal, 1.f / len);
}
break;
#elif 0
/* Re-do normal calculation in high precision (for accuracy with small separation values) */
struct v2_64 ps = V2_64_FROM_V2(proto[pen_ps_index]);
struct v2_64 pe = V2_64_FROM_V2(proto[pen_pe_index]);
struct v2_64 vse = v2_sub64(pe, ps);
struct v2_64 vso = v2_neg64(ps);
struct v2_64 vsd = v2_mul64(vse, (v2_dot64(vso, vse) / v2_len_sq64(vse)));
struct v2_64 normal64 = v2_add64(ps, vsd);
normal64 = v2_norm64(normal64);
normal = V2(normal64.x, normal64.y);
break;
#else
normal = v2_norm(dir);
break;
#endif
}
}
/* Insert point into prototype */
/* FIXME: Preserve winding order */
arena_push(scratch.arena, struct gjk_menkowski_point);
++proto_count;
for (u32 i = proto_count - 1; i > pen_pe_index; --i) {
u32 shift_from = (i > 0) ? i - 1 : proto_count - 1;
u32 shift_to = i;
proto[shift_to] = proto[shift_from];
}
proto[pen_pe_index] = m;
}
} else {
if (s.len == 1) {
/* TODO? */
} else {
ASSERT(s.len == 2);
#if 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, vao));
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));
#else
normal = v2_norm(dir);
#endif
colliding = true;
}
#endif
}
}
if (colliding) {
/* ========================== *
* Clipping
* ========================== */
/* FIXME: Limit max vertices in shape structure to at least < 16 for id generation to be correct */
ASSERT(shape0.count <= 16);
ASSERT(shape1.count <= 16);
DBGSTEP;
{
const f32 wedge_epsilon = 0.001f;
/* shape0 a -> b winding = clockwise */
u32 id_a0;
u32 id_b0;
struct v2 a0;
struct v2 b0;
struct v2 vab0;
/* shape1 a -> b winding = counterclockwise */
u32 id_a1;
u32 id_b1;
struct v2 a1;
struct v2 b1;
struct v2 vab1;
{
u32 p_i = poly_support_point_index(shape0, normal);
u32 a_i = (p_i > 0) ? (p_i - 1) : (shape0.count - 1);
u32 b_i = ((p_i + 1) < shape0.count) ? (p_i + 1) : 0;
struct v2 p = shape0.points[p_i];
struct v2 a = shape0.points[a_i];
struct v2 b = shape0.points[b_i];
struct v2 vap = v2_sub(p, a);
struct v2 vpb = v2_sub(b, p);
/* FIXME: Make winding order independent */
if (v2_wedge(vap, normal) < (v2_wedge(vpb, normal) + wedge_epsilon)) {
id_a0 = a_i;
id_b0 = p_i;
a0 = a;
b0 = p;
vab0 = vap;
} else {
id_a0 = p_i;
id_b0 = b_i;
a0 = p;
b0 = b;
vab0 = vpb;
}
}
{
struct v2 neg_normal = v2_neg(normal);
u32 p_i = poly_support_point_index(shape1, neg_normal);
u32 a_i = ((p_i + 1) < shape1.count) ? (p_i + 1) : 0;
u32 b_i = (p_i > 0) ? (p_i - 1) : (shape1.count - 1);
struct v2 p = shape1.points[p_i];
struct v2 a = shape1.points[a_i];
struct v2 b = shape1.points[b_i];
struct v2 vap = v2_sub(p, a);
struct v2 vpb = v2_sub(b, p);
/* FIXME: Make winding order independent */
if (v2_wedge(vap, normal) < (v2_wedge(vpb, normal) + wedge_epsilon)) {
id_a1 = a_i;
id_b1 = p_i;
a1 = a;
b1 = p;
vab1 = vap;
} else {
id_a1 = p_i;
id_b1 = b_i;
a1 = p;
b1 = b;
vab1 = vpb;
}
}
/* Clip */
(UNUSED)a0;
(UNUSED)b0;
(UNUSED)vab0;
(UNUSED)a1;
(UNUSED)b1;
(UNUSED)vab1;
f32 a0t = -1;
f32 a1t = -1;
f32 b0t = -1;
f32 b1t = -1;
(UNUSED)a0t;
(UNUSED)a1t;
(UNUSED)b0t;
(UNUSED)b1t;
struct v2 vba0 = v2_neg(vab0);
struct v2 vba1 = v2_neg(vab1);
(UNUSED)vba0;
(UNUSED)vba1;
{
{
struct v2 va0a1 = v2_sub(a1, a0);
struct v2 va1a0 = v2_neg(va0a1);
(UNUSED)va0a1;
(UNUSED)va1a0;
{
f32 w = v2_wedge(vab0, normal);
if (w != 0) {
w = 1 / w;
a0t = v2_wedge(va0a1, normal) * w;
}
}
{
f32 w = v2_wedge(vab1, normal);
if (w != 0) {
w = 1 / w;
a1t = v2_wedge(va1a0, normal) * w;
}
}
}
{
struct v2 vb0b1 = v2_sub(b1, b0);
struct v2 vb1b0 = v2_neg(vb0b1);
{
f32 w = v2_wedge(vba0, normal);
if (w != 0) {
w = 1 / w;
b0t = v2_wedge(vb0b1, normal) * w;
}
}
{
f32 w = v2_wedge(vba1, normal);
if (w != 0) {
w = 1 / w;
b1t = v2_wedge(vb1b0, normal) * w;
}
}
}
}
a0t = clamp_f32(a0t, 0, 1);
a1t = clamp_f32(a1t, 0, 1);
b0t = clamp_f32(b0t, 0, 1);
b1t = clamp_f32(b1t, 0, 1);
struct v2 a0_clipped = v2_add(a0, v2_mul(vab0, a0t));
struct v2 a1_clipped = v2_add(a1, v2_mul(vab1, a1t));
struct v2 b0_clipped = v2_add(b0, v2_mul(vba0, b0t));
struct v2 b1_clipped = v2_add(b1, v2_mul(vba1, b1t));
struct v2 va0a1_clipped = v2_sub(a1_clipped, a0_clipped);
struct v2 vb0b1_clipped = v2_sub(b1_clipped, b0_clipped);
f32 a_sep = v2_dot(va0a1_clipped, normal);
f32 b_sep = v2_dot(vb0b1_clipped, normal);
#if 0
if (a_sep < 0) {
struct gjk_contact_point *point = &points[num_points++];
point->id = id_a0 | (id_a1 << 4);
point->separation = a_sep;
point->point = v2_add(a0_clipped, v2_mul(va0a1_clipped, 0.5f));
}
if (b_sep < 0) {
struct gjk_contact_point *point = &points[num_points++];
point->id = id_b0 | (id_b1 << 4);
point->separation = b_sep;
point->point = v2_add(b0_clipped, v2_mul(vb0b1_clipped, 0.5f));
}
#else
if (a_sep < tolerance) {
struct gjk_contact_point *point = &points[num_points++];
point->id = id_a0 | (id_a1 << 4);
point->separation = a_sep;
point->point = v2_add(a0_clipped, v2_mul(va0a1_clipped, 0.5f));
}
if (b_sep < tolerance) {
struct gjk_contact_point *point = &points[num_points++];
point->id = id_b0 | (id_b1 << 4);
point->separation = b_sep;
point->point = v2_add(b0_clipped, v2_mul(vb0b1_clipped, 0.5f));
}
#endif
}
}
res.solved = true;
abort:
/* TODO: Remove this (testing) */
//num_points = max_u32(num_points, 1);
if (proto_count > 0) {
for (u32 i = 0; i < min_u32(proto_count, ARRAY_COUNT(res.prototype.points)); ++i) {
res.prototype.points[i] = proto[i];
}
res.prototype.len = proto_count;
} else {
if (s.len >= 1) {
res.prototype.points[0] = s.a;
if (s.len >= 2) {
res.prototype.points[1] = s.b;
if (s.len >= 3) {
res.prototype.points[2] = s.c;
}
}
}
res.prototype.len = s.len;
}
res.normal = normal;
res.points[0] = points[0];
res.points[1] = points[1];
res.num_points = num_points;
res.simplex = s;
scratch_end(scratch);
return res;
}
#endif

7
src/intrinsics.h
View File

@ -14,6 +14,13 @@ INLINE f32 ix_sqrt_f32(f32 f)
return _mm_cvtss_f32(n);
}
INLINE f64 ix_sqrt_f64(f64 f)
{
__m128d n = _mm_set_sd(f);
n = _mm_sqrt_sd(_mm_setzero_pd(), n);
return _mm_cvtsd_f64(n);
}
INLINE f32 ix_rsqrt_f32(f32 f)
{
__m128 n = _mm_set_ss(f);

5
src/math.h
View File

@ -386,6 +386,11 @@ INLINE f32 math_sqrt(f32 x)
return ix_sqrt_f32(x);
}
INLINE f32 math_sqrt64(f32 x)
{
return ix_sqrt_f64(x);
}
INLINE f32 math_rsqrt(f32 x)
{
return ix_rsqrt_f32(x);

6
src/user.c
View File

@ -936,6 +936,7 @@ INTERNAL void user_update(void)
debug_draw_xform(xf);
}
#if 0
/* Draw focus arrow */
if (entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED)) {
struct sprite_sheet *sheet = sprite_sheet_from_tag_async(sprite_frame_scope, ent->sprite);
@ -946,6 +947,7 @@ INTERNAL void user_update(void)
end = xform_mul_v2(G.world_view, end);
draw_solid_arrow_line(G.viewport_canvas, start, end, 3, 10, RGBA_32_F(1, 1, 1, 0.5));
}
#endif
/* Draw slices */
if (!sprite_tag_is_nil(ent->sprite)) {
@ -1030,7 +1032,7 @@ INTERNAL void user_update(void)
(UNUSED)e1_quad;
(UNUSED)e1_poly;
#if 1
#if 0
/* Draw menkowski */
{
@ -1159,7 +1161,7 @@ INTERNAL void user_update(void)
FMT_HEX(contact.id),
FMT_FLOAT_P(contact.normal_impulse, 3),
FMT_FLOAT_P(contact.tangent_impulse, 3),
FMT_FLOAT_P(contact.starting_separation, 3));
FMT_FLOAT_P(contact.starting_separation, 6));
draw_text(G.viewport_canvas, disp_font, v2_add(v2_round(xform_mul_v2(G.world_view, point)), V2(0, offset_px)), text);