diff --git a/src/entity.h b/src/entity.h index 79ef2825..2fa6b218 100644 --- a/src/entity.h +++ b/src/entity.h @@ -94,7 +94,6 @@ struct entity { /* TODO: Remove this (testing) */ b32 colliding; struct entity_handle colliding_with; - struct gjk_simplex simplex; struct v2 point; diff --git a/src/game.c b/src/game.c index 48adc723..f349b764 100644 --- a/src/game.c +++ b/src/game.c @@ -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; diff --git a/src/gjk.c b/src/gjk.c new file mode 100644 index 00000000..6c146c13 --- /dev/null +++ b/src/gjk.c @@ -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; +} diff --git a/src/gjk.h b/src/gjk.h new file mode 100644 index 00000000..25d4e0f4 --- /dev/null +++ b/src/gjk.h @@ -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 diff --git a/src/math.h b/src/math.h index 477e3eb9..297ec409 100644 --- a/src/math.h +++ b/src/math.h @@ -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; diff --git a/src/user.c b/src/user.c index 3a5e6d67..e592c8ac 100644 --- a/src/user.c +++ b/src/user.c @@ -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; diff --git a/src/util.c b/src/util.c index d9afaa9c..e5be98fd 100644 --- a/src/util.c +++ b/src/util.c @@ -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; -} diff --git a/src/util.h b/src/util.h index f8f7f067..ddfdd47f 100644 --- a/src/util.h +++ b/src/util.h @@ -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