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

working face clipping along normal in gjk_contact_points

Browse Source
This commit is contained in:
jacob 2024-09-24 18:55:41 -05:00
parent 88334f2ab1
commit a6412ba4f1
7 changed files with 595 additions and 96 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/draw.c
View File

@ -246,7 +246,7 @@ void draw_solid_arrow_line(struct renderer_canvas *canvas, struct v2 start, stru
struct v2 head_start = v2_add(end, head_start_dir); struct v2 head_start = v2_add(end, head_start_dir);
struct v2 head_p1_dir = v2_perp_mul(head_start_dir, head_width_ratio); struct v2 head_p1_dir = v2_perp_right_mul(head_start_dir, head_width_ratio);
struct v2 head_p2_dir = v2_neg(head_p1_dir); struct v2 head_p2_dir = v2_neg(head_p1_dir);
struct v2 head_p1 = v2_add(head_start, head_p1_dir); struct v2 head_p1 = v2_add(head_start, head_p1_dir);

1
src/entity.h
View File

@ -66,6 +66,7 @@ struct entity_store {
struct contact { struct contact {
struct v2 point; struct v2 point;
u32 id;
f32 separation; /* How far is p0 from p1 along normal */ f32 separation; /* How far is p0 from p1 along normal */
f32 accumulated_impulse; /* Accumulated impulse along normal */ f32 accumulated_impulse; /* Accumulated impulse along normal */

42
src/game.c
View File

@ -133,8 +133,8 @@ INTERNAL void spawn_test_entities(void)
//struct v2 pos = V2(0.374142020941, -0.246118023992); /* Touching glitch spot */ //struct v2 pos = V2(0.374142020941, -0.246118023992); /* Touching glitch spot */
//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 / 4; f32 r = PI / 4;
f32 r = PI / 3; //f32 r = PI / 3;
//f32 r = PI / 2; //f32 r = PI / 2;
//f32 r = 0; //f32 r = 0;
f32 skew = 0; f32 skew = 0;
@ -196,8 +196,8 @@ INTERNAL void spawn_test_entities(void)
if (!G.box_spawned) { if (!G.box_spawned) {
G.box_spawned = true; G.box_spawned = true;
//struct v2 pos = V2(0.5, -0.5); //struct v2 pos = V2(0.5, -1);
struct v2 pos = V2(0.5, -1); struct v2 pos = V2(1, -1);
struct v2 size = V2(1, 1); struct v2 size = V2(1, 1);
//f32 rot = PI / 4; //f32 rot = PI / 4;
f32 rot = 0; f32 rot = 0;
@ -356,13 +356,12 @@ INTERNAL void create_contact_manifolds(void)
manifold->solved = res.solved; manifold->solved = res.solved;
} }
if (res.num_pairs > 0) { if (res.num_points > 0) {
if (!manifold->valid) { if (!manifold->valid) {
manifold = entity_alloc(root); manifold = entity_alloc(root);
manifold->manifold_e0 = e0->handle; manifold->manifold_e0 = e0->handle;
manifold->manifold_e1 = e1->handle; manifold->manifold_e1 = e1->handle;
/* TODO: Should we recalculate normal as more contact points are added? */ /* 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); entity_enable_prop(manifold, ENTITY_PROP_MANIFOLD);
activate_now(manifold); activate_now(manifold);
@ -381,17 +380,14 @@ INTERNAL void create_contact_manifolds(void)
manifold->solved = res.solved; manifold->solved = res.solved;
} }
} }
manifold->manifold_normal = res.normal;
manifold->num_contacts = 0; manifold->num_contacts = 0;
/* Insert new contacts that aren't too close to original contacts */ /* Insert new contacts that aren't too close to original contacts */
for (u32 i = 0; i < res.num_pairs; ++i) { for (u32 i = 0; i < res.num_points; ++i) {
b32 should_insert = true; 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;
#if 0 #if 0
for (u32 j = 0; j < manifold->num_contacts; ++j) { for (u32 j = 0; j < manifold->num_contacts; ++j) {
struct contact *contact = &manifold->contacts[j]; struct contact *contact = &manifold->contacts[j];
@ -410,12 +406,14 @@ INTERNAL void create_contact_manifolds(void)
struct contact *c; struct contact *c;
c = &manifold->contacts[manifold->num_contacts++]; c = &manifold->contacts[manifold->num_contacts++];
f32 depth = v2_len(v2_sub(p1_world, p0_world)); u32 id = res.points[i].id;
struct v2 point = v2_mul(v2_add(p0_world, p1_world), 0.5f); struct v2 point = res.points[i].point;
f32 separation = res.points[i].separation;
MEMZERO_STRUCT(c); MEMZERO_STRUCT(c);
c->id = id;
c->point = point; c->point = point;
c->separation = depth; c->separation = separation;
} }
} }
@ -481,12 +479,6 @@ INTERNAL void create_contact_manifolds(void)
/* NOTE: This assumes that all contacts will share the same normal as first contact */
manifold->manifold_normal = v2_norm(v2_sub(res.pairs[0].p1, res.pairs[0].p0));
/* TODO: Remove this (testing) */ /* TODO: Remove this (testing) */
#if 0 #if 0
@ -578,8 +570,8 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias)
struct v2 vcp0 = v2_sub(p, e0_xf.og); struct v2 vcp0 = v2_sub(p, e0_xf.og);
struct v2 vcp1 = v2_sub(p, e1_xf.og); struct v2 vcp1 = v2_sub(p, e1_xf.og);
struct v2 vel0 = v2_add(e0->linear_velocity, v2_perp_mul(vcp0, e0->angular_velocity)); struct v2 vel0 = v2_add(e0->linear_velocity, v2_perp_right_mul(vcp0, e0->angular_velocity));
struct v2 vel1 = v2_add(e1->linear_velocity, v2_perp_mul(vcp1, e1->angular_velocity)); struct v2 vel1 = v2_add(e1->linear_velocity, v2_perp_right_mul(vcp1, e1->angular_velocity));
struct v2 vrel = v2_sub(vel1, vel0); struct v2 vrel = v2_sub(vel1, vel0);
@ -587,8 +579,8 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias)
/* FIXME: Clamp accumulated */ /* FIXME: Clamp accumulated */
vn = max_f32(vn, 0); vn = max_f32(vn, 0);
struct v2 idk0 = v2_perp_mul(vcp0, v2_wedge(vcp0, normal) * inv_i0); struct v2 idk0 = v2_perp_right_mul(vcp0, v2_wedge(vcp0, normal) * inv_i0);
struct v2 idk1 = v2_perp_mul(vcp1, v2_wedge(vcp1, normal) * inv_i1); struct v2 idk1 = v2_perp_right_mul(vcp1, v2_wedge(vcp1, normal) * inv_i1);
f32 k = inv_m0 + inv_m1 + v2_dot(normal, v2_add(idk0, idk1)); f32 k = inv_m0 + inv_m1 + v2_dot(normal, v2_add(idk0, idk1));
@ -1068,7 +1060,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
* Create forces from control focus (aim) * Create forces from control focus (aim)
* ========================== */ * ========================== */
#if 0 #if 1
for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
struct entity *ent = &store->entities[entity_index]; struct entity *ent = &store->entities[entity_index];
if (!(ent->valid && entity_has_prop(ent, ENTITY_PROP_ACTIVE))) continue; if (!(ent->valid && entity_has_prop(ent, ENTITY_PROP_ACTIVE))) continue;

541
src/gjk.c
View File

@ -9,7 +9,7 @@ u32 gjk_debug_steps = U32_MAX;
#define DBGSTEP if (dbg_step++ >= gjk_debug_steps) goto abort #define DBGSTEP if (dbg_step++ >= gjk_debug_steps) goto abort
INTERNAL struct v2 poly_support(struct v2_array a, struct v2 dir) INTERNAL struct v2 poly_support_point(struct v2_array a, struct v2 dir)
{ {
/* TODO: Could probably binary search for largest dot since shape is convex */ /* TODO: Could probably binary search for largest dot since shape is convex */
struct v2 furthest = a.points[0]; struct v2 furthest = a.points[0];
@ -25,23 +25,42 @@ INTERNAL struct v2 poly_support(struct v2_array a, struct v2 dir)
return furthest; return furthest;
} }
INTERNAL u32 poly_support_point_index(struct v2_array a, struct v2 dir)
{
u32 furthest = 0;
f32 furthest_dot = v2_dot(dir, a.points[0]);
for (u32 i = 1; i < a.count; ++i) {
struct v2 p = a.points[i];
f32 dot = v2_dot(dir, p);
if (dot > furthest_dot) {
furthest = i;
furthest_dot = dot;
}
}
return furthest;
}
INTERNAL struct v2 menkowski_point(struct v2_array poly0, struct v2_array shape1, struct v2 dir) INTERNAL struct v2 menkowski_point(struct v2_array poly0, struct v2_array shape1, struct v2 dir)
{ {
return v2_sub(poly_support(poly0, dir), poly_support(shape1, v2_neg(dir))); return v2_sub(poly_support_point(poly0, dir), poly_support_point(shape1, v2_neg(dir)));
} }
b32 gjk_boolean(struct v2_array shape0, struct v2_array shape1) b32 gjk_boolean(struct v2_array shape0, struct v2_array shape1)
{ {
struct { struct v2 a, b, c; } s = ZI; struct { struct v2 a, b, c; } s = ZI;
/* FIXME: Infinite loop when shapes exactly overlap same space */ /* FIXME: Infinite loop when shapes exactly overlap same space? */
struct v2 dir, p;
/* First point is support point in shape's general directions to eachother */ /* 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])); dir = v2_sub(shape1.points[0], shape0.points[0]);
if (v2_is_zero(dir)) dir = V2(1, 0);
s.a = menkowski_point(shape0, shape1, dir);
/* Second point is support point towards origin */ /* Second point is support point towards origin */
struct v2 dir = v2_neg(s.a); dir = v2_neg(s.a);
struct v2 p = menkowski_point(shape0, shape1, dir); p = menkowski_point(shape0, shape1, dir);
if (v2_dot(dir, p) >= 0) { if (v2_dot(dir, p) >= 0) {
s.b = s.a; s.b = s.a;
s.a = p; s.a = p;
@ -82,25 +101,6 @@ b32 gjk_boolean(struct v2_array shape0, struct v2_array shape1)
return false; return false;
} }
INTERNAL struct gjk_menkowski_point menkowski_point_extended(struct v2_array poly0, struct v2_array poly1, struct v2 dir)
{
struct gjk_menkowski_point res;
res.p0 = poly_support(poly0, dir);
res.p1 = poly_support(poly1, v2_neg(dir));
res.p = v2_sub(res.p0, res.p1);
return res;
}
@ -115,7 +115,7 @@ INTERNAL struct gjk_menkowski_point menkowski_point_extended(struct v2_array pol
* TODO: Remove these * TODO: Remove these
* ========================== */ * ========================== */
/* TODO: Remove this (debugging) */ /* TODO: Remove this (debugging) */
struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1) struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1)
{ {
struct v2_array res = { .points = arena_dry_push(arena, struct v2) }; struct v2_array res = { .points = arena_dry_push(arena, struct v2) };
@ -123,7 +123,7 @@ struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_
for (u64 i = 0; i < rays; ++i) { for (u64 i = 0; i < rays; ++i) {
f32 angle = ((f32)i / rays) * (2 * PI); f32 angle = ((f32)i / rays) * (2 * PI);
struct v2 dir = v2_from_angle(angle); struct v2 dir = v2_from_angle(angle);
struct v2 p = menkowski_point_extended(poly0, poly1, dir).p; struct v2 p = menkowski_point(poly0, poly1, dir);
if (res.count == 0 || !v2_eq(p, res.points[res.count - 1])) { if (res.count == 0 || !v2_eq(p, res.points[res.count - 1])) {
*arena_push(arena, struct v2) = p; *arena_push(arena, struct v2) = p;
++res.count; ++res.count;
@ -156,6 +156,444 @@ struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_arra
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: Set all epsilons used in this function to 0.005 */
/* TODO: Verify epsilon */
/* FIXME: Infinite loop when epsilon too low and a shape is too far from 0 (precision issue) */
const f32 epsilon = 0.0000100f;
struct gjk_simplex s = ZI;
b32 colliding = false;
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
* ========================== */
{
/* FIXME: Collision is false when 2 shapes exactly overlap */
/* 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);
s.a = menkowski_point(shape0, shape1, dir);
s.len = 1;
/* Second point is support point towards origin */
DBGSTEP;
dir = v2_neg(s.a);
m = menkowski_point(shape0, shape1, dir);
if (v2_dot(dir, m) > 0) {
DBGSTEP;
s.b = s.a;
s.a = m;
s.len = 2;
while (true) {
/* Third point is support point in direction of line normal towards origin */
DBGSTEP;
dir = v2_perp_towards_dir(v2_sub(s.b, s.a), v2_neg(s.a));
m = menkowski_point(shape0, shape1, dir);
if (v2_dot(dir, m) < 0) {
/* New point did not cross origin, collision impossible */
break;
}
s.c = s.b;
s.b = s.a;
s.a = m;
s.len = 3;
struct v2 vab = v2_sub(s.b, s.a);
struct v2 vac = v2_sub(s.c, s.a);
struct v2 a_to_origin = v2_neg(s.a);
DBGSTEP;
dir = v2_perp_towards_dir(vab, v2_neg(vac)); /* Normal of ab pointing away from c */
if (v2_dot(dir, a_to_origin) > 0) {
/* Point is in region ab, remove c from simplex */
s.len = 2;
} else {
/* Point is not in region ab */
dir = v2_perp_towards_dir(vac, v2_neg(vab)); /* Normal of ac pointing away from b */
if (v2_dot(dir, a_to_origin) > 0) {
/* Point is in region ac, remove b from simplex */
s.b = s.c;
s.len = 2;
} else {
/* Point is in simplex */
colliding = true;
break;
}
}
}
}
}
if (colliding) {
/* ========================== *
* Epa (to find collision normal)
* ========================== */
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 (true) {
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);
f32 d1 = v2_dot(vso, vse);
f32 d2 = v2_dot(vse, vse);
struct v2 vsd = v2_mul(vse, (d1 / d2));
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;
normal = pd;
}
}
/* 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 van = v2_sub(n, a);
struct v2 vab = v2_sub(b, a);
dir = v2_perp_towards_dir(vab, v2_neg(van));
}
m = menkowski_point(shape0, shape1, dir);
/* Check unique */
/* TODO: Better */
{
b32 unique = true;
for (u32 i = 0; i < proto_count; ++i) {
struct v2 edge_start = proto[i];
struct v2 edge_end = i < proto_count - 1 ? proto[i + 1] : proto[0];
if (math_fabs(v2_wedge(v2_sub(edge_end, edge_start), v2_sub(m, edge_start))) < epsilon) {
unique = false;
break;
}
}
if (!unique) {
normal = v2_norm(normal);
break;
}
}
/* 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;
}
/* ========================== *
* Clipping
* ========================== */
/* FIXME: Limit max vertices in shape structure to 8 for id generation to be correct */
ASSERT(shape0.count <= 8);
ASSERT(shape1.count <= 8);
DBGSTEP;
{
const f32 wedge_epsilon = 0.001;
/* 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 (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));
}
}
}
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;
}
/* ========================== *
* GJK contact pairs (unused)
* ========================== */
#if 0
INTERNAL struct gjk_menkowski_point menkowski_point_extended(struct v2_array poly0, struct v2_array poly1, struct v2 dir)
{
struct gjk_menkowski_point res;
res.p0 = poly_support(poly0, dir);
res.p1 = poly_support(poly1, v2_neg(dir));
res.p = v2_sub(res.p0, res.p1);
return res;
}
struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, struct v2_array shape1) 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 temp_arena scratch = scratch_begin_no_conflict(); /* TODO: Only begin scratch for EPA */
@ -404,6 +842,7 @@ abort:
scratch_end(scratch); scratch_end(scratch);
return res; return res;
} }
#endif
@ -436,8 +875,8 @@ abort:
* ========================== */ * ========================== */
#if 0 #if 0
/* TODO: Update this function to only calculate contact pairs if colliding. Will allow for shortcut-style GJK. /* TODO: Update this function to only calculate contact pairs if colliding. Will allow for shortcut-style GJK.
* `colliding` & `has_2nd_pair` become "num_pairs", where 0 = no collision. */ * `colliding` & `has_2nd_pair` become "num_pairs", where 0 = no collision. */
struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, struct v2_array shape1) struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, struct v2_array shape1)
{ {
struct temp_arena scratch = scratch_begin_no_conflict(); struct temp_arena scratch = scratch_begin_no_conflict();
@ -828,32 +1267,39 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array
voronoi_mask |= (v2_dot(rbc_dir, v2_neg(s.b.p)) >= 0) << 2; /* Regions bc, b, and c */ voronoi_mask |= (v2_dot(rbc_dir, v2_neg(s.b.p)) >= 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: { /* No region, must be in simplex */ default:
{ /* No region, must be in simplex */
colliding = true; colliding = true;
} break; } break;
case 1: { /* Region ab, remove c */ case 1:
{ /* Region ab, remove c */
dir = rab_dir; /* Next third point is in direction of region ab */ dir = rab_dir; /* Next third point is in direction of region ab */
s.len = 2; s.len = 2;
} break; } break;
case 2: { /* Region ac, remove b */ case 2:
{ /* Region ac, remove b */
dir = rac_dir; /* Next third point is in direction of region ac */ dir = rac_dir; /* Next third point is in direction of region ac */
s.b = s.c; s.b = s.c;
s.len = 2; s.len = 2;
} break; } break;
case 4: { /* Region bc, remove a */ case 4:
{ /* Region bc, remove a */
dir = rbc_dir; /* Next third point is in direction of region bc */ dir = rbc_dir; /* Next third point is in direction of region bc */
s.a = s.b; s.a = s.b;
s.b = s.c; s.b = s.c;
s.len = 2; s.len = 2;
} break; } break;
case 3: { /* Region a, remove bc */ case 3:
{ /* Region a, remove bc */
s.len = 1; s.len = 1;
} break; } break;
case 5: { /* Region b, remove ac */ case 5:
{ /* Region b, remove ac */
s.a = s.b; s.a = s.b;
s.len = 1; s.len = 1;
} break; } break;
case 6: { /* Region c, remove ab */ case 6:
{ /* Region c, remove ab */
s.a = s.c; s.a = s.c;
s.len = 1; s.len = 1;
} break; } break;
@ -1056,7 +1502,7 @@ INTERNAL struct poly_support_swept_result poly_support_swept(struct v2_array a,
modified = v2_add(p, linear_velocity); modified = v2_add(p, linear_velocity);
} else { } else {
modified = p; modified = p;
} }
#else #else
modified = v2_add(p, linear_velocity); modified = v2_add(p, linear_velocity);
#endif #endif
@ -1194,32 +1640,39 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array
voronoi_mask |= (v2_dot(rbc_dir, v2_neg(s.b.p)) >= 0) << 2; /* Regions bc, b, and c */ voronoi_mask |= (v2_dot(rbc_dir, v2_neg(s.b.p)) >= 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: { /* No region, must be in simplex */ default:
{ /* No region, must be in simplex */
colliding = true; colliding = true;
} break; } break;
case 1: { /* Region ab, remove c */ case 1:
{ /* Region ab, remove c */
dir = rab_dir; /* Next third point is in direction of region ab */ dir = rab_dir; /* Next third point is in direction of region ab */
s.len = 2; s.len = 2;
} break; } break;
case 2: { /* Region ac, remove b */ case 2:
{ /* Region ac, remove b */
dir = rac_dir; /* Next third point is in direction of region ac */ dir = rac_dir; /* Next third point is in direction of region ac */
s.b = s.c; s.b = s.c;
s.len = 2; s.len = 2;
} break; } break;
case 4: { /* Region bc, remove a */ case 4:
{ /* Region bc, remove a */
dir = rbc_dir; /* Next third point is in direction of region bc */ dir = rbc_dir; /* Next third point is in direction of region bc */
s.a = s.b; s.a = s.b;
s.b = s.c; s.b = s.c;
s.len = 2; s.len = 2;
} break; } break;
case 3: { /* Region a, remove bc */ case 3:
{ /* Region a, remove bc */
s.len = 1; s.len = 1;
} break; } break;
case 5: { /* Region b, remove ac */ case 5:
{ /* Region b, remove ac */
s.a = s.b; s.a = s.b;
s.len = 1; s.len = 1;
} break; } break;
case 6: { /* Region c, remove ab */ case 6:
{ /* Region c, remove ab */
s.a = s.c; s.a = s.c;
s.len = 1; s.len = 1;
} break; } break;

39
src/gjk.h
View File

@ -13,19 +13,41 @@ struct gjk_menkowski_point {
struct v2 p; /* Menkowski difference point */ struct v2 p; /* Menkowski difference point */
}; };
/* Returns simple true or false indicating shape collision */
b32 gjk_boolean(struct v2_array shape0, struct v2_array shape1);
struct gjk_simplex {
u32 len;
struct v2 a, b, c;
};
struct gjk_contact_point {
struct v2 point;
f32 separation;
u32 id; /* Based on polygon edge-to-edge */
};
struct gjk_prototype { struct v2 points[64]; u32 len; };
struct gjk_contact_points_result {
struct v2 normal;
struct gjk_contact_point points[2];
u32 num_points;
/* For debugging */
b32 solved;
struct gjk_simplex simplex;
struct gjk_prototype prototype;
};
struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, struct v2_array shape1);
#if 0
struct gjk_simplex { struct gjk_simplex {
u32 len; u32 len;
struct gjk_menkowski_point a, b, c; struct gjk_menkowski_point a, b, c;
}; };
/* Returns simple true or false indicating shape collision */
b32 gjk_boolean(struct v2_array shape0, struct v2_array shape1);
struct gjk_contact_pair {
struct v2 p0, p1;
};
struct gjk_prototype { struct v2 points[64]; u32 len; };
struct gjk_contact_points_result { struct gjk_contact_points_result {
struct gjk_contact_pair pairs[2]; struct gjk_contact_pair pairs[2];
@ -38,6 +60,7 @@ struct gjk_contact_points_result {
}; };
struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, struct v2_array shape1); struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, struct v2_array shape1);
#endif
struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1); struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1);

27
src/math.h
View File

@ -640,27 +640,42 @@ INLINE f32 v2_dot(struct v2 a, struct v2 b)
return a.x * b.x + a.y * b.y; return a.x * b.x + a.y * b.y;
} }
/* Returns signed area between vectors (positive in clockwise direction)
* AKA perpendicular dot product
* AKA 2d cross-product */
INLINE f32 v2_wedge(struct v2 a, struct v2 b) INLINE f32 v2_wedge(struct v2 a, struct v2 b)
{ {
return a.x * b.y - a.y * b.x; return a.x * b.y - a.y * b.x;
} }
/* Right (clockwise) perpendicular vector */ /* Clockwise perpendicular vector */
INLINE struct v2 v2_perp(struct v2 a) INLINE struct v2 v2_perp_right(struct v2 a)
{ {
return V2(-a.y, a.x); return V2(-a.y, a.x);
} }
/* Right (clockwise) perpendicular vector scaled by s */ /* Counter-clockwise perpendicular vector */
INLINE struct v2 v2_perp_mul(struct v2 a, f32 s) INLINE struct v2 v2_perp_left(struct v2 a)
{
return V2(a.y, -a.x);
}
/* Clockwise perpendicular vector scaled by s */
INLINE struct v2 v2_perp_right_mul(struct v2 a, f32 s)
{ {
return V2(s * -a.y, s * a.x); return V2(s * -a.y, s * a.x);
} }
/* Counter-clockwise perpendicular vector scaled by s */
INLINE struct v2 v2_perp_left_mul(struct v2 a, f32 s)
{
return V2(s * a.y, s * -a.x);
}
INLINE struct v2 v2_perp_towards_dir(struct v2 v, struct v2 dir) INLINE struct v2 v2_perp_towards_dir(struct v2 v, struct v2 dir)
{ {
f32 wedge = v2_wedge(v, dir); f32 wedge = v2_wedge(v, dir);
return v2_perp_mul(v, (wedge >= 0) - (wedge < 0)); return v2_perp_right_mul(v, (wedge >= 0) - (wedge < 0));
} }
INLINE struct v2 v2_norm(struct v2 a) INLINE struct v2 v2_norm(struct v2 a)
@ -1085,7 +1100,7 @@ INLINE struct quad quad_from_line(struct v2 start, struct v2 end, f32 thickness)
f32 width = thickness / 2.f; f32 width = thickness / 2.f;
struct v2 dir = v2_norm(v2_sub(end, start)); struct v2 dir = v2_norm(v2_sub(end, start));
struct v2 dir_perp = v2_perp(dir); struct v2 dir_perp = v2_perp_right(dir);
struct v2 left = v2_mul(dir_perp, -width); struct v2 left = v2_mul(dir_perp, -width);
struct v2 right = v2_mul(dir_perp, width); struct v2 right = v2_mul(dir_perp, width);

39
src/user.c
View File

@ -1029,7 +1029,7 @@ INTERNAL void user_update(void)
(UNUSED)e1_quad; (UNUSED)e1_quad;
(UNUSED)e1_poly; (UNUSED)e1_poly;
#if 1 #if 0
/* Draw menkowski */ /* Draw menkowski */
{ {
@ -1093,7 +1093,7 @@ INTERNAL void user_update(void)
u32 color_third = RGBA_32_F(0, 0, 1, 0.75); u32 color_third = RGBA_32_F(0, 0, 1, 0.75);
struct gjk_simplex simplex = ent->simplex; struct gjk_simplex simplex = ent->simplex;
struct v2 simplex_points[] = { simplex.a.p, simplex.b.p, simplex.c.p }; struct v2 simplex_points[] = { simplex.a, simplex.b, simplex.c };
for (u64 i = 0; i < ARRAY_COUNT(simplex_points); ++i) simplex_points[i] = xform_mul_v2(G.world_view, simplex_points[i]); for (u64 i = 0; i < ARRAY_COUNT(simplex_points); ++i) simplex_points[i] = xform_mul_v2(G.world_view, simplex_points[i]);
struct v2_array simplex_array = { .count = simplex.len, .points = simplex_points }; struct v2_array simplex_array = { .count = simplex.len, .points = simplex_points };
@ -1119,11 +1119,21 @@ INTERNAL void user_update(void)
/* Draw contacts */ /* Draw contacts */
{ {
f32 radius = 5; f32 radius = 5;
u32 colors[2] = { 0 };
if (entity_has_prop(e0, ENTITY_PROP_PLAYER_CONTROLLED)) {
colors[0] = RGBA_32_F(1, 0, 0, 0.50);
colors[1] = RGBA_32_F(1, 1, 0, 0.50);
} else {
colors[0] = RGBA_32_F(1, 0, 1, 0.50);
colors[1] = RGBA_32_F(0, 1, 1, 0.50);
}
for (u32 i = 0; i < ent->num_contacts; ++i) { for (u32 i = 0; i < ent->num_contacts; ++i) {
struct contact contact = ent->contacts[i]; struct contact contact = ent->contacts[i];
/* Draw point */ /* Draw point */
{ {
u32 color = entity_has_prop(e0, ENTITY_PROP_PLAYER_CONTROLLED) ? RGBA_32_F(1, 0, 0, 0.50) : RGBA_32_F(0, 1, 1, 0.50); u32 color = i < ARRAY_COUNT(colors) ? colors[i] : COLOR_RED;
//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;
struct v2 point = contact.point; struct v2 point = contact.point;
@ -1140,17 +1150,22 @@ INTERNAL void user_update(void)
struct v2 end = xform_mul_v2(G.world_view, v2_add(contact.point, v2_mul(v2_norm(ent->manifold_normal), len))); struct v2 end = xform_mul_v2(G.world_view, v2_add(contact.point, v2_mul(v2_norm(ent->manifold_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);
} }
/* Draw id */
#if 0
{ {
u32 color = entity_has_prop(e1, ENTITY_PROP_PLAYER_CONTROLLED) ? RGBA_32_F(1, 0, 0, 0.50) : RGBA_32_F(0, 1, 1, 0.50); struct font *disp_font = font_load_async(STR("res/fonts/fixedsys.ttf"), 12.0f);
//struct v2 point = xform_mul_v2(e1_xf, contact.p1_local); if (disp_font) {
//struct v2 point = contact.p1_initial_world; f32 offset_px = -20;
struct v2 point = contact.point; u32 id = contact.id;
point = xform_mul_v2(G.world_view, point);
draw_solid_circle(G.viewport_canvas, point, radius, color, 10); struct string fmt = STR(
"id: 0x%F"
);
struct string text = string_format(temp.arena, fmt, FMT_HEX(id));
draw_text(G.viewport_canvas, disp_font, v2_add(v2_round(xform_mul_v2(G.world_view, contact.point)), V2(0, offset_px)), text);
}
} }
#endif
} }
} }
#endif #endif