956 lines
32 KiB
C
956 lines
32 KiB
C
#include "collider.h"
|
|
#include "math.h"
|
|
#include "arena.h"
|
|
#include "scratch.h"
|
|
|
|
/* How close can non-overlapping shapes be before collision is considered */
|
|
#define COLLISION_TOLERANCE 0.005f
|
|
|
|
/* NOTE: Should always be less than tolerance, since colliding = true if origin is within this distance. */
|
|
#define MIN_UNIQUE_PT_DIST_SQ (0.001f * 0.001f)
|
|
|
|
/* To prevent extremely large prototypes when origin is in exact center of rounded feature */
|
|
#define MAX_EPA_ITERATIONS 64
|
|
|
|
#if COLLIDER_DEBUG
|
|
u32 collider_debug_steps = U32_MAX;
|
|
//u32 collider_debug_steps = 1000000;
|
|
//u32 collider_debug_steps = 50;
|
|
|
|
INTERNAL void _dbgbreakable(void)
|
|
{
|
|
#if RTC
|
|
DEBUGBREAKABLE;
|
|
#endif
|
|
}
|
|
|
|
#define DBGSTEP \
|
|
dbg_step++; \
|
|
if (dbg_step >= collider_debug_steps) { \
|
|
goto abort; \
|
|
} else if (dbg_step >= collider_debug_steps - 1) { \
|
|
_dbgbreakable(); \
|
|
}
|
|
#else
|
|
#define DBGSTEP
|
|
#endif
|
|
|
|
struct v2 collider_support_point(struct collider_shape *a, struct xform xf, struct v2 dir)
|
|
{
|
|
struct v2 *points = a->points;
|
|
u32 count = a->count;
|
|
f32 radius = a->radius;
|
|
|
|
dir = v2_rotated(dir, -xform_get_rotation(xf));
|
|
dir = v2_mul_v2(dir, xform_get_scale(xf));
|
|
|
|
struct v2 furthest = ZI;
|
|
f32 furthest_dot = -F32_INFINITY;
|
|
for (u32 i = 0; i < count; ++i) {
|
|
struct v2 p = points[i];
|
|
f32 dot = v2_dot(dir, p);
|
|
if (dot > furthest_dot) {
|
|
furthest = p;
|
|
furthest_dot = dot;
|
|
}
|
|
}
|
|
|
|
if (radius > 0.0) {
|
|
dir = v2_with_len(dir, radius);
|
|
furthest = v2_add(furthest, dir);
|
|
}
|
|
|
|
furthest = xform_mul_v2(xf, furthest);
|
|
|
|
return furthest;
|
|
}
|
|
|
|
INTERNAL u32 collider_support_point_index(struct collider_shape *a, struct xform xf, struct v2 dir)
|
|
{
|
|
/* TODO: Could probably binary search for largest dot since shape is convex */
|
|
struct v2 *points = a->points;
|
|
u32 count = a->count;
|
|
|
|
dir = v2_rotated(dir, -xform_get_rotation(xf));
|
|
dir = v2_mul_v2(dir, xform_get_scale(xf));
|
|
|
|
u32 furthest = 0;
|
|
f32 furthest_dot = -F32_INFINITY;
|
|
for (u32 i = 0; i < count; ++i) {
|
|
struct v2 p = points[i];
|
|
f32 dot = v2_dot(dir, p);
|
|
if (dot > furthest_dot) {
|
|
furthest = i;
|
|
furthest_dot = dot;
|
|
}
|
|
}
|
|
return furthest;
|
|
}
|
|
|
|
INTERNAL struct collider_menkowski_point get_menkowski_point(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1, struct v2 dir)
|
|
{
|
|
struct collider_menkowski_point res;
|
|
res.s0 = collider_support_point(shape0, xf0, dir);
|
|
res.s1 = collider_support_point(shape1, xf1, v2_neg(dir));
|
|
res.p = v2_sub(res.s0, res.s1);
|
|
return res;
|
|
}
|
|
|
|
/* ========================== *
|
|
* GJK
|
|
*
|
|
* Determine simplex in menkowksi difference that encapsulates origin if shapes
|
|
* overlap, or closest edge / point to origin on menkowski difference if they
|
|
* do not.
|
|
* ========================== */
|
|
|
|
struct gjk_result {
|
|
struct collider_menkowski_simplex simplex;
|
|
struct v2 final_dir;
|
|
|
|
/* If true, simplex represents triangle inside of menkowski difference
|
|
* encapsulating the origin. If false, simplex represents the closest
|
|
* feature on menkowski difference to the origin. */
|
|
b32 overlapping;
|
|
|
|
#if COLLIDER_DEBUG
|
|
u32 dbg_step;
|
|
#endif
|
|
};
|
|
|
|
#if COLLIDER_DEBUG
|
|
INTERNAL struct gjk_result gjk_get_simplex(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1, f32 min_unique_pt_dist_sq, u32 dbg_step)
|
|
#else
|
|
INTERNAL struct gjk_result gjk_get_simplex(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1, f32 min_unique_pt_dist_sq)
|
|
#endif
|
|
{
|
|
b32 overlapping = false;
|
|
struct collider_menkowski_simplex s = ZI;
|
|
struct v2 dir = ZI;
|
|
struct collider_menkowski_point m = ZI;
|
|
|
|
/* First point is support point in shape's general directions to eachother */
|
|
dir = v2_sub(xf1.og, xf0.og);
|
|
if (v2_is_zero(dir)) dir = V2(1, 0);
|
|
s.a = get_menkowski_point(shape0, shape1, xf0, xf1, dir);
|
|
s.len = 1;
|
|
|
|
struct v2 removed_a = ZI;
|
|
struct v2 removed_b = ZI;
|
|
u32 num_removed = 0;
|
|
while (true) {
|
|
if (s.len == 1) {
|
|
/* Second point is support point towards origin */
|
|
dir = v2_neg(s.a.p);
|
|
|
|
DBGSTEP;
|
|
m = get_menkowski_point(shape0, shape1, xf0, xf1, dir);
|
|
/* Check that new point is far enough away from existing point */
|
|
if (v2_len_sq(v2_sub(m.p, s.a.p)) < min_unique_pt_dist_sq) {
|
|
overlapping = false;
|
|
break;
|
|
}
|
|
s.b = s.a;
|
|
s.a = m;
|
|
s.len = 2;
|
|
|
|
/* Third point is support point in direction of line normal towards origin */
|
|
dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p));
|
|
}
|
|
|
|
{
|
|
DBGSTEP;
|
|
m = get_menkowski_point(shape0, shape1, xf0, xf1, dir);
|
|
/* Check that new point is far enough away from existing points */
|
|
if (v2_len_sq(v2_sub(m.p, s.a.p)) < min_unique_pt_dist_sq ||
|
|
v2_len_sq(v2_sub(m.p, s.b.p)) < min_unique_pt_dist_sq ||
|
|
(
|
|
(num_removed >= 1) && (
|
|
(v2_len_sq(v2_sub(m.p, removed_a)) < min_unique_pt_dist_sq) ||
|
|
(num_removed >= 2 && v2_len_sq(v2_sub(m.p, removed_b)) < min_unique_pt_dist_sq))
|
|
) ||
|
|
math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_sub(m.p, s.a.p))) < min_unique_pt_dist_sq) {
|
|
overlapping = false;
|
|
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))) <= min_unique_pt_dist_sq) ||
|
|
(math_fabs(v2_wedge(v2_sub(s.c.p, s.b.p), v2_neg(s.b.p))) <= min_unique_pt_dist_sq) ||
|
|
(math_fabs(v2_wedge(v2_sub(s.c.p, s.a.p), v2_neg(s.a.p))) <= min_unique_pt_dist_sq)) {
|
|
/* Simplex lies on origin */
|
|
overlapping = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
/* Determine region of the simplex in which the origin lies */
|
|
DBGSTEP;
|
|
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);
|
|
|
|
f32 rab_dot = v2_dot(rab_dir, v2_neg(s.a.p));
|
|
f32 rac_dot = v2_dot(rac_dir, v2_neg(s.a.p));
|
|
f32 rbc_dot = v2_dot(rbc_dir, v2_neg(s.b.p));
|
|
|
|
f32 vab_dot = v2_dot(vab, v2_neg(s.a.p)) / v2_len_sq(vab);
|
|
f32 vac_dot = v2_dot(vac, v2_neg(s.a.p)) / v2_len_sq(vac);
|
|
f32 vbc_dot = v2_dot(vbc, v2_neg(s.b.p)) / v2_len_sq(vbc);
|
|
|
|
if (rab_dot >= 0 && vab_dot >= 0 && vab_dot <= 1) {
|
|
/* Region ab, remove c */
|
|
num_removed = 1;
|
|
removed_a = s.c.p;
|
|
s.len = 2;
|
|
dir = rab_dir; /* Next third point is in direction of region ab */
|
|
} else if (rac_dot >= 0 && vac_dot >= 0 && vac_dot <= 1) {
|
|
/* Region ac, remove b */
|
|
num_removed = 1;
|
|
removed_a = s.b.p;
|
|
s.len = 2;
|
|
s.b = s.c;
|
|
dir = rac_dir; /* Next third point is in direction of region ac */
|
|
} else if (rbc_dot >= 0 && vbc_dot >= 0 && vbc_dot <= 1) {
|
|
/* Region bc, remove a */
|
|
num_removed = 1;
|
|
removed_a = s.a.p;
|
|
s.len = 2;
|
|
s.a = s.b;
|
|
s.b = s.c;
|
|
dir = rbc_dir; /* Next third point is in direction of region bc */
|
|
} else if (vab_dot <= 0 && vac_dot <= 0) {
|
|
/* Region a, remove bc */
|
|
num_removed = 2;
|
|
removed_a = s.b.p;
|
|
removed_b = s.c.p;
|
|
s.len = 1;
|
|
} else if (vab_dot >= 1 && vbc_dot <= 0) {
|
|
/* Region b, remove ac */
|
|
num_removed = 2;
|
|
removed_a = s.a.p;
|
|
removed_b = s.c.p;
|
|
s.len = 1;
|
|
s.a = s.b;
|
|
} else if (vac_dot >= 1 && vbc_dot >= 1) {
|
|
/* Region c, remove ab */
|
|
num_removed = 2;
|
|
removed_a = s.a.p;
|
|
removed_b = s.b.p;
|
|
s.len = 1;
|
|
s.a = s.c;
|
|
} else {
|
|
/* No region, must be in simplex */
|
|
overlapping = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
#if COLLIDER_DEBUG
|
|
abort:
|
|
#endif
|
|
|
|
struct gjk_result res = {
|
|
.simplex = s,
|
|
.overlapping = overlapping,
|
|
.final_dir = dir,
|
|
#if COLLIDER_DEBUG
|
|
.dbg_step = dbg_step
|
|
#endif
|
|
};
|
|
|
|
return res;
|
|
}
|
|
|
|
/* ========================== *
|
|
* EPA
|
|
*
|
|
* Expands upon result of GJK calculation to determine collision normal & closest edge when shapes are overlapping
|
|
* ========================== */
|
|
|
|
struct epa_result {
|
|
struct v2 normal;
|
|
struct collider_menkowski_feature closest_feature; /* Represents closest feature (edge or point) to origin on menkowski difference */
|
|
|
|
#if COLLIDER_DEBUG
|
|
struct collider_prototype prototype;
|
|
u32 dbg_step;
|
|
#endif
|
|
};
|
|
|
|
#if COLLIDER_DEBUG
|
|
INTERNAL struct epa_result epa_get_normal_from_gjk(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1, struct gjk_result gjk_res, f32 min_unique_pt_dist_sq, u32 max_iterations, u32 dbg_step)
|
|
#else
|
|
INTERNAL struct epa_result epa_get_normal_from_gjk(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1, struct gjk_result gjk_res, f32 min_unique_pt_dist_sq, u32 max_iterations)
|
|
#endif
|
|
{
|
|
struct temp_arena scratch = scratch_begin_no_conflict();
|
|
|
|
struct collider_menkowski_feature closest_feature = ZI;
|
|
struct v2 normal = ZI;
|
|
|
|
struct collider_menkowski_point *proto = NULL;
|
|
u32 proto_count = 0;
|
|
if (gjk_res.overlapping) {
|
|
struct collider_menkowski_simplex s = gjk_res.simplex;
|
|
proto = arena_dry_push(scratch.arena, struct collider_menkowski_point);
|
|
{
|
|
ASSERT(s.len == 3);
|
|
struct collider_menkowski_point *tmp = arena_push_array(scratch.arena, struct collider_menkowski_point, 3);
|
|
tmp[0] = s.a;
|
|
tmp[1] = s.b;
|
|
tmp[2] = s.c;
|
|
proto_count = 3;
|
|
}
|
|
|
|
i32 winding = v2_winding(v2_sub(s.c.p, s.a.p), v2_sub(s.b.p, s.a.p));
|
|
|
|
u32 epa_iterations = 0;
|
|
while (true) {
|
|
++epa_iterations;
|
|
|
|
/* Find dir from origin to closest edge */
|
|
/* FIXME: Winding order of ps & pe index */
|
|
f32 closest_len_sq = F32_INFINITY;
|
|
struct collider_menkowski_point closest_a = ZI;
|
|
struct collider_menkowski_point closest_b = ZI;
|
|
u32 closest_b_index = 0;
|
|
for (u32 i = 0; i < proto_count; ++i) {
|
|
u32 a_index = i;
|
|
u32 b_index = (i < proto_count - 1) ? (i + 1) : 0;
|
|
struct collider_menkowski_point a = proto[a_index];
|
|
struct collider_menkowski_point b = proto[b_index];
|
|
|
|
struct v2 vab = v2_sub(b.p, a.p);
|
|
struct v2 vao = v2_neg(a.p);
|
|
|
|
f32 proj_ratio = clamp_f32(v2_dot(vao, vab) / v2_len_sq(vab), 0, 1);
|
|
struct v2 proj = v2_add(a.p, v2_mul(vab, proj_ratio));
|
|
|
|
f32 proj_len_sq = v2_len_sq(proj);
|
|
if (proj_len_sq < closest_len_sq - min_unique_pt_dist_sq) {
|
|
closest_a = a;
|
|
closest_b = b;
|
|
closest_b_index = b_index;
|
|
closest_len_sq = proj_len_sq;
|
|
}
|
|
}
|
|
struct v2 vab = v2_sub(closest_b.p, closest_a.p);
|
|
|
|
/* Find new point in dir */
|
|
struct v2 dir = v2_mul(v2_perp(vab), winding);
|
|
struct collider_menkowski_point m = get_menkowski_point(shape0, shape1, xf0, xf1, dir);
|
|
|
|
/* TODO: Remove this (debugging) */
|
|
#if COLLIDER_DEBUG
|
|
{
|
|
normal = v2_norm(dir);
|
|
closest_feature.a = closest_a;
|
|
closest_feature.b = closest_b;
|
|
closest_feature.len = 2;
|
|
}
|
|
#endif
|
|
|
|
/* Check validity of new point */
|
|
DBGSTEP;
|
|
{
|
|
b32 valid = true;
|
|
|
|
{
|
|
/* NOTE: Changing this value affects how stable normals are for circular colliders */
|
|
//const f32 validity_epsilon = min_unique_pt_dist_sq; /* Arbitrary */
|
|
const f32 validity_epsilon = 0.0000000001f; /* Arbitrary */
|
|
|
|
struct v2 vam = v2_sub(m.p, closest_a.p);
|
|
struct v2 vbm = v2_sub(closest_b.p, closest_a.p);
|
|
|
|
f32 dot = v2_dot(vab, vam) / v2_len_sq(vab);
|
|
|
|
if (dot >= -validity_epsilon && dot <= 1 - validity_epsilon && (v2_wedge(vab, vam) * -winding) >= -validity_epsilon) {
|
|
/* New point is not between edge */
|
|
valid = false;
|
|
} else if (v2_len_sq(vam) < min_unique_pt_dist_sq || v2_len_sq(vbm) < min_unique_pt_dist_sq) {
|
|
/* New point is too close to existing */
|
|
valid = false;
|
|
}
|
|
}
|
|
|
|
if (!valid || epa_iterations >= max_iterations) {
|
|
normal = v2_norm(dir);
|
|
closest_feature.a = closest_a;
|
|
closest_feature.b = closest_b;
|
|
closest_feature.len = 2;
|
|
break;
|
|
}
|
|
}
|
|
|
|
/* Insert point into prototype */
|
|
arena_push(scratch.arena, struct collider_menkowski_point);
|
|
++proto_count;
|
|
for (u32 i = proto_count - 1; i > closest_b_index; --i) {
|
|
u32 shift_from = (i > 0) ? i - 1 : proto_count - 1;
|
|
u32 shift_to = i;
|
|
proto[shift_to] = proto[shift_from];
|
|
}
|
|
proto[closest_b_index] = m;
|
|
}
|
|
} else {
|
|
normal = v2_norm(gjk_res.final_dir);
|
|
closest_feature.len = gjk_res.simplex.len;
|
|
closest_feature.a = gjk_res.simplex.a;
|
|
closest_feature.b = gjk_res.simplex.b;
|
|
}
|
|
|
|
#if COLLIDER_DEBUG
|
|
abort:
|
|
#endif
|
|
|
|
struct epa_result res = {
|
|
.normal = normal,
|
|
.closest_feature = closest_feature
|
|
};
|
|
|
|
#if COLLIDER_DEBUG
|
|
res.dbg_step = dbg_step;
|
|
u32 len = min_u32(proto_count, ARRAY_COUNT(res.prototype.points));
|
|
for (u32 i = 0; i < len; ++i) {
|
|
res.prototype.points[i] = proto[i].p;
|
|
}
|
|
res.prototype.len = len;
|
|
#endif
|
|
|
|
scratch_end(scratch);
|
|
return res;
|
|
}
|
|
|
|
/* ========================== *
|
|
* Collision points
|
|
* ========================== */
|
|
|
|
struct collider_collision_points_result collider_collision_points(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1)
|
|
{
|
|
struct collider_collision_points_result res = ZI;
|
|
|
|
const f32 tolerance = COLLISION_TOLERANCE;
|
|
const f32 min_unique_pt_dist_sq = MIN_UNIQUE_PT_DIST_SQ;
|
|
const u32 max_epa_iterations = MAX_EPA_ITERATIONS;
|
|
|
|
struct v2 *points0 = shape0->points;
|
|
struct v2 *points1 = shape1->points;
|
|
u32 count0 = shape0->count;
|
|
u32 count1 = shape1->count;
|
|
f32 radius0 = shape0->radius;
|
|
f32 radius1 = shape1->radius;
|
|
|
|
struct collider_collision_point points[2] = ZI;
|
|
u32 num_points = 0;
|
|
b32 colliding = false;
|
|
struct v2 normal = ZI;
|
|
|
|
#if COLLIDER_DEBUG
|
|
u32 dbg_step = 0;
|
|
#endif
|
|
|
|
|
|
/* Run GJK */
|
|
#if COLLIDER_DEBUG
|
|
struct gjk_result gjk_res = gjk_get_simplex(shape0, shape1, xf0, xf1, min_unique_pt_dist_sq, dbg_step);
|
|
dbg_step = gjk_res.dbg_step;
|
|
#else
|
|
struct gjk_result gjk_res = gjk_get_simplex(shape0, shape1, xf0, xf1, min_unique_pt_dist_sq);
|
|
#endif
|
|
DBGSTEP;
|
|
|
|
/* Run EPA */
|
|
#if COLLIDER_DEBUG
|
|
struct epa_result epa_res = epa_get_normal_from_gjk(shape0, shape1, xf0, xf1, gjk_res, min_unique_pt_dist_sq, max_epa_iterations, dbg_step);
|
|
dbg_step = epa_res.dbg_step;
|
|
#else
|
|
struct epa_result epa_res = epa_get_normal_from_gjk(shape0, shape1, xf0, xf1, gjk_res, min_unique_pt_dist_sq, max_epa_iterations);
|
|
#endif
|
|
normal = epa_res.normal;
|
|
DBGSTEP;
|
|
|
|
/* Determine collision */
|
|
if (gjk_res.overlapping) {
|
|
colliding = true;
|
|
} else {
|
|
struct collider_menkowski_feature f = epa_res.closest_feature;
|
|
/* Shapes not overlapping, determine if distance between shapes within tolerance */
|
|
if (f.len == 1) {
|
|
struct v2 p = v2_neg(f.a.p);
|
|
if (v2_len_sq(p) <= (tolerance * tolerance)) {
|
|
colliding = true;
|
|
}
|
|
} else {
|
|
/* Project origin to determine if distance is within tolerance. */
|
|
ASSERT(f.len == 2);
|
|
struct v2 vab = v2_sub(f.b.p, f.a.p);
|
|
struct v2 vao = v2_neg(f.a.p);
|
|
f32 ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1);
|
|
struct v2 p = v2_add(f.a.p, v2_mul(vab, ratio));
|
|
if (v2_len_sq(p) <= (tolerance * tolerance)) {
|
|
colliding = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
/* Clip to determine final points */
|
|
if (colliding) {
|
|
/* Max vertices must be < 16 to fit in 4 bit ids */
|
|
CT_ASSERT(ARRAY_COUNT(shape0->points) <= 16);
|
|
|
|
DBGSTEP;
|
|
{
|
|
const f32 wedge_epsilon = 0.05f;
|
|
//const f32 wedge_epsilon = 0.1f;
|
|
|
|
/* shape0 a -> b winding = clockwise */
|
|
u32 id_a0;
|
|
u32 id_b0;
|
|
struct v2 a0;
|
|
struct v2 b0;
|
|
|
|
/* shape1 a -> b winding = counterclockwise */
|
|
u32 id_a1;
|
|
u32 id_b1;
|
|
struct v2 a1;
|
|
struct v2 b1;
|
|
{
|
|
u32 p_i = collider_support_point_index(shape0, xf0, normal);
|
|
u32 a_i = (p_i > 0) ? (p_i - 1) : (count0 - 1);
|
|
u32 b_i = ((p_i + 1) < count0) ? (p_i + 1) : 0;
|
|
|
|
struct v2 p = xform_mul_v2(xf0, points0[p_i]);
|
|
struct v2 a = xform_mul_v2(xf0, points0[a_i]);
|
|
struct v2 b = xform_mul_v2(xf0, points0[b_i]);
|
|
|
|
struct v2 vap = v2_sub(p, a);
|
|
struct v2 vpb = v2_sub(b, p);
|
|
|
|
f32 vap_wedge = v2_wedge(vap, normal);
|
|
f32 vpb_wedge = v2_wedge(vpb, normal);
|
|
b32 reversed = count0 > 1 && v2_winding(vap, vpb) < 0;
|
|
if (reversed) {
|
|
/* Winding is reversed, swap vap & vpb wedge */
|
|
f32 t = vap_wedge;
|
|
vap_wedge = vpb_wedge;
|
|
vpb_wedge = t;
|
|
}
|
|
|
|
if (vap_wedge < (vpb_wedge + wedge_epsilon)) {
|
|
if (reversed) {
|
|
id_a0 = p_i;
|
|
id_b0 = a_i;
|
|
a0 = p;
|
|
b0 = a;
|
|
} else {
|
|
id_a0 = a_i;
|
|
id_b0 = p_i;
|
|
a0 = a;
|
|
b0 = p;
|
|
}
|
|
} else {
|
|
if (reversed) {
|
|
id_a0 = b_i;
|
|
id_b0 = p_i;
|
|
a0 = b;
|
|
b0 = p;
|
|
} else {
|
|
id_a0 = p_i;
|
|
id_b0 = b_i;
|
|
a0 = p;
|
|
b0 = b;
|
|
}
|
|
}
|
|
}
|
|
{
|
|
struct v2 neg_normal = v2_neg(normal);
|
|
|
|
u32 p_i = collider_support_point_index(shape1, xf1, neg_normal);
|
|
u32 a_i = (p_i > 0) ? (p_i - 1) : (count1 - 1);
|
|
u32 b_i = ((p_i + 1) < count1) ? (p_i + 1) : 0;
|
|
|
|
struct v2 p = xform_mul_v2(xf1, points1[p_i]);
|
|
struct v2 a = xform_mul_v2(xf1, points1[a_i]);
|
|
struct v2 b = xform_mul_v2(xf1, points1[b_i]);
|
|
|
|
struct v2 vap = v2_sub(p, a);
|
|
struct v2 vpb = v2_sub(b, p);
|
|
|
|
f32 vap_wedge = v2_wedge(vap, normal);
|
|
f32 vpb_wedge = v2_wedge(vpb, normal);
|
|
b32 reversed = count1 > 1 && v2_winding(vap, vpb) < 0;
|
|
if (reversed) {
|
|
/* Winding is reversed, swapvap & vpb wedge*/
|
|
f32 t = vap_wedge;
|
|
vap_wedge = vpb_wedge;
|
|
vpb_wedge = t;
|
|
}
|
|
|
|
if (vap_wedge > (vpb_wedge + wedge_epsilon)) {
|
|
if (reversed) {
|
|
id_a1 = a_i;
|
|
id_b1 = p_i;
|
|
a1 = a;
|
|
b1 = p;
|
|
} else {
|
|
id_a1 = p_i;
|
|
id_b1 = a_i;
|
|
a1 = p;
|
|
b1 = a;
|
|
}
|
|
} else {
|
|
if (reversed) {
|
|
id_a1 = p_i;
|
|
id_b1 = b_i;
|
|
a1 = p;
|
|
b1 = b;
|
|
} else {
|
|
id_a1 = b_i;
|
|
id_b1 = p_i;
|
|
a1 = b;
|
|
b1 = p;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (radius0 > 0.0) {
|
|
struct v2 radius_dir = v2_rotated(normal, -xform_get_rotation(xf0));
|
|
radius_dir = v2_mul_v2(radius_dir, xform_get_scale(xf0));
|
|
radius_dir = v2_with_len(radius_dir, radius0);
|
|
radius_dir = xform_basis_mul_v2(xf0, radius_dir);
|
|
a0 = v2_add(a0, radius_dir);
|
|
b0 = v2_add(b0, radius_dir);
|
|
}
|
|
|
|
if (radius1 > 0.0) {
|
|
struct v2 radius_dir = v2_rotated(normal, -xform_get_rotation(xf1));
|
|
radius_dir = v2_mul_v2(radius_dir, xform_get_scale(xf1));
|
|
radius_dir = v2_with_len(radius_dir, radius1);
|
|
radius_dir = xform_basis_mul_v2(xf1, radius_dir);
|
|
a1 = v2_sub(a1, radius_dir);
|
|
b1 = v2_sub(b1, radius_dir);
|
|
}
|
|
|
|
f32 a0t = 0;
|
|
f32 a1t = 0;
|
|
f32 b0t = 0;
|
|
f32 b1t = 0;
|
|
|
|
struct v2 vab0 = v2_sub(b0, a0);
|
|
struct v2 vab1 = v2_sub(b1, a1);
|
|
|
|
{
|
|
|
|
f32 vab0_wedge_normal = v2_wedge(vab0, normal);
|
|
f32 vab1_wedge_normal = v2_wedge(vab1, normal);
|
|
|
|
b32 collapse0 = math_fabs(vab0_wedge_normal) < wedge_epsilon;
|
|
b32 collapse1 = math_fabs(vab1_wedge_normal) < wedge_epsilon;
|
|
|
|
if (collapse0) {
|
|
if (v2_dot(b0, normal) >= v2_dot(a0, normal)) {
|
|
a0 = b0;
|
|
a0t = 1;
|
|
b0t = 0;
|
|
} else {
|
|
b0 = a0;
|
|
a0t = 0;
|
|
b0t = 1;
|
|
}
|
|
vab0 = V2(0, 0);
|
|
}
|
|
|
|
if (collapse1) {
|
|
if (v2_dot(b1, normal) < v2_dot(a1, normal)) {
|
|
a1 = b1;
|
|
a1t = 1;
|
|
b1t = 0;
|
|
} else {
|
|
b1 = a1;
|
|
a1t = 0;
|
|
b1t = 1;
|
|
}
|
|
vab1 = V2(0, 0);
|
|
}
|
|
|
|
struct v2 va0a1 = v2_sub(a1, a0);
|
|
struct v2 vb0b1 = v2_sub(b1, b0);
|
|
f32 va0a1_wedge_normal = v2_wedge(va0a1, normal);
|
|
f32 vb0b1_wedge_normal = v2_wedge(vb0b1, normal);
|
|
|
|
if (!collapse0) {
|
|
f32 w = 1 / vab0_wedge_normal;
|
|
a0t = clamp_f32(va0a1_wedge_normal * w, 0, 1);
|
|
b0t = clamp_f32(vb0b1_wedge_normal * -w, 0, 1);
|
|
}
|
|
|
|
if (!collapse1) {
|
|
f32 w = 1 / vab1_wedge_normal;
|
|
a1t = clamp_f32(-va0a1_wedge_normal * w, 0, 1);
|
|
b1t = clamp_f32(-vb0b1_wedge_normal * -w, 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(vab0, -b0t));
|
|
struct v2 b1_clipped = v2_add(b1, v2_mul(vab1, -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);
|
|
|
|
struct v2 contact_a = v2_add(a0_clipped, v2_mul(va0a1_clipped, 0.5f));
|
|
struct v2 contact_b = v2_add(b0_clipped, v2_mul(vb0b1_clipped, 0.5f));
|
|
|
|
b32 ignore_a = false;
|
|
b32 ignore_b = false;
|
|
if (v2_len_sq(v2_sub(contact_b, contact_a)) < (0.005f * 0.005f)) {
|
|
/* Merge contacts */
|
|
if (a_sep > b_sep) {
|
|
ignore_a = true;
|
|
} else {
|
|
ignore_b = true;
|
|
}
|
|
}
|
|
|
|
if (a_sep < tolerance && !ignore_a) {
|
|
struct collider_collision_point *point = &points[num_points++];
|
|
point->id = id_a0 | (id_a1 << 4);
|
|
point->separation = a_sep;
|
|
point->point = contact_a;
|
|
}
|
|
|
|
if (b_sep < tolerance && !ignore_b) {
|
|
struct collider_collision_point *point = &points[num_points++];
|
|
point->id = id_b0 | (id_b1 << 4);
|
|
point->separation = b_sep;
|
|
point->point = contact_b;
|
|
}
|
|
|
|
#if 1
|
|
res.a0 = a0_clipped;
|
|
res.a1 = a1_clipped;
|
|
res.b0 = b0_clipped;
|
|
res.b1 = b1_clipped;
|
|
#else
|
|
res.a0 = a0;
|
|
res.a1 = a1;
|
|
res.b0 = b0;
|
|
res.b1 = b1;
|
|
#endif
|
|
}
|
|
}
|
|
|
|
#if COLLIDER_DEBUG
|
|
res.solved = true;
|
|
abort:
|
|
res.simplex = gjk_res.simplex;
|
|
res.prototype.len = epa_res.prototype.len;
|
|
MEMCPY(res.prototype.points, epa_res.prototype.points, sizeof(res.prototype.points[0]) * res.prototype.len);
|
|
#endif
|
|
res.normal = normal;
|
|
res.points[0] = points[0];
|
|
res.points[1] = points[1];
|
|
res.num_points = num_points;
|
|
return res;
|
|
}
|
|
|
|
/* ========================== *
|
|
* Closest points
|
|
* ========================== */
|
|
|
|
/* TODO: De-duplicate code between collider_closest_points & collider_collision_points */
|
|
|
|
struct collider_closest_points_result collider_closest_points(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1)
|
|
{
|
|
struct collider_closest_points_result res = ZI;
|
|
|
|
const f32 tolerance = COLLISION_TOLERANCE;
|
|
const f32 min_unique_pt_dist_sq = MIN_UNIQUE_PT_DIST_SQ;
|
|
const u32 max_epa_iterations = MAX_EPA_ITERATIONS;
|
|
|
|
struct v2 p0 = ZI;
|
|
struct v2 p1 = ZI;
|
|
b32 colliding = false;
|
|
|
|
#if COLLIDER_DEBUG
|
|
u32 dbg_step = 0;
|
|
#endif
|
|
|
|
/* Run GJK */
|
|
#if COLLIDER_DEBUG
|
|
struct gjk_result gjk_res = gjk_get_simplex(shape0, shape1, xf0, xf1, min_unique_pt_dist_sq, dbg_step);
|
|
dbg_step = gjk_res.dbg_step;
|
|
#else
|
|
struct gjk_result gjk_res = gjk_get_simplex(shape0, shape1, xf0, xf1, min_unique_pt_dist_sq);
|
|
#endif
|
|
DBGSTEP;
|
|
|
|
/* Run EPA */
|
|
#if COLLIDER_DEBUG
|
|
struct epa_result epa_res = epa_get_normal_from_gjk(shape0, shape1, xf0, xf1, gjk_res, min_unique_pt_dist_sq, max_epa_iterations, dbg_step);
|
|
dbg_step = epa_res.dbg_step;
|
|
#else
|
|
struct epa_result epa_res = epa_get_normal_from_gjk(shape0, shape1, xf0, xf1, gjk_res, min_unique_pt_dist_sq, max_epa_iterations);
|
|
#endif
|
|
DBGSTEP;
|
|
|
|
/* ========================== *
|
|
* Resolve points
|
|
* ========================== */
|
|
|
|
colliding = gjk_res.overlapping;
|
|
struct collider_menkowski_feature f = epa_res.closest_feature;
|
|
if (f.len == 1) {
|
|
p0 = f.a.s0;
|
|
p1 = f.a.s1;
|
|
colliding = gjk_res.overlapping || v2_len_sq(v2_neg(f.a.p)) <= (tolerance * tolerance);
|
|
} else {
|
|
ASSERT(f.len == 2);
|
|
/* FIXME: Winding order dependent? */
|
|
f32 ratio;
|
|
{
|
|
/* Determine ratio between edge a & b that projected origin lies */
|
|
struct v2 vab = v2_sub(f.b.p, f.a.p);
|
|
struct v2 vao = v2_neg(f.a.p);
|
|
ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1);
|
|
}
|
|
/* Shape 0 */
|
|
p0 = v2_sub(f.b.s0, f.a.s0);
|
|
p0 = v2_mul(p0, ratio);
|
|
p0 = v2_add(p0, f.a.s0);
|
|
/* Shape 1 */
|
|
p1 = v2_sub(f.b.s1, f.a.s1);
|
|
p1 = v2_mul(p1, ratio);
|
|
p1 = v2_add(p1, f.a.s1);
|
|
colliding = gjk_res.overlapping || v2_len_sq(v2_sub(p1, p0)) <= (tolerance * tolerance);
|
|
}
|
|
|
|
#if COLLIDER_DEBUG
|
|
res.solved = true;
|
|
abort:
|
|
res.simplex = gjk_res.simplex;
|
|
res.prototype.len = epa_res.prototype.len;
|
|
MEMCPY(res.prototype.points, epa_res.prototype.points, sizeof(res.prototype.points[0]) *res.prototype.len);
|
|
res.simplex = gjk_res.simplex;
|
|
#endif
|
|
res.p0 = p0;
|
|
res.p1 = p1;
|
|
res.colliding = colliding;
|
|
return res;
|
|
}
|
|
|
|
/* ========================== *
|
|
* Debug functions
|
|
* TODO: Remove these
|
|
* ========================== */
|
|
|
|
/* TODO: Remove this (debugging) */
|
|
struct v2_array menkowski(struct arena *arena, struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1, u32 detail)
|
|
{
|
|
struct v2_array res = { .points = arena_dry_push(arena, struct v2) };
|
|
for (u64 i = 0; i < detail; ++i) {
|
|
f32 angle = ((f32)i / detail) * (2 * PI);
|
|
struct v2 dir = v2_from_angle(angle);
|
|
struct collider_menkowski_point m = get_menkowski_point(shape0, shape1, xf0, xf1, dir);
|
|
if (res.count == 0 || !v2_eq(m.p, res.points[res.count - 1])) {
|
|
*arena_push(arena, struct v2) = m.p;
|
|
++res.count;
|
|
}
|
|
}
|
|
return res;
|
|
}
|
|
|
|
/* TODO: Remove this (debugging) */
|
|
struct v2_array cloud(struct arena *arena, struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1)
|
|
{
|
|
/* FIXME: Account for radius */
|
|
struct v2_array res = { .points = arena_dry_push(arena, struct v2) };
|
|
struct v2 *points0 = shape0->points;
|
|
struct v2 *points1 = shape1->points;
|
|
u32 count0 = shape0->count;
|
|
u32 count1 = shape1->count;
|
|
for (u64 i = 0; i < count0; ++i) {
|
|
struct v2 p0 = xform_mul_v2(xf0, points0[i]);
|
|
for (u64 j = 0; j < count1; ++j) {
|
|
struct v2 p1 = xform_mul_v2(xf1, points1[j]);
|
|
*arena_push(arena, struct v2) = v2_sub(p0, p1);
|
|
++res.count;
|
|
}
|
|
}
|
|
return res;
|
|
}
|
|
|
|
/* ========================== *
|
|
* Boolean GJK (unused)
|
|
* ========================== */
|
|
|
|
#if 0
|
|
b32 collider_collision_boolean(struct collider_shape *shape0, struct collider_shape *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(starting_point(shape1), starting_point(shape0));
|
|
if (v2_is_zero(dir)) dir = V2(1, 0);
|
|
s.a = get_menkowski_point(shape0, shape1, dir);
|
|
|
|
/* Second point is support point towards origin */
|
|
dir = v2_neg(s.a);
|
|
p = get_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 = get_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;
|
|
}
|
|
#endif
|