rename module gjk -> collider. remove unused gjk functions.
This commit is contained in:
parent
06afe6263f
commit
5ef8ee3f40
596
src/collider.c
Normal file
596
src/collider.c
Normal file
@ -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;
|
||||||
|
}
|
||||||
49
src/collider.h
Normal file
49
src/collider.h
Normal file
@ -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
|
||||||
@ -56,7 +56,7 @@ struct entity_store {
|
|||||||
|
|
||||||
|
|
||||||
/* TODO: Remove this */
|
/* TODO: Remove this */
|
||||||
#include "gjk.h"
|
#include "collider.h"
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
@ -116,8 +116,8 @@ struct entity {
|
|||||||
|
|
||||||
/* TODO: Remove this (testing) */
|
/* TODO: Remove this (testing) */
|
||||||
struct entity_handle colliding_with;
|
struct entity_handle colliding_with;
|
||||||
struct gjk_simplex simplex;
|
struct collider_simplex simplex;
|
||||||
struct gjk_prototype prototype;
|
struct collider_prototype prototype;
|
||||||
struct v2 pendir;
|
struct v2 pendir;
|
||||||
b32 solved;
|
b32 solved;
|
||||||
i32 path;
|
i32 path;
|
||||||
|
|||||||
@ -10,7 +10,7 @@
|
|||||||
#include "atomic.h"
|
#include "atomic.h"
|
||||||
#include "app.h"
|
#include "app.h"
|
||||||
#include "log.h"
|
#include "log.h"
|
||||||
#include "gjk.h"
|
#include "collider.h"
|
||||||
|
|
||||||
GLOBAL struct {
|
GLOBAL struct {
|
||||||
struct atomic_i32 game_thread_shutdown;
|
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 */
|
/* Parts of algorithm are hard-coded to support 2 contact points */
|
||||||
CT_ASSERT(ARRAY_COUNT(manifold->contacts) == 2);
|
CT_ASSERT(ARRAY_COUNT(manifold->contacts) == 2);
|
||||||
@ -427,7 +427,7 @@ INTERNAL void create_contact_manifolds(void)
|
|||||||
|
|
||||||
/* Update / insert returned contacts */
|
/* Update / insert returned contacts */
|
||||||
for (u32 i = 0; i < res.num_points; ++i) {
|
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;
|
struct v2 point = res_point->point;
|
||||||
f32 sep = res_point->separation;
|
f32 sep = res_point->separation;
|
||||||
u32 id = res_point->id;
|
u32 id = res_point->id;
|
||||||
|
|||||||
@ -36,7 +36,7 @@ struct game_cmd {
|
|||||||
struct v2 aim_pos;
|
struct v2 aim_pos;
|
||||||
|
|
||||||
#if RTC
|
#if RTC
|
||||||
u32 gjk_steps;
|
u32 collider_gjk_steps;
|
||||||
#endif
|
#endif
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
106
src/gjk.h
106
src/gjk.h
@ -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
|
|
||||||
22
src/user.c
22
src/user.c
@ -16,7 +16,7 @@
|
|||||||
#include "entity.h"
|
#include "entity.h"
|
||||||
#include "mixer.h"
|
#include "mixer.h"
|
||||||
#include "atomic.h"
|
#include "atomic.h"
|
||||||
#include "gjk.h"
|
#include "collider.h"
|
||||||
|
|
||||||
struct bind_state {
|
struct bind_state {
|
||||||
b32 is_held; /* Is this bind held down this frame */
|
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
|
#if RTC
|
||||||
/* Debug */
|
/* Debug */
|
||||||
|
|
||||||
[SYS_BTN_FORWARD_SLASH] = USER_BIND_KIND_RESET_GJK_STEPS,
|
[SYS_BTN_FORWARD_SLASH] = USER_BIND_KIND_RESET_COLLIDER_GJK_STEPS,
|
||||||
[SYS_BTN_COMMA] = USER_BIND_KIND_DECR_GJK_STEPS,
|
[SYS_BTN_COMMA] = USER_BIND_KIND_DECR_COLLIDER_GJK_STEPS,
|
||||||
[SYS_BTN_PERIOD] = USER_BIND_KIND_INCR_GJK_STEPS
|
[SYS_BTN_PERIOD] = USER_BIND_KIND_INCR_COLLIDER_GJK_STEPS
|
||||||
#endif
|
#endif
|
||||||
};
|
};
|
||||||
|
|
||||||
@ -1095,7 +1095,7 @@ INTERNAL void user_update(void)
|
|||||||
u32 color_second = RGBA_32_F(0, 1, 0, 0.75);
|
u32 color_second = RGBA_32_F(0, 1, 0, 0.75);
|
||||||
u32 color_third = RGBA_32_F(0, 0, 1, 0.75);
|
u32 color_third = RGBA_32_F(0, 0, 1, 0.75);
|
||||||
|
|
||||||
struct gjk_simplex simplex = ent->simplex;
|
struct collider_simplex simplex = ent->simplex;
|
||||||
struct v2 simplex_points[] = { simplex.a, simplex.b, simplex.c };
|
struct v2 simplex_points[] = { simplex.a, simplex.b, simplex.c };
|
||||||
for (u64 i = 0; i < ARRAY_COUNT(simplex_points); ++i) simplex_points[i] = xform_mul_v2(G.world_view, simplex_points[i]);
|
for (u64 i = 0; i < ARRAY_COUNT(simplex_points); ++i) simplex_points[i] = xform_mul_v2(G.world_view, simplex_points[i]);
|
||||||
struct v2_array simplex_array = { .count = simplex.len, .points = simplex_points };
|
struct v2_array simplex_array = { .count = simplex.len, .points = simplex_points };
|
||||||
@ -1295,11 +1295,11 @@ INTERNAL void user_update(void)
|
|||||||
#if RTC
|
#if RTC
|
||||||
/* Gjk steps */
|
/* Gjk steps */
|
||||||
{
|
{
|
||||||
i64 new_steps = gjk_debug_steps;
|
i64 new_steps = collider_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_INCR_COLLIDER_GJK_STEPS].num_presses_and_repeats;
|
||||||
new_steps -= G.bind_states[USER_BIND_KIND_DECR_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_GJK_STEPS].num_presses_and_repeats > 0) new_steps = 0;
|
if (G.bind_states[USER_BIND_KIND_RESET_COLLIDER_GJK_STEPS].num_presses_and_repeats > 0) new_steps = 0;
|
||||||
gjk_debug_steps = (u32)clamp_i64(new_steps, 0, U32_MAX);
|
collider_debug_steps = (u32)clamp_i64(new_steps, 0, U32_MAX);
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
@ -1374,7 +1374,7 @@ INTERNAL void user_update(void)
|
|||||||
pos.y += spacing;
|
pos.y += spacing;
|
||||||
|
|
||||||
#if RTC
|
#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;
|
pos.y += spacing;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|||||||
@ -38,9 +38,9 @@ enum user_bind_kind {
|
|||||||
#if RTC
|
#if RTC
|
||||||
/* Debug */
|
/* Debug */
|
||||||
|
|
||||||
USER_BIND_KIND_RESET_GJK_STEPS,
|
USER_BIND_KIND_RESET_COLLIDER_GJK_STEPS,
|
||||||
USER_BIND_KIND_INCR_GJK_STEPS,
|
USER_BIND_KIND_INCR_COLLIDER_GJK_STEPS,
|
||||||
USER_BIND_KIND_DECR_GJK_STEPS,
|
USER_BIND_KIND_DECR_COLLIDER_GJK_STEPS,
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
USER_BIND_KIND_COUNT
|
USER_BIND_KIND_COUNT
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user