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

gjk.c & gjk.h

Browse Source
This commit is contained in:
jacob 2024-09-03 15:59:00 -05:00
parent c3b96d1597
commit 9cb34ea1cd
8 changed files with 450 additions and 537 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/entity.h
View File

@ -94,7 +94,6 @@ struct entity {
/* TODO: Remove this (testing) */
b32 colliding;
struct entity_handle colliding_with;
struct gjk_simplex simplex;
struct v2 point;

12
src/game.c
View File

@ -10,6 +10,7 @@
#include "atomic.h"
#include "app.h"
#include "log.h"
#include "gjk.h"
GLOBAL struct {
struct atomic_i32 game_thread_shutdown;
@ -778,7 +779,6 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
struct v2 point0 = V2(0, 0);
struct v2 point1 = V2(0, 0);
struct entity *colliding_with = entity_nil();
struct gjk_simplex final_simplex = { 0 };
for (u64 e1_index = 0; e1_index < store->reserved; ++e1_index) {
struct entity *e1 = &store->entities[e1_index];
if (e1 == e0) continue;
@ -799,14 +799,6 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
};
}
#if 0
struct gjk_simplex res = gjk_extended(e0_poly, e1_poly, G.gjk_steps);
colliding = res.colliding;
point0 = res.p0;
point1 = res.p1;
colliding_with = e1;
final_simplex = res.final_simplex;
#else
struct v2 pendir = V2(0, 0);
//struct v2 pendir = V2(0, 99999);
@ -817,7 +809,6 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
point1 = res.p1;
colliding_with = e1;
//final_simplex = res.simplex;
#endif
if (colliding) {
@ -837,7 +828,6 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
e0->colliding = colliding;
e0->colliding_with = colliding_with->handle;
e0->point = point0;
e0->simplex = final_simplex;
if (colliding_with->valid) {
colliding_with->colliding = colliding;

405
src/gjk.c Normal file
View File

@ -0,0 +1,405 @@
#include "gjk.h"
#include "math.h"
#include "arena.h"
#include "scratch.h"
struct menkowski_point {
struct v2 p0; /* Support point of first shape in dir */
struct v2 p1; /* Support point of second shape in -dir */
struct v2 p; /* Menkowski difference point */
};
struct simplex {
u32 len;
struct menkowski_point a, b, c;
};
INTERNAL struct v2 poly_support(struct v2_array a, struct v2 dir)
{
/* TODO: Could probably binary search for largest dot since shape is convex */
struct v2 furthest = a.points[0];
f32 furthest_dot = v2_dot(dir, furthest);
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 = p;
furthest_dot = dot;
}
}
return furthest;
}
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)));
}
INTERNAL struct menkowski_point menkowski_point_extended(struct v2_array poly0, struct v2_array poly1, struct v2 dir)
{
struct 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;
}
b32 gjk_boolean(struct v2_array shape0, struct v2_array shape1)
{
struct { struct v2 a, b, c; } s = { 0 };
/* 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]));
/* Second point is support point towards origin */
struct v2 dir = v2_neg(s.a);
struct v2 p = menkowski_point(shape0, shape1, dir);
if (v2_dot(dir, p) >= 0) {
s.b = s.a;
s.a = p;
while (true) {
/* 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));
p = menkowski_point(shape0, shape1, dir);
if (v2_dot(dir, p) < 0) {
/* New point did not cross origin, collision impossible */
break;
}
s.c = s.b;
s.b = s.a;
s.a = p;
dir = v2_perp_towards_dir(v2_sub(s.b, s.a), v2_neg(v2_sub(s.c, s.a))); /* Normal of ab pointing away from c */
struct v2 a_to_origin = v2_neg(s.a);
if (v2_dot(dir, a_to_origin) >= 0) {
/* Point is in region ab, remove c from simplex (will happen automatically next iteration) */
} else {
/* Point is not in region ab */
dir = v2_perp_towards_dir(v2_sub(s.c, s.a), v2_neg(v2_sub(s.b, s.a))); /* 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;
} else {
/* Point is in simplex */
return true;
}
}
}
}
return false;
}
struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1, struct v2 penetration_dir)
{
struct gjk_extended_result res = { 0 };
/* TODO: Verify epsilon */
f32 unique_epsilon = 0.00001;
b32 use_penetration_dir = false;
struct simplex s = { 0 };
/* ========================== *
* GJK collision check
* ========================== */
struct v2 dir = { 0 };
struct menkowski_point m = { 0 };
{
/* First point is support point in shape's general directions to eachother */
s.a = menkowski_point_extended(shape0, shape1, v2_sub(shape1.points[0], shape0.points[0]));
s.len = 1;
/* Second point is support point towards origin */
dir = v2_neg(s.a.p);
m = menkowski_point_extended(shape0, shape1, dir);
if (v2_dot(dir, m.p) >= 0) {
s.b = s.a;
s.a = m;
s.len = 2;
while (true) {
/* Third point is support point in direction of line normal towards origin */
dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p));
m = menkowski_point_extended(shape0, shape1, dir);
if (v2_dot(dir, m.p) < 0) {
/* New point did not cross origin, collision impossible */
break;
}
s.c = s.b;
s.b = s.a;
s.a = m;
s.len = 3;
dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), v2_neg(v2_sub(s.c.p, s.a.p))); /* Normal of ab pointing away from c */
struct v2 a_to_origin = v2_neg(s.a.p);
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(v2_sub(s.c.p, s.a.p), v2_neg(v2_sub(s.b.p, s.a.p))); /* 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 {
res.colliding = true;
break;
}
}
}
}
}
if (res.colliding) {
use_penetration_dir = !v2_eq(penetration_dir, V2(0, 0));
if (use_penetration_dir) {
/* ========================== *
* Move simplex towards penetration dir
* ========================== */
while (true) {
/* Second point is support point towards penetration_dir */
if (s.len == 1) {
dir = v2_sub(v2_mul(penetration_dir, v2_dot(penetration_dir, s.a.p) / v2_dot(penetration_dir, penetration_dir)), s.a.p);
m = menkowski_point_extended(shape0, shape1, dir);
if (v2_eq(m.p, s.a.p)) {
break;
}
s.b = s.a;
s.a = m;
s.len = 2;
/* Third point is support point in direction of line normal towards `a` projected onto penetration_dir */
dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), penetration_dir);
}
if (s.len == 2) {
m = menkowski_point_extended(shape0, shape1, dir);
if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_sub(m.p, s.a.p))) < unique_epsilon) {
/* New point is already on the current line */
break;
}
s.c = s.b;
s.b = s.a;
s.a = m;
s.len = 3;
}
i32 a_wedgesign = math_fsign(v2_wedge(penetration_dir, s.a.p));
i32 b_wedgesign = math_fsign(v2_wedge(penetration_dir, s.b.p));
i32 c_wedgesign = math_fsign(v2_wedge(penetration_dir, s.c.p));
if (a_wedgesign != b_wedgesign) {
/* Remove c */
dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), penetration_dir);
s.len = 2;
} else if (b_wedgesign != c_wedgesign) {
/* Remove b */
dir = v2_perp_towards_dir(v2_sub(s.c.p, s.a.p), penetration_dir);
s.b = s.c;
s.len = 2;
} else {
/* Remove a */
dir = v2_perp_towards_dir(v2_sub(s.c.p, s.b.p), penetration_dir);
s.a = s.b;
s.b = s.c;
s.len = 2;
}
}
} else {
/* ========================== *
* Expand simplex towards closest edge (EPA)
* ========================== */
struct temp_arena scratch = scratch_begin_no_conflict();
struct menkowski_point *proto = arena_dry_push(scratch.arena, struct menkowski_point);
u32 proto_count = 0;
{
ASSERT(s.len == 3);
struct menkowski_point *tmp = arena_push_array(scratch.arena, struct menkowski_point, 3);
tmp[0] = s.a;
tmp[1] = s.b;
tmp[2] = s.c;
proto_count = 3;
}
while (true) {
struct v2 pen = V2(0, 0);
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].p;
struct v2 pe = proto[pe_index].p;
struct v2 vse = v2_sub(pe, ps);
struct v2 vso = v2_neg(ps);
struct v2 vsd = v2_mul(vse, (v2_dot(vso, vse) / v2_dot(vse, 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 = pd;
pen_len_sq = pd_len_sq;
}
}
/* Find new point in dir */
m = menkowski_point_extended(shape0, shape1, pen);
/* Check unique */
/* TODO: Better */
{
b32 unique = true;
for (u32 i = 0; i < proto_count; ++i) {
struct v2 edge_start = proto[i].p;
struct v2 edge_end = i < proto_count - 1 ? proto[i + 1].p : proto[0].p;
if (math_fabs(v2_wedge(v2_sub(edge_end, edge_start), v2_sub(m.p, edge_start))) < unique_epsilon) {
unique = false;
break;
}
}
if (!unique) {
s.a = proto[pen_ps_index];
s.b = proto[pen_pe_index];
s.len = 2;
break;
}
}
/* Insert point into prototype */
/* FIXME: Preserve winding order */
arena_push(scratch.arena, struct v2);
++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;
}
scratch_end(scratch);
}
} else {
/* ========================== *
* Move simplex towards closest edge to origin
* ========================== */
if (s.len == 2) {
/* Third point is support point in direction of line normal towards `a` projected onto penetration_dir */
dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), penetration_dir);
}
while (true) {
/* Second point is support point towards origin */
if (s.len == 1) {
m = menkowski_point_extended(shape0, shape1, v2_neg(s.a.p));
if (v2_eq(m.p, s.a.p)) {
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.p, s.a.p), v2_neg(s.a.p));
}
if (s.len == 2) {
m = menkowski_point_extended(shape0, shape1, dir);
if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_sub(m.p, s.a.p))) < unique_epsilon) {
/* New point is already on the current line */
break;
}
s.c = s.b;
s.b = s.a;
s.a = m;
s.len = 3;
}
/* Remove point or edge and determine next direction based on voronoi regions */
struct v2 rab_dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), v2_neg(v2_sub(s.c.p, s.a.p)));
struct v2 rac_dir = v2_perp_towards_dir(v2_sub(s.c.p, s.a.p), v2_neg(v2_sub(s.b.p, s.a.p)));
struct v2 rbc_dir = v2_perp_towards_dir(v2_sub(s.c.p, s.b.p), v2_neg(v2_sub(s.a.p, s.b.p)));
i32 voronoi_mask = 0;
voronoi_mask |= ((v2_dot(rab_dir, v2_neg(s.a.p)) >= 0) << 0); /* Regions ab, a, and b*/
voronoi_mask |= ((v2_dot(rac_dir, v2_neg(s.a.p)) >= 0) << 1); /* Regions ac, a, and c */
voronoi_mask |= ((v2_dot(rbc_dir, v2_neg(s.b.p)) >= 0) << 2); /* Regions bc, b, and c */
switch (voronoi_mask) {
case 1: { /* Region ab, remove c */
dir = rab_dir;
s.len = 2;
} break;
case 2: { /* Region ac, remove b */
dir = rac_dir;
s.b = s.c;
s.len = 2;
} break;
case 4: { /* Region bc, remove a */
dir = rbc_dir;
s.a = s.b;
s.b = s.c;
s.len = 2;
} break;
case 3: { /* Region a, remove bc */
s.len = 1;
} break;
case 5: { /* Region b, remove ac */
s.a = s.b;
s.len = 1;
} break;
case 6: { /* Region c, remove ab */
s.a = s.c;
s.len = 1;
} break;
}
}
}
/* Resolve points */
if (s.len == 1) {
res.p0 = s.a.p0;
res.p1 = s.a.p1;
} else if (s.len == 2) {
/* FIXME: Winding order dependent? */
f32 ratio;
if (use_penetration_dir) {
/* Determine ratio between edge a & b that penetration dir intersection lies */
f32 wedgea = math_fabs(v2_wedge(penetration_dir, s.a.p));
f32 wedgeb = math_fabs(v2_wedge(penetration_dir, s.b.p));
ratio = wedgea / (wedgea + wedgeb);
} else {
/* Determine ratio between edge a & b that projected origin lies */
struct v2 vab = v2_sub(s.b.p, s.a.p);
struct v2 vao = v2_neg(s.a.p);
ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1);
}
/* Shape 0 */
res.p0 = v2_sub(s.b.p0, s.a.p0);
res.p0 = v2_mul(res.p0, ratio);
res.p0 = v2_add(res.p0, s.a.p0);
/* Shape 1 */
res.p1 = v2_sub(s.b.p1, s.a.p1);
res.p1 = v2_mul(res.p1, ratio);
res.p1 = v2_add(res.p1, s.a.p1);
}
return res;
}

20
src/gjk.h Normal file
View File

@ -0,0 +1,20 @@
#ifndef GJK_H
#define GJK_H
struct gjk_extended_result {
b32 colliding;
struct v2 p0, p1; /* Closest points (or penetrating points if colliding) on each shape */
};
/* Returns simple true or false indicating shape collision */
b32 gjk_boolean(struct v2_array shape0, struct v2_array shape1);
/* Returns shape whether shapes are colliding well as closest / penetrating points on each shape.
*
* If shapes are colliding and `penetration_dir` is non-zero, the shortest
* direction to resolve the collision will be used to calculate penetrating
* points. Otherwise, the penetrating points will be calculated using the
* supplied direction. */
struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1, struct v2 penetration_dir);
#endif

29
src/math.h
View File

@ -635,6 +635,16 @@ INLINE f32 v2_len_sq(struct v2 a)
return a.x * a.x + a.y * a.y;
}
INLINE f32 v2_dot(struct v2 a, struct v2 b)
{
return a.x * b.x + a.y * b.y;
}
INLINE f32 v2_wedge(struct v2 a, struct v2 b)
{
return a.x * b.y - a.y * b.x;
}
/* Clockwise perpendicular vector */
INLINE struct v2 v2_perp_cw(struct v2 a)
{
@ -647,9 +657,16 @@ INLINE struct v2 v2_perp_ccw(struct v2 a)
return V2(a.y, -a.x);
}
INLINE struct v2 v2_perp_towards_dir(struct v2 v, struct v2 dir)
{
struct v2 perp = v2_perp_cw(v);
i32 sign = 1 - ((v2_dot(perp, dir) < 0) << 1);
return v2_mul(perp, sign);
}
INLINE struct v2 v2_norm(struct v2 a)
{
f32 l = v2_len_sq(a);
f32 l = a.x * a.x + a.y * a.y;
if (l != 0) {
f32 denom = 1.f / math_sqrt(l);
a.x *= denom;
@ -682,16 +699,6 @@ INLINE struct v2 v2_ceil(struct v2 a)
);
}
INLINE f32 v2_dot(struct v2 a, struct v2 b)
{
return a.x * b.x + a.y * b.y;
}
INLINE f32 v2_wedge(struct v2 a, struct v2 b)
{
return a.x * b.y - a.y * b.x;
}
INLINE f32 v2_distance(struct v2 a, struct v2 b)
{
f32 dx = b.x - a.x;

8
src/user.c
View File

@ -1,4 +1,5 @@
#include "user.h"
#include "app.h"
#include "renderer.h"
#include "font.h"
#include "sprite.h"
@ -15,7 +16,7 @@
#include "entity.h"
#include "mixer.h"
#include "atomic.h"
#include "app.h"
#include "gjk.h"
struct bind_state {
b32 is_held; /* Is this bind held down this frame */
@ -993,8 +994,10 @@ INTERNAL void user_update(void)
if (ent->is_top) {
b32 colliding = ent->colliding;
struct entity *e1 = entity_from_handle(store, ent->colliding_with);
(UNUSED)e1;
(UNUSED)colliding;
#if 0
/* Draw menkowski */
if (entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED)) {
struct quad ent_quad;
@ -1033,6 +1036,7 @@ INTERNAL void user_update(void)
draw_solid_poly_line(G.viewport_canvas, m, true, thickness, color);
//draw_solid_poly(G.viewport_canvas, m, color);
}
#endif
/* Draw point */
{
@ -1042,6 +1046,7 @@ INTERNAL void user_update(void)
draw_solid_circle(G.viewport_canvas, point, thickness, color, 10);
}
#if 0
/* Draw simplex */
{
f32 thickness = 2;
@ -1072,7 +1077,6 @@ INTERNAL void user_update(void)
}
}
#if 0
/* Draw pen */
if (colliding) {
f32 thickness = 2;

485
src/util.c
View File

@ -34,488 +34,3 @@ struct string util_file_name_from_path(struct string path)
};
}
#endif
/* ========================== *
* Collision test
* ========================== */
#include "math.h"
#include "arena.h"
#include "scratch.h"
/* TODO: Remove / move this */
struct v2 perp_towards_point(struct v2 start, struct v2 end, struct v2 p)
{
struct v2 perp = v2_perp_cw(v2_sub(end, start));
i32 sign = 1 - ((v2_dot(perp, v2_sub(p, start)) < 0) << 1);
return v2_mul(perp, sign);
}
struct v2 perp_towards_dir(struct v2 start, struct v2 end, struct v2 dir)
{
struct v2 perp = v2_perp_cw(v2_sub(end, start));
i32 sign = 1 - ((v2_dot(perp, v2_add(dir, start)) < 0) << 1);
return v2_mul(perp, sign);
}
/* Returns 1 if winding forward, -1 if backward */
i32 poly_get_winding_order(struct v2_array poly)
{
i32 res;
if (poly.count >= 3) {
struct v2 a = poly.points[0];
struct v2 b = poly.points[1];
struct v2 c = poly.points[2];
if (v2_wedge(v2_sub(b, a), v2_sub(c, b)) > 0) {
res = 1;
} else {
res = -1;
}
} else {
res = -1;
}
return res;
}
INTERNAL struct v2 poly_support(struct v2_array a, struct v2 dir)
{
/* TODO: Could probably binary search for largest dot since shape is convex */
struct v2 furthest = a.points[0];
f32 furthest_dot = v2_dot(dir, furthest);
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 = p;
furthest_dot = dot;
}
}
return furthest;
}
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)));
}
b32 gjk_boolean(struct v2_array shape0, struct v2_array shape1)
{
struct { struct v2 a, b, c; } s = { 0 };
/* 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]));
/* Second point is support point towards origin */
struct v2 dir = v2_neg(s.a);
struct v2 p = menkowski_point(shape0, shape1, dir);
if (v2_dot(dir, p) >= 0) {
s.b = s.a;
s.a = p;
while (true) {
/* Third point is support point in direction of line normal towards origin */
dir = perp_towards_point(s.a, s.b, V2(0, 0));
p = menkowski_point(shape0, shape1, dir);
if (v2_dot(dir, p) < 0) {
/* New point did not cross origin, collision impossible */
break;
}
s.c = s.b;
s.b = s.a;
s.a = p;
dir = v2_neg(perp_towards_point(s.a, s.b, s.c)); /* Normal of ab pointing away from c */
struct v2 a_to_origin = v2_neg(s.a);
if (v2_dot(dir, a_to_origin) >= 0) {
/* Point is in region ab, remove c from simplex (will happen automatically next iteration) */
} else {
/* Point is not in region ab */
dir = v2_neg(perp_towards_point(s.a, s.c, s.b)); /* 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;
} else {
/* Point is in simplex */
return true;
}
}
}
}
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;
}
struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1, struct v2 penetration_dir)
{
struct gjk_extended_result res = { 0 };
/* TODO: Verify epsilon */
f32 unique_epsilon = 0.00001;
b32 use_penetration_dir = false;
struct gjk_simplex s = { 0 };
/* ========================== *
* Collision check
* ========================== */
struct v2 dir = { 0 };
struct gjk_menkowski_point m = { 0 };
{
/* First point is support point in shape's general directions to eachother */
s.a = menkowski_point_extended(shape0, shape1, v2_sub(shape1.points[0], shape0.points[0]));
s.len = 1;
/* Second point is support point towards origin */
dir = v2_neg(s.a.p);
m = menkowski_point_extended(shape0, shape1, dir);
if (v2_dot(dir, m.p) >= 0) {
s.b = s.a;
s.a = m;
s.len = 2;
while (true) {
/* Third point is support point in direction of line normal towards origin */
dir = perp_towards_point(s.a.p, s.b.p, V2(0, 0));
m = menkowski_point_extended(shape0, shape1, dir);
if (v2_dot(dir, m.p) < 0) {
/* New point did not cross origin, collision impossible */
break;
}
s.c = s.b;
s.b = s.a;
s.a = m;
s.len = 3;
dir = v2_neg(perp_towards_point(s.a.p, s.b.p, s.c.p)); /* Normal of ab pointing away from c */
struct v2 a_to_origin = v2_neg(s.a.p);
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_neg(perp_towards_point(s.a.p, s.c.p, s.b.p)); /* 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 {
res.colliding = true;
break;
}
}
}
}
}
if (res.colliding) {
use_penetration_dir = !v2_eq(penetration_dir, V2(0, 0));
if (use_penetration_dir) {
/* ========================== *
* Penetration dir expansion
* ========================== */
while (true) {
/* Second point is support point towards penetration_dir */
if (s.len == 1) {
dir = v2_sub(v2_mul(penetration_dir, v2_dot(penetration_dir, s.a.p) / v2_dot(penetration_dir, penetration_dir)), s.a.p);
m = menkowski_point_extended(shape0, shape1, dir);
if (v2_eq(m.p, s.a.p)) {
break;
}
s.b = s.a;
s.a = m;
s.len = 2;
/* Third point is support point in direction of line normal towards `a` projected onto penetration_dir */
dir = perp_towards_dir(s.a.p, s.b.p, penetration_dir);
}
if (s.len == 2) {
m = menkowski_point_extended(shape0, shape1, dir);
if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_sub(m.p, s.a.p))) < unique_epsilon) {
/* New point is already on the current line */
break;
}
s.c = s.b;
s.b = s.a;
s.a = m;
s.len = 3;
}
i32 a_wedgesign = math_fsign(v2_wedge(penetration_dir, s.a.p));
i32 b_wedgesign = math_fsign(v2_wedge(penetration_dir, s.b.p));
i32 c_wedgesign = math_fsign(v2_wedge(penetration_dir, s.c.p));
if (a_wedgesign != b_wedgesign) {
/* Remove c */
dir = perp_towards_dir(s.a.p, s.b.p, penetration_dir);
s.len = 2;
} else if (b_wedgesign != c_wedgesign) {
/* Remove b */
dir = perp_towards_dir(s.a.p, s.c.p, penetration_dir);
s.b = s.c;
s.len = 2;
} else {
/* Remove a */
dir = perp_towards_dir(s.b.p, s.c.p, penetration_dir);
s.a = s.b;
s.b = s.c;
s.len = 2;
}
}
} else {
/* ========================== *
* Epa expansion
* ========================== */
ASSERT(s.len == 3);
struct temp_arena scratch = scratch_begin_no_conflict();
struct gjk_menkowski_point *proto = arena_dry_push(scratch.arena, struct gjk_menkowski_point);
u32 proto_count = 0;
{
struct gjk_menkowski_point *tmp = arena_push_array(scratch.arena, struct gjk_menkowski_point, 3);
proto_count = s.len;
tmp[0] = s.a;
if (proto_count >= 2) {
tmp[1] = s.b;
if (proto_count >= 3) {
tmp[2] = s.c;
}
}
}
while (true) {
struct v2 pen = V2(0, 0);
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].p;
struct v2 pe = proto[pe_index].p;
struct v2 vse = v2_sub(pe, ps);
struct v2 vso = v2_neg(ps);
struct v2 vsd = v2_mul(vse, (v2_dot(vso, vse) / v2_dot(vse, 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 = pd;
pen_len_sq = pd_len_sq;
}
}
/* Find new point in dir */
m = menkowski_point_extended(shape0, shape1, pen);
/* Check unique */
/* TODO: Better */
{
b32 unique = true;
for (u32 i = 0; i < proto_count; ++i) {
struct v2 edge_start = proto[i].p;
struct v2 edge_end = i < proto_count - 1 ? proto[i + 1].p : proto[0].p;
if (math_fabs(v2_wedge(v2_sub(edge_end, edge_start), v2_sub(m.p, edge_start))) < unique_epsilon) {
unique = false;
break;
}
}
if (!unique) {
s.a = proto[pen_ps_index];
s.b = proto[pen_pe_index];
s.len = 2;
break;
}
}
/* Insert point into prototype */
/* FIXME: Preserve winding order */
arena_push(scratch.arena, struct v2);
++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;
}
scratch_end(scratch);
}
} else {
/* ========================== *
* Closest point expansion
* ========================== */
if (s.len == 2) {
/* Third point is support point in direction of line normal towards `a` projected onto penetration_dir */
dir = perp_towards_dir(s.a.p, s.b.p, penetration_dir);
}
while (true) {
/* Second point is support point towards origin */
if (s.len == 1) {
m = menkowski_point_extended(shape0, shape1, v2_neg(s.a.p));
if (v2_eq(m.p, s.a.p)) {
break;
}
s.b = s.a;
s.a = m;
s.len = 2;
/* Third point is support point in direction of line normal towards origin */
dir = perp_towards_point(s.a.p, s.b.p, V2(0, 0));
}
if (s.len == 2) {
m = menkowski_point_extended(shape0, shape1, dir);
if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_sub(m.p, s.a.p))) < unique_epsilon) {
/* New point is already on the current line */
break;
}
s.c = s.b;
s.b = s.a;
s.a = m;
s.len = 3;
}
/* Remove point or edge and determine next direction based on vornoi regions */
i32 code = 0;
struct v2 rab_dir = v2_neg(perp_towards_point(s.a.p, s.b.p, s.c.p));
struct v2 rac_dir = v2_neg(perp_towards_point(s.a.p, s.c.p, s.b.p));
struct v2 rbc_dir = v2_neg(perp_towards_point(s.b.p, s.c.p, s.a.p));
code |= ((v2_dot(rab_dir, v2_neg(s.a.p)) >= 0) << 0); /* Regions ab, a, and b*/
code |= ((v2_dot(rac_dir, v2_neg(s.a.p)) >= 0) << 1); /* Regions ac, a, and c */
code |= ((v2_dot(rbc_dir, v2_neg(s.b.p)) >= 0) << 2); /* Regions bc, b, and c */
switch (code) {
case 1: { /* Region ab, remove c */
dir = rab_dir;
s.len = 2;
} break;
case 2: { /* Region ac, remove b */
dir = rac_dir;
s.b = s.c;
s.len = 2;
} break;
case 4: { /* Region bc, remove a */
dir = rbc_dir;
s.a = s.b;
s.b = s.c;
s.len = 2;
} break;
case 3: { /* Region a, remove bc */
s.len = 1;
} break;
case 5: { /* Region b, remove ac */
s.a = s.b;
s.len = 1;
} break;
case 6: { /* Region c, remove ab */
s.a = s.c;
s.len = 1;
} break;
}
}
}
/* Resolve points */
if (s.len == 1) {
res.p0 = s.a.p0;
res.p1 = s.a.p1;
} else if (s.len == 2) {
/* FIXME: Winding order dependent? */
f32 ratio;
if (use_penetration_dir) {
/* Determine ratio between edge a & b that penetration dir intersection lies */
f32 wedgea = math_fabs(v2_wedge(penetration_dir, s.a.p));
f32 wedgeb = math_fabs(v2_wedge(penetration_dir, s.b.p));
ratio = wedgea / (wedgea + wedgeb);
} else {
/* Determine ratio between edge a & b that projected origin lies */
struct v2 vab = v2_sub(s.b.p, s.a.p);
struct v2 vao = v2_neg(s.a.p);
ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1);
}
/* Determine point on shape 0 */
{
res.p0 = v2_sub(s.b.p0, s.a.p0);
res.p0 = v2_mul(res.p0, ratio);
res.p0 = v2_add(res.p0, s.a.p0);
}
/* Determine point on shape 1 */
{
res.p1 = v2_sub(s.b.p1, s.a.p1);
res.p1 = v2_mul(res.p1, ratio);
res.p1 = v2_add(res.p1, s.a.p1);
}
}
return res;
}
/* TODO: Remove this (debugging) */
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) };
u64 rays = 500;
for (u64 i = 0; i < rays; ++i) {
f32 angle = ((f32)i / rays) * (2 * PI);
struct v2 dir = v2_from_angle(angle);
struct v2 p = menkowski_point_extended(poly0, poly1, dir).p;
if (res.count == 0 || !v2_eq(p, res.points[res.count - 1])) {
*arena_push(arena, struct v2) = p;
++res.count;
}
}
return res;
}

27
src/util.h
View File

@ -180,31 +180,4 @@ INLINE void sleep_frame(sys_timestamp_t last_frame_time, f64 target_dt)
}
}
/* ========================== *
* Collision testing
* ========================== */
struct gjk_menkowski_point {
struct v2 p0; /* Support point of first shape in dir */
struct v2 p1; /* Support point of second shape in -dir */
struct v2 p; /* Menkowski difference point */
};
struct gjk_simplex {
u32 len;
struct gjk_menkowski_point a, b, c;
};
struct gjk_extended_result {
b32 colliding;
struct v2 p0, p1;
};
struct v2 perp_towards_point(struct v2 start, struct v2 end, struct v2 p);
struct v2 perp_towards_dir(struct v2 start, struct v2 end, struct v2 dir);
struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1);
i32 poly_get_winding_order(struct v2_array poly);
b32 gjk_boolean(struct v2_array shape0, struct v2_array shape1);
struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1, struct v2 penetration_dir);
#endif