From 5ef8ee3f408e5785dc506d49372812b5601701fd Mon Sep 17 00:00:00 2001 From: jacob Date: Tue, 8 Oct 2024 07:43:32 -0500 Subject: [PATCH] rename module gjk -> collider. remove unused gjk functions. --- src/collider.c | 596 ++++++++++++ src/collider.h | 49 + src/entity.h | 6 +- src/game.c | 6 +- src/game.h | 2 +- src/gjk.c | 2490 ------------------------------------------------ src/gjk.h | 106 --- src/user.c | 22 +- src/user.h | 6 +- 9 files changed, 666 insertions(+), 2617 deletions(-) create mode 100644 src/collider.c create mode 100644 src/collider.h delete mode 100644 src/gjk.c delete mode 100644 src/gjk.h diff --git a/src/collider.c b/src/collider.c new file mode 100644 index 00000000..ff6409e3 --- /dev/null +++ b/src/collider.c @@ -0,0 +1,596 @@ +#include "collider.h" +#include "math.h" +#include "arena.h" +#include "scratch.h" + +#if COLLIDER_DEBUG +u32 collider_debug_steps = U32_MAX; +#endif + +#define DBGSTEP if (dbg_step++ >= collider_debug_steps) goto abort + +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 */ + 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 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) +{ + return v2_sub(poly_support_point(poly0, dir), poly_support_point(shape1, v2_neg(dir))); +} + +b32 collider_collision_boolean(struct v2_array shape0, struct v2_array shape1) +{ + struct { struct v2 a, b, c; } s = ZI; + + /* 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 */ + 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 */ + dir = v2_neg(s.a); + 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; + + 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); + + 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 (will happen automatically next iteration) */ + } 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; + } else { + /* Point is in simplex */ + return true; + } + } + } + } + + return false; +} + +struct collider_collision_points_result collider_collision_points(struct v2_array shape0, struct v2_array shape1) +{ + struct temp_arena scratch = scratch_begin_no_conflict(); /* TODO: Only begin scratch for EPA */ + struct collider_collision_points_result res = ZI; + + /* TODO: Parameterize */ + const f32 tolerance = 0.0025f; + const f32 min_unique_pt_dist_sq = 0.001f * 0.001f; + + b32 colliding = false; + b32 simplex_is_closest_edge = false; + + struct collider_simplex s = ZI; + struct v2 *proto = NULL; + u32 proto_count = 0; + + struct v2 normal = ZI; + struct collider_collision_point points[2] = ZI; + u32 num_points = 0; + + struct v2 dir = ZI; + struct v2 m = ZI; + +#if COLLIDER_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; + + struct v2 removed_a = ZI; + struct v2 removed_b = ZI; + u32 num_removed = 0; + b32 done = false; + while (!done) { + if (s.len == 1) { + /* Second point is support point towards origin */ + dir = v2_neg(s.a); + + DBGSTEP; + m = menkowski_point(shape0, shape1, dir); + /* Check that new point is far enough away from existing point */ + if (v2_len_sq(v2_sub(m, s.a)) < min_unique_pt_dist_sq) { + done = true; + 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 || + v2_len_sq(v2_sub(m, s.b)) < min_unique_pt_dist_sq || + ((num_removed >= 1) && ( + (v2_len_sq(v2_sub(m, removed_a)) < min_unique_pt_dist_sq) || + (num_removed >= 2 && v2_len_sq(v2_sub(m, removed_b)) < min_unique_pt_dist_sq) + )) || + math_fabs(v2_wedge(v2_sub(s.b, s.a), v2_sub(m, s.a))) < min_unique_pt_dist_sq + ) { + simplex_is_closest_edge = true; + done = true; + break; + } + s.c = s.b; + s.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) { + case 0: + { /* No region, must be in simplex */ + colliding = true; + done = 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; + default: + { + /* Unknown region (should be impossible) */ + ASSERT(false); + res.path = -1; + done = true; + } 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 simplex */ + /* 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 edge_start = proto[i]; + struct v2 edge_end = i < proto_count - 1 ? proto[i + 1] : proto[0]; + struct v2 vsm = v2_sub(m, edge_start); + if (v2_len_sq(vsm) < min_unique_pt_dist_sq || + math_fabs(v2_wedge(v2_sub(edge_end, edge_start), vsm)) < min_unique_pt_dist_sq) { + unique = false; + break; + } + } + if (!unique) { + res.path = 1; + normal = v2_norm(dir); + break; + } + } + + /* Insert point into prototype */ + /* FIXME: Preserve winding order */ + arena_push(scratch.arena, struct collider_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 (simplex_is_closest_edge && s.len > 1) { + /* Shapes are not overlapping (origin is outside of simplex). Project + * origin to determine if distance is within tolerance. */ + ASSERT(s.len == 2); + 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)) { + res.path = 2; + normal = v2_norm(dir); + colliding = true; + } + } + + 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; + } + } + + f32 a0t = 0; + f32 a1t = 0; + f32 b0t = 0; + f32 b1t = 0; + + struct v2 vba0 = v2_neg(vab0); + struct v2 vba1 = v2_neg(vab1); + { + { + struct v2 va0a1 = v2_sub(a1, a0); + struct v2 va1a0 = v2_neg(va0a1); + { + 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 < tolerance) { + struct collider_collision_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 collider_collision_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: + 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; +} + +/* ========================== * + * Debug functions + * TODO: Remove these + * ========================== */ + + /* 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(poly0, poly1, dir); + if (res.count == 0 || !v2_eq(p, res.points[res.count - 1])) { + *arena_push(arena, struct v2) = p; + ++res.count; + } + } + return res; +} + +/* TODO: Remove this (debugging) */ +struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_array poly1) +{ + struct v2_array res = { .points = arena_dry_push(arena, struct v2) }; + for (u64 i = 0; i < poly0.count; ++i) { + struct v2 p0 = poly0.points[i]; + for (u64 j = 0; j < poly1.count; ++j) { + struct v2 p1 = poly1.points[j]; + *arena_push(arena, struct v2) = v2_sub(p0, p1); + ++res.count; + } + } + + return res; +} diff --git a/src/collider.h b/src/collider.h new file mode 100644 index 00000000..5c2fea98 --- /dev/null +++ b/src/collider.h @@ -0,0 +1,49 @@ +#ifndef COLLIDER_H +#define COLLIDER_H + +#define COLLIDER_DEBUG RTC + +#if COLLIDER_DEBUG +extern u32 collider_debug_steps; +#endif + +struct collider_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 */ +}; + +/* Returns simple true or false indicating shape collision */ +b32 collider_collision_boolean(struct v2_array shape0, struct v2_array shape1); + +struct collider_simplex { + u32 len; + struct v2 a, b, c; +}; + +struct collider_collision_point { + struct v2 point; + f32 separation; + u32 id; /* Based on polygon edge-to-edge */ +}; + +struct collider_prototype { struct v2 points[256]; u32 len; }; +struct collider_collision_points_result { + + struct v2 normal; + struct collider_collision_point points[2]; + u32 num_points; + + /* For debugging */ + b32 solved; + i32 path; + struct collider_simplex simplex; + struct collider_prototype prototype; +}; + +struct collider_collision_points_result collider_collision_points(struct v2_array shape0, struct v2_array shape1); + +struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1); +struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_array poly1); + +#endif diff --git a/src/entity.h b/src/entity.h index 6f465ade..14e49bc3 100644 --- a/src/entity.h +++ b/src/entity.h @@ -56,7 +56,7 @@ struct entity_store { /* TODO: Remove this */ -#include "gjk.h" +#include "collider.h" @@ -116,8 +116,8 @@ struct entity { /* TODO: Remove this (testing) */ struct entity_handle colliding_with; - struct gjk_simplex simplex; - struct gjk_prototype prototype; + struct collider_simplex simplex; + struct collider_prototype prototype; struct v2 pendir; b32 solved; i32 path; diff --git a/src/game.c b/src/game.c index e70721e6..f75648bd 100644 --- a/src/game.c +++ b/src/game.c @@ -10,7 +10,7 @@ #include "atomic.h" #include "app.h" #include "log.h" -#include "gjk.h" +#include "collider.h" GLOBAL struct { struct atomic_i32 game_thread_shutdown; @@ -364,7 +364,7 @@ INTERNAL void create_contact_manifolds(void) }; } - struct gjk_contact_points_result res = gjk_contact_points(e0_poly, e1_poly); + struct collider_collision_points_result res = collider_collision_points(e0_poly, e1_poly); /* Parts of algorithm are hard-coded to support 2 contact points */ CT_ASSERT(ARRAY_COUNT(manifold->contacts) == 2); @@ -427,7 +427,7 @@ INTERNAL void create_contact_manifolds(void) /* Update / insert returned contacts */ for (u32 i = 0; i < res.num_points; ++i) { - struct gjk_contact_point *res_point = &res.points[i]; + struct collider_collision_point *res_point = &res.points[i]; struct v2 point = res_point->point; f32 sep = res_point->separation; u32 id = res_point->id; diff --git a/src/game.h b/src/game.h index a21c8495..a48b4350 100644 --- a/src/game.h +++ b/src/game.h @@ -36,7 +36,7 @@ struct game_cmd { struct v2 aim_pos; #if RTC - u32 gjk_steps; + u32 collider_gjk_steps; #endif }; diff --git a/src/gjk.c b/src/gjk.c deleted file mode 100644 index e3ecd1bc..00000000 --- a/src/gjk.c +++ /dev/null @@ -1,2490 +0,0 @@ -#include "gjk.h" -#include "math.h" -#include "arena.h" -#include "scratch.h" - -#if GJK_DEBUG -u32 gjk_debug_steps = U32_MAX; -#endif - -#define DBGSTEP if (dbg_step++ >= gjk_debug_steps) goto abort - -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 */ - 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 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) -{ - 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) -{ - struct { struct v2 a, b, c; } s = ZI; - - /* 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 */ - 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 */ - dir = v2_neg(s.a); - 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; - - 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); - - 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 (will happen automatically next iteration) */ - } 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; - } else { - /* Point is in simplex */ - return true; - } - } - } - } - - return false; -} - - - - - - - - - - -/* ========================== * - * Debug functions - * TODO: Remove these - * ========================== */ - - /* 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(poly0, poly1, dir); - if (res.count == 0 || !v2_eq(p, res.points[res.count - 1])) { - *arena_push(arena, struct v2) = p; - ++res.count; - } - } - return res; -} - -/* TODO: Remove this (debugging) */ -struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_array poly1) -{ - struct v2_array res = { .points = arena_dry_push(arena, struct v2) }; - for (u64 i = 0; i < poly0.count; ++i) { - struct v2 p0 = poly0.points[i]; - for (u64 j = 0; j < poly1.count; ++j) { - struct v2 p1 = poly1.points[j]; - *arena_push(arena, struct v2) = v2_sub(p0, p1); - ++res.count; - } - } - - return res; -} - - - - - - - - - - - - - - - - - -#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 */ - struct gjk_contact_points_result res = ZI; - - /* TODO: Set all epsilons used in this function to 0.005 */ - - 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 - * ========================== */ - - { - /* 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 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; - 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 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 */ - { - const f32 min_new_pt_dist_sq = 0.001f; - 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_new_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; -#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 - } - } - - /* 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; - } - } - - 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 - 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 - } - } - - 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; -} - -#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.0025f; - const f32 min_unique_pt_dist_sq = 0.001f * 0.001f; - - b32 colliding = false; - b32 simplex_is_closest_edge = 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; - - 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; - - struct v2 removed_a = ZI; - struct v2 removed_b = ZI; - u32 num_removed = 0; - b32 done = false; - while (!done) { - if (s.len == 1) { - /* Second point is support point towards origin */ - dir = v2_neg(s.a); - - DBGSTEP; - m = menkowski_point(shape0, shape1, dir); - /* Check that new point is far enough away from existing point */ - if (v2_len_sq(v2_sub(m, s.a)) < min_unique_pt_dist_sq) { - done = true; - 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 || - v2_len_sq(v2_sub(m, s.b)) < min_unique_pt_dist_sq || - ((num_removed >= 1) && ( - (v2_len_sq(v2_sub(m, removed_a)) < min_unique_pt_dist_sq) || - (num_removed >= 2 && v2_len_sq(v2_sub(m, removed_b)) < min_unique_pt_dist_sq) - )) || - math_fabs(v2_wedge(v2_sub(s.b, s.a), v2_sub(m, s.a))) < min_unique_pt_dist_sq - ) { - simplex_is_closest_edge = true; - done = true; - break; - } - s.c = s.b; - s.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) { - case 0: - { /* No region, must be in simplex */ - colliding = true; - done = 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; - default: - { - /* Unknown region (should be impossible) */ - ASSERT(false); - res.path = -1; - done = true; - } 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 simplex */ - /* 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 edge_start = proto[i]; - struct v2 edge_end = i < proto_count - 1 ? proto[i + 1] : proto[0]; - struct v2 vsm = v2_sub(m, edge_start); - if (v2_len_sq(vsm) < min_unique_pt_dist_sq || - math_fabs(v2_wedge(v2_sub(edge_end, edge_start), vsm)) < min_unique_pt_dist_sq) { - unique = false; - break; - } - } - if (!unique) { - res.path = 1; - normal = v2_norm(dir); - 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; - } - } else if (simplex_is_closest_edge && s.len > 1) { - /* Shapes are not overlapping (origin is outside of simplex). Project - * origin to determine if distance is within tolerance. */ - ASSERT(s.len == 2); - 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)) { - res.path = 2; - normal = v2_norm(dir); - colliding = true; - } - } - - 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; - } - } - - f32 a0t = 0; - f32 a1t = 0; - f32 b0t = 0; - f32 b1t = 0; - - struct v2 vba0 = v2_neg(vab0); - struct v2 vba1 = v2_neg(vab1); - { - { - struct v2 va0a1 = v2_sub(a1, a0); - struct v2 va1a0 = v2_neg(va0a1); - { - 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 < 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)); - } - } - } - - res.solved = true; -abort: - 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 - - - - - - - - - - - - - - - - - - - - - - - - -/* ========================== * - * 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 temp_arena scratch = scratch_begin_no_conflict(); /* TODO: Only begin scratch for EPA */ - struct gjk_contact_points_result res = ZI; - - /* TODO: Verify epsilon */ - /* FIXME: Infinite loop when epsilon too low and shapes is too far from 0 (precision issue) */ - const f32 epsilon = 0.0000100f; - struct gjk_simplex s = ZI; - b32 colliding = false; - struct gjk_menkowski_point *proto = NULL; - u32 proto_count = 0; - - struct gjk_contact_pair pair0 = ZI; - struct gjk_contact_pair pair1 = ZI; - u32 num_pairs = 0; - - /* Used by GJK & EPA */ - struct v2 dir = ZI; - struct gjk_menkowski_point 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 */ - 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 */ - DBGSTEP; - dir = v2_neg(s.a.p); - m = menkowski_point_extended(shape0, shape1, dir); - if (v2_dot(dir, m.p) > 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.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; - - struct v2 vab = v2_sub(s.b.p, s.a.p); - struct v2 vac = v2_sub(s.c.p, s.a.p); - struct v2 a_to_origin = v2_neg(s.a.p); - - 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 - * ========================== */ - - proto = arena_dry_push(scratch.arena, struct gjk_menkowski_point); - proto_count = 0; - { - ASSERT(s.len == 3); - struct gjk_menkowski_point *tmp = arena_push_array(scratch.arena, struct gjk_menkowski_point, 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].p; - struct v2 pe = proto[pe_index].p; - - 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; - } - } - - /* TODO: Move to break (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 */ - struct v2 n = proto[(pen_pe_index < proto_count - 1) ? (pen_pe_index + 1) : 0].p; /* Next point along prototype after edge */ - struct v2 van = v2_sub(n, s.a.p); - struct v2 vab = v2_sub(s.b.p, s.a.p); - dir = v2_perp_towards_dir(vab, v2_neg(van)); - } - m = menkowski_point_extended(shape0, shape1, dir); - - /* 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))) < epsilon) { - unique = false; - break; - } - } - if (!unique) { - 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; - } - - /* ========================== * - * Resolve points - * ========================== */ - - if (s.len == 1) { - num_pairs = 1; - pair0.p0 = s.a.p0; - pair0.p1 = s.a.p1; - } else { - ASSERT(s.len == 2); - /* TODO: Epsilon */ - if (!v2_eq(s.a.p0, s.b.p0) && !v2_eq(s.a.p1, s.b.p1)) { - /* Closest features are both faces, return clipped edge points of each face */ - num_pairs = 2; - struct v2 a = s.a.p0; - struct v2 b = s.b.p0; - struct v2 c = s.a.p1; - struct v2 d = s.b.p1; - - struct v2 vab = v2_sub(b, a); - struct v2 vac = v2_sub(c, a); - struct v2 vad = v2_sub(d, a); - - struct v2 vcd = v2_sub(d, c); - struct v2 vca = v2_sub(a, c); - struct v2 vcb = v2_sub(b, c); - - f32 inv_vab_len_sq = 1.f / v2_len_sq(vab); - f32 inv_vcd_len_sq = 1.f / v2_len_sq(vcd); - - pair0.p0 = v2_add(a, v2_mul(vab, clamp_f32(v2_dot(vab, vac) * inv_vab_len_sq, 0, 1))); - pair0.p1 = v2_add(c, v2_mul(vcd, clamp_f32(v2_dot(vcd, vcb) * inv_vcd_len_sq, 0, 1))); - pair1.p0 = v2_add(a, v2_mul(vab, clamp_f32(v2_dot(vab, vad) * inv_vab_len_sq, 0, 1))); - pair1.p1 = v2_add(c, v2_mul(vcd, clamp_f32(v2_dot(vcd, vca) * inv_vcd_len_sq, 0, 1))); - } else { - /* Determine ratio between edge a & b that projected origin lies */ - num_pairs = 1; - struct v2 vab = v2_sub(s.b.p, s.a.p); - f32 ratio = clamp_f32(v2_dot(vab, v2_neg(s.a.p)) / v2_dot(vab, vab), 0, 1); - pair0.p0 = v2_add(s.a.p0, v2_mul(v2_sub(s.b.p0, s.a.p0), ratio)); - pair0.p1 = v2_add(s.a.p1, v2_mul(v2_sub(s.b.p1, s.a.p1), ratio)); - } - } - } - - res.solved = true; -abort: - - 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].p; - } - res.prototype.len = proto_count; - } else { - if (s.len >= 1) { - res.prototype.points[0] = s.a.p; - if (s.len >= 2) { - res.prototype.points[1] = s.b.p; - if (s.len >= 3) { - res.prototype.points[2] = s.c.p; - } - } - } - res.prototype.len = s.len; - } - res.pairs[0] = pair0; - res.pairs[1] = pair1; - res.num_pairs = num_pairs; - res.simplex = s; - scratch_end(scratch); - return res; -} -#endif - - - - - - - - - - - - - - - - - - - - - - - - - - - -/* ========================== * - * GJK closest / deepest contact pairs (unused) - * ========================== */ - -#if 0 - /* 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. */ -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 gjk_contact_points_result res = ZI; - - /* TODO: Verify epsilon */ - /* FIXME: Infinite loop when epsilon too low and shapes is too far from 0 (precision issue) */ - const f32 epsilon = 0.0000100; - struct gjk_simplex s = ZI; - b32 colliding = false; - struct gjk_menkowski_point *proto = NULL; - u32 proto_count = 0; - - struct gjk_contact_pair pair0 = ZI; - struct gjk_contact_pair pair1 = ZI; - u32 num_pairs = 0; - -#if GJK_DEBUG - u32 dbg_step = 0; -#endif - - /* ========================== * - * GJK collision check - * Construct encapsulating simplex OR closest feature if not colliding - * ========================== */ - - struct v2 dir = ZI; - struct gjk_menkowski_point m = ZI; - /* Determine encapsulating simplex if colliding, or closest edge / point to origin on simplex */ - { - /* 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; - - while (!colliding) { - if (s.len == 1) { - /* Second point is support point towards origin */ - dir = v2_neg(s.a.p); - - DBGSTEP; - m = menkowski_point_extended(shape0, shape1, dir); - if (v2_eq(m.p, s.a.p)) { - /* Point is the same */ - break; - } - s.b = s.a; - s.a = m; - s.len = 2; - if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) < epsilon) { - /* New ab lies on origin */ - break; - } - - /* 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)); - } - - { - DBGSTEP; - 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))) < epsilon) { - /* New point is on existing line ab */ - break; - } - s.c = s.b; - s.b = s.a; - s.a = m; - s.len = 3; - if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) < epsilon) { - /* New ab lies on origin */ - s.len = 2; - break; - } else if (math_fabs(v2_wedge(v2_sub(s.c.p, s.a.p), v2_neg(s.a.p))) < epsilon) { - /* New ac lies on origin */ - s.b = s.c; - s.len = 2; - break; - } - } - - /* Determine voronoi region of the simplex in which the origin lies */ - i32 voronoi_mask = 0; - struct v2 vab = v2_sub(s.b.p, s.a.p); - struct v2 vac = v2_sub(s.c.p, s.a.p); - struct v2 vbc = v2_sub(s.c.p, s.b.p); - 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.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 */ - /* 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 */ - dir = rab_dir; /* Next third point is in direction of region ab */ - s.len = 2; - } break; - case 2: - { /* Region ac, remove b */ - dir = rac_dir; /* Next third point is in direction of region ac */ - s.b = s.c; - s.len = 2; - } break; - case 4: - { /* Region bc, remove a */ - dir = rbc_dir; /* Next third point is in direction of region bc */ - 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; - } - } - } - - if (colliding) { - /* ========================== * - * Epa - * ========================== */ - proto = arena_dry_push(scratch.arena, struct gjk_menkowski_point); - proto_count = 0; - { - ASSERT(s.len == 3); - struct gjk_menkowski_point *tmp = arena_push_array(scratch.arena, struct gjk_menkowski_point, 3); - tmp[0] = s.a; - tmp[1] = s.b; - tmp[2] = s.c; - proto_count = 3; - } - - /* ========================== * - * Expand simplex towards closest edge to origin inside menkowski - * ========================== */ - - 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].p; - struct v2 pe = proto[pe_index].p; - - 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; - dir = pd; - } - } - - /* TODO: Move to break (debugging) */ - s.a = proto[pen_ps_index]; - s.b = proto[pen_pe_index]; - s.len = 2; - - /* Find new point in dir */ - m = menkowski_point_extended(shape0, shape1, dir); - - /* Check unique */ - /* TODO: Better */ - DBGSTEP; - { - 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))) < epsilon) { - unique = false; - break; - } - } - if (!unique) { - 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; - } - } - - /* Resolve points */ - if (s.len == 1) { - num_pairs = 1; - pair0.p0 = s.a.p0; - pair0.p1 = s.a.p1; - } else { - ASSERT(s.len == 2); - /* TODO: Epsilon */ - if (!v2_eq(s.a.p0, s.b.p0) && !v2_eq(s.a.p1, s.b.p1)) { - /* Closest features are both faces, return clipped edge points of each face */ - num_pairs = 2; - struct v2 a = s.a.p0; - struct v2 b = s.b.p0; - struct v2 c = s.a.p1; - struct v2 d = s.b.p1; - - struct v2 vab = v2_sub(b, a); - struct v2 vac = v2_sub(c, a); - struct v2 vad = v2_sub(d, a); - - struct v2 vcd = v2_sub(d, c); - struct v2 vca = v2_sub(a, c); - struct v2 vcb = v2_sub(b, c); - - f32 inv_vab_len_sq = 1.f / v2_len_sq(vab); - f32 inv_vcd_len_sq = 1.f / v2_len_sq(vcd); - - pair0.p0 = v2_add(a, v2_mul(vab, clamp_f32(v2_dot(vab, vac) * inv_vab_len_sq, 0, 1))); - pair0.p1 = v2_add(c, v2_mul(vcd, clamp_f32(v2_dot(vcd, vcb) * inv_vcd_len_sq, 0, 1))); - pair1.p0 = v2_add(a, v2_mul(vab, clamp_f32(v2_dot(vab, vad) * inv_vab_len_sq, 0, 1))); - pair1.p1 = v2_add(c, v2_mul(vcd, clamp_f32(v2_dot(vcd, vca) * inv_vcd_len_sq, 0, 1))); - } else { - /* Determine ratio between edge a & b that projected origin lies */ - num_pairs = 1; - struct v2 vab = v2_sub(s.b.p, s.a.p); - f32 ratio = clamp_f32(v2_dot(vab, v2_neg(s.a.p)) / v2_dot(vab, vab), 0, 1); - pair0.p0 = v2_add(s.a.p0, v2_mul(v2_sub(s.b.p0, s.a.p0), ratio)); - pair0.p1 = v2_add(s.a.p1, v2_mul(v2_sub(s.b.p1, s.a.p1), ratio)); - } - } - - res.solved = true; -abort: - - - /* TODO: Remove this */ - if (!colliding) num_pairs = 0; - - 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].p; - } - res.prototype.len = proto_count; - } else { - if (s.len >= 1) { - res.prototype.points[0] = s.a.p; - if (s.len >= 2) { - res.prototype.points[1] = s.b.p; - if (s.len >= 3) { - res.prototype.points[2] = s.c.p; - } - } - } - res.prototype.len = s.len; - } - res.pairs[0] = pair0; - res.pairs[1] = pair1; - res.num_pairs = num_pairs; - res.simplex = s; - scratch_end(scratch); - return res; -} -#endif - -/* ========================== * - * GJK closest / deepest point (unused) - * ========================== */ - -#if 0 -struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1) -{ - struct temp_arena scratch = scratch_begin_no_conflict(); - struct gjk_extended_result res = ZI; - - /* TODO: Verify epsilon */ - const f32 epsilon = 0.0000100; - struct gjk_simplex s = ZI; - b32 colliding = false; - struct v2 shape0_p = ZI; - struct v2 shape1_p = ZI; - struct gjk_menkowski_point *proto = NULL; - u32 proto_count = 0; - -#if GJK_DEBUG - u32 dbg_step = 0; -#endif - - /* ========================== * - * GJK collision check - * Construct encapsulating simplex OR closest feature if not colliding - * ========================== */ - - struct v2 dir = ZI; - struct gjk_menkowski_point m = ZI; - /* Determine encapsulating simplex if colliding, or closest edge / point to origin on simplex */ - { - /* 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; - - while (!colliding) { - if (s.len == 1) { - /* Second point is support point towards origin */ - dir = v2_neg(s.a.p); - - DBGSTEP; - m = menkowski_point_extended(shape0, shape1, dir); - if (v2_eq(m.p, s.a.p)) { - /* Point is the same */ - break; - } - s.b = s.a; - s.a = m; - s.len = 2; - if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) < epsilon) { - /* New ab lies on origin */ - break; - } - - /* 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)); - } - - { - DBGSTEP; - 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))) < epsilon) { - /* New point is on existing line ab */ - break; - } - s.c = s.b; - s.b = s.a; - s.a = m; - s.len = 3; - if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) < epsilon) { - /* New ab lies on origin */ - s.len = 2; - break; - } else if (math_fabs(v2_wedge(v2_sub(s.c.p, s.a.p), v2_neg(s.a.p))) < epsilon) { - /* New ac lies on origin */ - s.b = s.c; - s.len = 2; - break; - } - } - - /* Determine voronoi region of the simplex in which the origin lies */ - i32 voronoi_mask = 0; - struct v2 vab = v2_sub(s.b.p, s.a.p); - struct v2 vac = v2_sub(s.c.p, s.a.p); - struct v2 vbc = v2_sub(s.c.p, s.b.p); - 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.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 */ - /* 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 */ - dir = rab_dir; /* Next third point is in direction of region ab */ - s.len = 2; - } break; - case 2: - { /* Region ac, remove b */ - dir = rac_dir; /* Next third point is in direction of region ac */ - s.b = s.c; - s.len = 2; - } break; - case 4: - { /* Region bc, remove a */ - dir = rbc_dir; /* Next third point is in direction of region bc */ - 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; - } - } - } - - if (colliding) { - /* ========================== * - * Epa - * ========================== */ - proto = arena_dry_push(scratch.arena, struct gjk_menkowski_point); - proto_count = 0; - { - ASSERT(s.len == 3); - struct gjk_menkowski_point *tmp = arena_push_array(scratch.arena, struct gjk_menkowski_point, 3); - tmp[0] = s.a; - tmp[1] = s.b; - tmp[2] = s.c; - proto_count = 3; - } - - /* ========================== * - * Expand simplex towards closest edge to origin inside menkowski - * ========================== */ - - 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].p; - struct v2 pe = proto[pe_index].p; - - 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; - dir = pd; - } - } - - /* TODO: Move to break (debugging) */ - s.a = proto[pen_ps_index]; - s.b = proto[pen_pe_index]; - s.len = 2; - - /* Find new point in dir */ - m = menkowski_point_extended(shape0, shape1, dir); - - /* Check unique */ - /* TODO: Better */ - DBGSTEP; - { - 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))) < epsilon) { - unique = false; - break; - } - } - if (!unique) { - 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; - } - } - - /* Resolve points */ - if (s.len == 1) { - shape0_p = s.a.p0; - shape1_p = s.a.p1; - } else if (s.len == 2) { - /* FIXME: Winding order dependent? */ - ASSERT(s.len == 2); - f32 ratio; - { - /* 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 */ - shape0_p = v2_sub(s.b.p0, s.a.p0); - shape0_p = v2_mul(shape0_p, ratio); - shape0_p = v2_add(shape0_p, s.a.p0); - /* Shape 1 */ - shape1_p = v2_sub(s.b.p1, s.a.p1); - shape1_p = v2_mul(shape1_p, ratio); - shape1_p = v2_add(shape1_p, s.a.p1); - } - - res.solved = true; -abort: - 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].p; - } - res.prototype.len = proto_count; - } else { - if (s.len >= 1) { - res.prototype.points[0] = s.a.p; - if (s.len >= 2) { - res.prototype.points[1] = s.b.p; - if (s.len >= 3) { - res.prototype.points[2] = s.c.p; - } - } - } - res.prototype.len = s.len; - } - res.colliding = colliding; - res.p0 = shape0_p; - res.p1 = shape1_p; - res.simplex = s; - scratch_end(scratch); - return res; -} -#endif - - - - - - - - - -/* ========================== * - * Swept GJK (unused) - * ========================== */ - -#if 0 - -struct gjk_swept_result { - b32 colliding; - struct v2 p0, p1; /* Closest points on each shape */ - - /* For debugging */ - struct gjk_simplex simplex; - b32 velocity_intersects; - b32 solved; - struct gjk_prototype prototype; -}; - -struct poly_support_swept_result { - struct v2 p; - struct v2 original; -}; - -INTERNAL struct poly_support_swept_result poly_support_swept(struct v2_array a, struct v2 dir, struct v2 linear_velocity) -{ - /* TODO: Could probably binary search for largest dot since shape is convex */ - struct v2 furthest = V2(0, 0); - struct v2 furthest_original = V2(0, 0); - f32 furthest_dot = -F32_INFINITY; - - for (u32 i = 0; i < a.count; ++i) { - struct v2 p = a.points[i]; - f32 dot = v2_dot(dir, p); - if (dot > furthest_dot) { - furthest = p; - furthest_original = p; - furthest_dot = dot; - } - } - - for (u32 i = 0; i < a.count; ++i) { - struct v2 p = a.points[i]; - struct v2 modified; -#if 0 - if (v2_dot(linear_velocity, dir) > 0) { - modified = v2_add(p, linear_velocity); - } else { - modified = p; - } -#else - modified = v2_add(p, linear_velocity); -#endif - f32 dot = v2_dot(dir, modified); - if (dot > furthest_dot) { - furthest = modified; - furthest_original = p; - furthest_dot = dot; - } - } - - struct poly_support_swept_result res = ZI; - res.p = furthest; - res.original = furthest_original; - return res; -} - -INTERNAL struct gjk_menkowski_point menkowski_point_extended_swept(struct v2_array poly0, struct v2_array poly1, struct v2 dir, struct v2 linear_velocity) -{ - struct gjk_menkowski_point res; - struct poly_support_swept_result res0 = poly_support_swept(poly0, dir, linear_velocity); - struct poly_support_swept_result res1 = poly_support_swept(poly1, v2_neg(dir), V2(0, 0)); - res.p0 = res0.original; - res.p1 = res1.original; - res.p = v2_sub(res0.p, res1.p); - return res; -} - -struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1, struct v2 linear_velocity) -{ - struct temp_arena scratch = scratch_begin_no_conflict(); - struct gjk_extended_result res = ZI; - - /* FIXME: Divs by 0 */ - - /* TODO: Verify epsilon */ - f32 epsilon = 0.00001; - struct gjk_simplex s = { - .len = 3, - .a = V2(F32_NAN, F32_NAN), - .b = V2(F32_NAN, F32_NAN), - .c = V2(F32_NAN, F32_NAN) - }; - b32 colliding = false; - struct v2 shape0_p = ZI; - struct v2 shape1_p = ZI; - b32 velocity_intersects = false; - f32 velocity_intersection = 0; - - /* TODO: Move this back down */ - struct gjk_menkowski_point *proto = arena_dry_push(scratch.arena, struct gjk_menkowski_point); - u32 proto_count = 0; - -#if GJK_DEBUG - u32 dbg_step = 0; -#endif - - /* ========================== * - * GJK collision check - * Construct encapsulating simplex OR closest feature if not colliding - * ========================== */ - - struct v2 dir = ZI; - struct gjk_menkowski_point m = ZI; - /* Determine encapsulating simplex if colliding, or closest edge / point to origin on simplex */ - { - /* First point is support point in shape's general directions to eachother */ - s.a = menkowski_point_extended_swept(shape0, shape1, v2_sub(shape1.points[0], shape0.points[0]), linear_velocity); - s.len = 1; - - while (!colliding) { - if (s.len == 1) { - /* Second point is support point towards origin */ - dir = v2_neg(s.a.p); - - DBGSTEP; - m = menkowski_point_extended_swept(shape0, shape1, dir, linear_velocity); - if (v2_eq(m.p, s.a.p)) { - /* Point is the same */ - break; - } - s.b = s.a; - s.a = m; - s.len = 2; - if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) < epsilon) { - /* New ab lies on origin */ - break; - } - /* 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)); - } - - { - DBGSTEP; - m = menkowski_point_extended_swept(shape0, shape1, dir, linear_velocity); - if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_sub(m.p, s.a.p))) < epsilon) { - /* New point is on existing line ab */ - break; - } - if (v2_eq(m.p, s.a.p) || v2_eq(m.p, s.b.p) || v2_eq(m.p, s.c.p)) { - /* New point is existing c */ - s.b = s.a; - s.a = m; - s.len = 2; - break; - } - s.c = s.b; - s.b = s.a; - s.a = m; - s.len = 3; - if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) < epsilon) { - /* New ab lies on origin */ - s.len = 2; - break; - } - if (math_fabs(v2_wedge(v2_sub(s.c.p, s.a.p), v2_neg(s.a.p))) < epsilon) { - /* New ac lies on origin */ - s.b = s.c; - s.len = 2; - break; - } - } - - DBGSTEP; - /* Determine voronoi region of the simplex in which the origin lies */ - i32 voronoi_mask = 0; - struct v2 vab = v2_sub(s.b.p, s.a.p); - struct v2 vac = v2_sub(s.c.p, s.a.p); - struct v2 vbc = v2_sub(s.c.p, s.b.p); - 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.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 */ - /* 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 */ - dir = rab_dir; /* Next third point is in direction of region ab */ - s.len = 2; - } break; - case 2: - { /* Region ac, remove b */ - dir = rac_dir; /* Next third point is in direction of region ac */ - s.b = s.c; - s.len = 2; - } break; - case 4: - { /* Region bc, remove a */ - dir = rbc_dir; /* Next third point is in direction of region bc */ - 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; - } - } - } - - if (colliding) { - /* ========================== * - * Epa - * ========================== */ - proto = arena_dry_push(scratch.arena, struct gjk_menkowski_point); - proto_count = 0; - { - ASSERT(s.len == 3); - struct gjk_menkowski_point *tmp = arena_push_array(scratch.arena, struct gjk_menkowski_point, 3); - tmp[0] = s.a; - tmp[1] = s.b; - tmp[2] = s.c; - proto_count = 3; - } - - if (v2_eq(linear_velocity, V2(0, 0))) { - /* ========================== * - * Expand simplex towards closest edge to origin inside menkowski - * ========================== */ - - while (true) { - DBGSTEP; - 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); - - 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; - dir = pd; - } - } - - /* TODO: Move to break (debugging) */ - s.a = proto[pen_ps_index]; - s.b = proto[pen_pe_index]; - s.len = 2; - - /* Find new point in dir */ - m = menkowski_point_extended_swept(shape0, shape1, dir, linear_velocity); - - /* 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))) < epsilon) { - unique = false; - break; - } - } - if (!unique) { - 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; - } - } else { - /* ========================== * - * Expand simplex towards furthest edge along velocity - * ========================== */ - - while (true) { - DBGSTEP; - - /* FIXME: Winding order of ps & pe index */ - f32 highest_score = -F32_INFINITY; - u32 picked_start_index = 0; - u32 picked_end_index = 0; - b32 picked_intersects = false; - f32 picked_intersection = 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); - - f32 w = v2_wedge(vse, linear_velocity); - f32 t1 = 0; - f32 t2 = 0; - b32 intersects = false; - if (w != 0) { - w = 1 / w; - t1 = v2_wedge(vso, linear_velocity) * w; - t2 = v2_wedge(vso, vse) * w; - intersects = -epsilon < t1 && t1 < 1 + epsilon && -epsilon < t2 && t2 < 1 + epsilon; - } - - struct v2 next_dir = dir; - f32 score = highest_score; - if (intersects) { - /* Score is intersection (+1 to ensure intersections get priority) */ - score = 1 + t1; - next_dir = v2_perp_towards_dir(vse, linear_velocity); - } else { - /* Score is inverse projection len */ - struct v2 vsv = v2_sub(linear_velocity, ps); - struct v2 vsd = v2_mul(vse, v2_dot(vse, vsv) / v2_dot(vse, vse)); - f32 len = v2_len(v2_sub(vsd, vsv)); - score = 1.0 - len; - next_dir = v2_perp_towards_dir(vse, ps); - } - - if (score > highest_score) { - picked_start_index = ps_index; - picked_end_index = pe_index; - highest_score = score; - picked_intersects = intersects; - picked_intersection = t1; - dir = next_dir; - } - } - - /* TODO: Move to break (debugging) */ - velocity_intersects = picked_intersects; - velocity_intersection = picked_intersection; - s.a = proto[picked_start_index]; - s.b = proto[picked_end_index]; - s.len = 2; - - /* Find new point in dir */ - m = menkowski_point_extended_swept(shape0, shape1, dir, linear_velocity); - - /* Check unique */ - /* TODO: Better */ - /* FIXME: Winding order */ - u32 insert_start_index = 0; - u32 insert_end_index = 1; - { - b32 unique = true; - for (u32 i = 0; i < proto_count; ++i) { - u32 start_index = i; - u32 end_index = i < proto_count - 1 ? i + 1 : 0; - struct v2 edge_start = proto[start_index].p; - struct v2 edge_end = proto[end_index].p; - - struct v2 vso = v2_neg(edge_start); - struct v2 vse = v2_sub(edge_end, edge_start); - struct v2 vsm = v2_sub(m.p, edge_start); - - f32 w = v2_wedge(vse, vsm); - if (math_fabs(w) > epsilon) { - w = v2_wedge(vse, m.p); - if (w != 0) { - w = 1 / w; - f32 t1 = v2_wedge(vso, m.p) * w; - f32 t2 = v2_wedge(vso, vse) * w; - b32 intersects = 0 <= t1 && t1 <= 1 && 0 <= t2 && t2 <= 1; - if (intersects) { - insert_start_index = start_index; - insert_end_index = end_index; - } - } - } else { - unique = false; - break; - } - } - if (!unique) { - break; - } - } - - (UNUSED)insert_start_index; - DBGSTEP; - /* Insert point into prototype */ - /* FIXME: Preserve winding order */ - arena_push(scratch.arena, struct v2); - ++proto_count; - for (u32 i = proto_count - 1; i > insert_end_index; --i) { - u32 shift_from = (i > 0) ? i - 1 : proto_count - 1; - u32 shift_to = i; - proto[shift_to] = proto[shift_from]; - } - proto[insert_end_index] = m; - } - } - } - - /* Resolve points */ - if (s.len == 1) { - ASSERT(!colliding); /* Is this possible? Would make calculating collision normal tricky. */ - shape0_p = s.a.p0; - shape1_p = s.a.p1; - } else { - ASSERT(s.len == 2); - /* FIXME: Winding order dependent? */ - f32 ratio; - if (colliding && !v2_eq(linear_velocity, V2(0, 0))) { - if (velocity_intersects) { - /* Ratio between edge a & b that velocity intersection lies */ - ratio = velocity_intersection; - } else { - /* Ratio between edge a & b that projected velocity lies */ - struct v2 vab = v2_sub(s.b.p, s.a.p); - struct v2 vap = v2_sub(linear_velocity, s.a.p); - ratio = clamp_f32(v2_dot(vab, vap) / v2_dot(vab, vab), 0, 1); - } - } 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 */ - shape0_p = v2_sub(s.b.p0, s.a.p0); - shape0_p = v2_mul(shape0_p, ratio); - shape0_p = v2_add(shape0_p, s.a.p0); - /* Shape 1 */ - shape1_p = v2_sub(s.b.p1, s.a.p1); - shape1_p = v2_mul(shape1_p, ratio); - shape1_p = v2_add(shape1_p, s.a.p1); - } - - - res.solved = true; -abort: - 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].p; - } - res.prototype.len = proto_count; - } else { - if (s.len >= 1) { - res.prototype.points[0] = s.a.p; - if (s.len >= 2) { - res.prototype.points[1] = s.b.p; - if (s.len >= 3) { - res.prototype.points[2] = s.c.p; - } - } - } - res.prototype.len = s.len; - } - res.colliding = colliding; - res.p0 = shape0_p; - res.p1 = shape1_p; - res.simplex = s; - res.velocity_intersects = velocity_intersects; - scratch_end(scratch); - return res; -} - - -/* TODO: Remove this (debugging) */ -struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1, struct v2 linear_velocity) -{ - 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_swept(poly0, poly1, dir, linear_velocity).p; - if (res.count == 0 || !v2_eq(p, res.points[res.count - 1])) { - *arena_push(arena, struct v2) = p; - ++res.count; - } - } - return res; -} - -/* TODO: Remove this (debugging) */ -struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_array poly1, struct v2 linear_velocity) -{ - struct v2_array res = { .points = arena_dry_push(arena, struct v2) }; - for (u64 i = 0; i < poly0.count; ++i) { - struct v2 p0 = poly0.points[i]; - for (u64 j = 0; j < poly1.count; ++j) { - struct v2 p1 = poly1.points[j]; - struct v2 diff = v2_sub(p0, p1); - { - *arena_push(arena, struct v2) = diff; - ++res.count; - } - { - *arena_push(arena, struct v2) = v2_add(diff, linear_velocity); - ++res.count; - } - } - } - - return res; -} - -#endif diff --git a/src/gjk.h b/src/gjk.h deleted file mode 100644 index 37966dd9..00000000 --- a/src/gjk.h +++ /dev/null @@ -1,106 +0,0 @@ -#ifndef GJK_H -#define GJK_H - -#define GJK_DEBUG RTC - -#if GJK_DEBUG -extern u32 gjk_debug_steps; -#endif - -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 */ -}; - -/* 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; - i32 path; - 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 { - u32 len; - struct gjk_menkowski_point a, b, c; -}; - -struct gjk_contact_points_result { - - struct gjk_contact_pair pairs[2]; - u32 num_pairs; - - /* 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); -#endif - -struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1); - -struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_array poly1); - -#if 0 -struct gjk_extended_result { - b32 colliding; - struct v2 p0, p1; /* Closest points (or penetrating points if colliding) on each shape */ - - /* For debugging */ - b32 solved; - struct gjk_simplex simplex; - struct gjk_prototype prototype; -}; - -/* Returns whether shapes are colliding well as closest / penetrating points on each shape. */ -struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1); -#endif - -#if 0 - -struct gjk_extended_result { - b32 colliding; - struct v2 p0, p1; /* Closest points on each shape */ - - /* For debugging */ - struct gjk_simplex simplex; - b32 velocity_intersects; - b32 solved; - struct gjk_prototype prototype; -}; - -struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1, struct v2 linear_velocity); - -struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1, struct v2 linear_velocity); - -struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_array poly1, struct v2 linear_velocity); - -#endif - -#endif diff --git a/src/user.c b/src/user.c index 11979f39..accfce65 100644 --- a/src/user.c +++ b/src/user.c @@ -16,7 +16,7 @@ #include "entity.h" #include "mixer.h" #include "atomic.h" -#include "gjk.h" +#include "collider.h" struct bind_state { b32 is_held; /* Is this bind held down this frame */ @@ -102,9 +102,9 @@ GLOBAL READONLY enum user_bind_kind g_binds[SYS_BTN_COUNT] = { #if RTC /* Debug */ - [SYS_BTN_FORWARD_SLASH] = USER_BIND_KIND_RESET_GJK_STEPS, - [SYS_BTN_COMMA] = USER_BIND_KIND_DECR_GJK_STEPS, - [SYS_BTN_PERIOD] = USER_BIND_KIND_INCR_GJK_STEPS + [SYS_BTN_FORWARD_SLASH] = USER_BIND_KIND_RESET_COLLIDER_GJK_STEPS, + [SYS_BTN_COMMA] = USER_BIND_KIND_DECR_COLLIDER_GJK_STEPS, + [SYS_BTN_PERIOD] = USER_BIND_KIND_INCR_COLLIDER_GJK_STEPS #endif }; @@ -1095,7 +1095,7 @@ INTERNAL void user_update(void) u32 color_second = RGBA_32_F(0, 1, 0, 0.75); u32 color_third = RGBA_32_F(0, 0, 1, 0.75); - struct gjk_simplex simplex = ent->simplex; + struct collider_simplex simplex = ent->simplex; 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]); struct v2_array simplex_array = { .count = simplex.len, .points = simplex_points }; @@ -1295,11 +1295,11 @@ INTERNAL void user_update(void) #if RTC /* Gjk steps */ { - i64 new_steps = gjk_debug_steps; - new_steps += G.bind_states[USER_BIND_KIND_INCR_GJK_STEPS].num_presses_and_repeats; - new_steps -= G.bind_states[USER_BIND_KIND_DECR_GJK_STEPS].num_presses_and_repeats; - if (G.bind_states[USER_BIND_KIND_RESET_GJK_STEPS].num_presses_and_repeats > 0) new_steps = 0; - gjk_debug_steps = (u32)clamp_i64(new_steps, 0, U32_MAX); + i64 new_steps = collider_debug_steps; + new_steps += G.bind_states[USER_BIND_KIND_INCR_COLLIDER_GJK_STEPS].num_presses_and_repeats; + new_steps -= G.bind_states[USER_BIND_KIND_DECR_COLLIDER_GJK_STEPS].num_presses_and_repeats; + if (G.bind_states[USER_BIND_KIND_RESET_COLLIDER_GJK_STEPS].num_presses_and_repeats > 0) new_steps = 0; + collider_debug_steps = (u32)clamp_i64(new_steps, 0, U32_MAX); } #endif } @@ -1374,7 +1374,7 @@ INTERNAL void user_update(void) pos.y += spacing; #if RTC - draw_text(G.viewport_canvas, font, pos, string_format(temp.arena, STR("gjk steps: %F"), FMT_UINT(gjk_debug_steps))); + draw_text(G.viewport_canvas, font, pos, string_format(temp.arena, STR("collider gjk steps: %F"), FMT_UINT(collider_debug_steps))); pos.y += spacing; #endif diff --git a/src/user.h b/src/user.h index 27939616..e2e9b057 100644 --- a/src/user.h +++ b/src/user.h @@ -38,9 +38,9 @@ enum user_bind_kind { #if RTC /* Debug */ - USER_BIND_KIND_RESET_GJK_STEPS, - USER_BIND_KIND_INCR_GJK_STEPS, - USER_BIND_KIND_DECR_GJK_STEPS, + USER_BIND_KIND_RESET_COLLIDER_GJK_STEPS, + USER_BIND_KIND_INCR_COLLIDER_GJK_STEPS, + USER_BIND_KIND_DECR_COLLIDER_GJK_STEPS, #endif USER_BIND_KIND_COUNT