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

de-duplicate collider.c gjk & epa logic

Browse Source
This commit is contained in:
jacob 2025-01-10 11:37:27 -06:00
parent 02290601c3
commit 1849f1143a
4 changed files with 409 additions and 553 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

623
src/collider.c
View File

@ -3,6 +3,15 @@
#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;
@ -87,58 +96,39 @@ INTERNAL struct collider_menkowski_point get_menkowski_point(struct collider_sha
return res;
}
struct collider_collision_points_result collider_collision_points(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1)
{
struct temp_arena scratch = scratch_begin_no_conflict(); /* TODO: Only begin scratch for EPA */
struct collider_collision_points_result res = ZI;
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;
(UNUSED)radius0;
(UNUSED)radius1;
/* TODO: Parameterize */
/* How close can non-overlapping shapes be before collision is considered */
//const f32 tolerance = 0.f;
//const f32 tolerance = 0.05f;
const f32 tolerance = 0.005f;
/* NOTE: Should always be less than tolerance, since colliding=true if origin is within this distance. */
//const f32 min_unique_pt_dist_sq = 0.0001f * 0.0001f;
const f32 min_unique_pt_dist_sq = 0.001f * 0.001f;
/* To prevent extremely large prototypes when origin is in exact center of rounded feature */
const u32 max_epa_iterations = 64;
b32 colliding = false;
b32 simplex_is_closest_edge = false;
struct collider_simplex s = ZI;
struct collider_menkowski_point *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 collider_menkowski_point 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)
* 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);
@ -157,7 +147,7 @@ struct collider_collision_points_result collider_collision_points(struct collide
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) {
simplex_is_closest_edge = true;
overlapping = false;
break;
}
s.b = s.a;
@ -179,10 +169,8 @@ struct collider_collision_points_result collider_collision_points(struct collide
(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)
{
colliding = false;
simplex_is_closest_edge = true;
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;
@ -194,7 +182,7 @@ struct collider_collision_points_result collider_collision_points(struct collide
(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 */
colliding = true;
overlapping = true;
break;
}
}
@ -260,19 +248,59 @@ struct collider_collision_points_result collider_collision_points(struct collide
s.a = s.c;
} else {
/* No region, must be in simplex */
colliding = true;
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;
}
if (colliding) {
/* ========================== *
* Epa (to find collision normal from inside shape)
* 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);
proto_count = 0;
{
ASSERT(s.len == 3);
struct collider_menkowski_point *tmp = arena_push_array(scratch.arena, struct collider_menkowski_point, 3);
@ -285,7 +313,7 @@ struct collider_collision_points_result collider_collision_points(struct collide
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 (colliding) {
while (true) {
++epa_iterations;
/* Find dir from origin to closest edge */
@ -317,16 +345,18 @@ struct collider_collision_points_result collider_collision_points(struct collide
struct v2 vab = v2_sub(closest_b.p, closest_a.p);
/* Find new point in dir */
dir = v2_mul(v2_perp(vab), winding);
m = get_menkowski_point(shape0, shape1, xf0, xf1, 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);
s.a = closest_a;
s.b = closest_b;
s.len = 2;
closest_feature.a = closest_a;
closest_feature.b = closest_b;
closest_feature.len = 2;
}
#endif
/* Check validity of new point */
DBGSTEP;
@ -352,9 +382,11 @@ struct collider_collision_points_result collider_collision_points(struct collide
}
}
if (!valid || epa_iterations >= max_epa_iterations) {
res.path = 1;
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;
}
}
@ -369,35 +401,109 @@ struct collider_collision_points_result collider_collision_points(struct collide
}
proto[closest_b_index] = m;
}
} else if (simplex_is_closest_edge) {
if (s.len == 1) {
struct v2 p = v2_neg(s.a.p);
} 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)) {
res.path = 2;
normal = v2_norm(dir);
colliding = true;
}
} else {
/* 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.p, s.a.p);
struct v2 vao = v2_neg(s.a.p);
/* 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(s.a.p, v2_mul(vab, ratio));
struct v2 p = v2_add(f.a.p, v2_mul(vab, ratio));
if (v2_len_sq(p) <= (tolerance * tolerance)) {
res.path = 2;
normal = v2_norm(dir);
colliding = true;
}
}
}
/* Clip to determine final points */
if (colliding) {
/* ========================== *
* Clip to determine final points
* ========================== */
/* Max vertices must be < 16 to fit in 4 bit ids */
CT_ASSERT(ARRAY_COUNT(shape0->points) <= 16);
@ -647,21 +753,17 @@ struct collider_collision_points_result collider_collision_points(struct collide
}
}
res.solved = true;
#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
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;
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;
}
@ -673,347 +775,80 @@ struct collider_collision_points_result collider_collision_points(struct collide
struct collider_closest_points_result collider_closest_points(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1)
{
struct temp_arena scratch = scratch_begin_no_conflict(); /* TODO: Only begin scratch for EPA */
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;
f32 radius0 = shape0->radius;
f32 radius1 = shape1->radius;
(UNUSED)radius0;
(UNUSED)radius1;
/* TODO: Parameterize */
/* How close can non-overlapping shapes be before collision is considered */
//const f32 tolerance = 0.f;
//const f32 tolerance = 0.05f;
const f32 tolerance = 0.005f;
/* NOTE: Should always be less than tolerance, since colliding=true if origin is within this distance. */
//const f32 min_unique_pt_dist_sq = 0.0001f * 0.0001f;
const f32 min_unique_pt_dist_sq = 0.001f * 0.001f;
/* To prevent extremely large prototypes when origin is in exact center of rounded feature */
const u32 max_epa_iterations = 64;
b32 colliding = false;
b32 simplex_is_closest_edge = false;
struct collider_simplex s = ZI;
struct collider_menkowski_point *proto = NULL;
u32 proto_count = 0;
struct v2 normal = ZI;
struct v2 dir = ZI;
struct collider_menkowski_point 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 */
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);
/* 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;
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) {
simplex_is_closest_edge = 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.p, s.a.p), v2_neg(s.a.p));
}
{
/* 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;
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) {
colliding = false;
simplex_is_closest_edge = true;
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 */
colliding = 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 */
colliding = true;
break;
}
}
}
if (colliding) {
/* ========================== *
* Epa (to find collision normal from inside shape)
* ========================== */
proto = arena_dry_push(scratch.arena, struct collider_menkowski_point);
proto_count = 0;
{
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 (colliding) {
++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 */
dir = v2_mul(v2_perp(vab), winding);
m = get_menkowski_point(shape0, shape1, xf0, xf1, dir);
/* TODO: Remove this (debugging) */
{
normal = v2_norm(dir);
s.a = closest_a;
s.b = closest_b;
s.len = 2;
}
/* 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_epa_iterations) {
res.path = 1;
normal = v2_norm(dir);
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 if (simplex_is_closest_edge) {
if (s.len == 1) {
struct v2 p = v2_neg(s.a.p);
if (v2_len_sq(p) <= (tolerance * tolerance)) {
res.path = 2;
normal = v2_norm(dir);
colliding = true;
}
} else {
/* 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.p, s.a.p);
struct v2 vao = v2_neg(s.a.p);
f32 ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1);
struct v2 p = v2_add(s.a.p, v2_mul(vab, ratio));
if (v2_len_sq(p) <= (tolerance * tolerance)) {
res.path = 2;
normal = v2_norm(dir);
colliding = true;
}
}
}
/* ========================== *
* Resolve points
* ========================== */
if (s.len == 1) {
p0 = s.a.s0;
p1 = s.a.s1;
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(s.len == 2);
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(s.b.p, s.a.p);
struct v2 vao = v2_neg(s.a.p);
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(s.b.s0, s.a.s0);
p0 = v2_sub(f.b.s0, f.a.s0);
p0 = v2_mul(p0, ratio);
p0 = v2_add(p0, s.a.s0);
p0 = v2_add(p0, f.a.s0);
/* Shape 1 */
p1 = v2_sub(s.b.s1, s.a.s1);
p1 = v2_sub(f.b.s1, f.a.s1);
p1 = v2_mul(p1, ratio);
p1 = v2_add(p1, s.a.s1);
p1 = v2_add(p1, f.a.s1);
colliding = gjk_res.overlapping || v2_len_sq(v2_sub(p1, p0)) <= (tolerance * tolerance);
}
res.solved = true;
#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
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;
res.normal = normal;
res.simplex = s;
res.p0 = p0;
res.p1 = p1;
res.colliding = colliding;
scratch_end(scratch);
return res;
}

14
src/collider.h
View File

@ -18,11 +18,16 @@ struct collider_menkowski_point {
struct v2 s1; /* Support point of second shape in -dir */
};
struct collider_simplex {
struct collider_menkowski_simplex {
u32 len;
struct collider_menkowski_point a, b, c;
};
struct collider_menkowski_feature {
u32 len;
struct collider_menkowski_point a, b;
};
struct collider_collision_point {
struct v2 point;
f32 separation;
@ -37,8 +42,7 @@ struct collider_collision_points_result {
/* For debugging */
b32 solved;
i32 path;
struct collider_simplex simplex;
struct collider_menkowski_simplex simplex;
struct collider_prototype prototype;
struct v2 a0, b0, a1, b1; /* Clipping faces */
};
@ -46,14 +50,12 @@ struct collider_collision_points_result {
struct collider_collision_points_result collider_collision_points(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1);
struct collider_closest_points_result {
struct v2 normal;
struct v2 p0, p1;
b32 colliding;
/* For debugging */
b32 solved;
i32 path;
struct collider_simplex simplex;
struct collider_menkowski_simplex simplex;
struct collider_prototype prototype;
};

3
src/entity.h
View File

@ -9,7 +9,8 @@ enum entity_prop {
ENTITY_PROP_ACTIVE,
ENTITY_PROP_RELEASE_AT_END_OF_FRAME,
ENTITY_PROP_RELEASE_BEFORE_PUBLISH,
ENTITY_PROP_RELEASE_AFTER_PUBLISH,
ENTITY_PROP_PHYSICAL_DYNAMIC,
ENTITY_PROP_PHYSICAL_KINEMATIC,

96
src/game.c
View File

@ -543,7 +543,7 @@ INTERNAL void create_contacts(void)
event->hit_event.e0 = e0->handle;
event->hit_event.e1 = e1->handle;
event->hit_event.normal = collider_res.normal;
entity_enable_prop(event, ENTITY_PROP_RELEASE_AT_END_OF_FRAME);
entity_enable_prop(event, ENTITY_PROP_RELEASE_BEFORE_PUBLISH);
entity_enable_prop(event, ENTITY_PROP_ACTIVE);
/* Calculate point */
@ -778,7 +778,7 @@ INTERNAL void prepare_contacts(void)
} else {
/* Mark constraint for removal */
entity_disable_prop(constraint_ent, ENTITY_PROP_ACTIVE);
entity_enable_prop(constraint_ent, ENTITY_PROP_RELEASE_AT_END_OF_FRAME);
entity_enable_prop(constraint_ent, ENTITY_PROP_RELEASE_BEFORE_PUBLISH);
/* Remove from lookup */
struct entity_lookup_key key = entity_lookup_key_from_two_handles(constraint->e0, constraint->e1);
struct entity_lookup_entry *entry = entity_lookup_get(&G.contact_lookup, key);
@ -806,7 +806,7 @@ INTERNAL void prepare_contacts(void)
|| !(entity_has_prop(e1, ENTITY_PROP_PHYSICAL_DYNAMIC) || entity_has_prop(e1, ENTITY_PROP_PHYSICAL_KINEMATIC))) {
/* Mark dbg ent for removal */
entity_disable_prop(dbg_ent, ENTITY_PROP_ACTIVE);
entity_enable_prop(dbg_ent, ENTITY_PROP_RELEASE_AT_END_OF_FRAME);
entity_enable_prop(dbg_ent, ENTITY_PROP_RELEASE_BEFORE_PUBLISH);
/* Remove from lookup */
struct entity_lookup_key key = entity_lookup_key_from_two_handles(dbg->e0, dbg->e1);
@ -816,7 +816,7 @@ INTERNAL void prepare_contacts(void)
--e0->colliding;
}
if (e1->valid) {
--e0->colliding;
--e1->colliding;
}
if (entry) {
@ -1064,7 +1064,7 @@ INTERNAL void prepare_motor_joints(void)
} else {
/* Mark joint for removal */
entity_disable_prop(joint_ent, ENTITY_PROP_ACTIVE);
entity_enable_prop(joint_ent, ENTITY_PROP_RELEASE_AT_END_OF_FRAME);
entity_enable_prop(joint_ent, ENTITY_PROP_RELEASE_BEFORE_PUBLISH);
}
}
}
@ -1307,7 +1307,7 @@ INTERNAL void prepare_mouse_joints(void)
} else {
/* Mark joint for removal */
entity_disable_prop(joint_ent, ENTITY_PROP_ACTIVE);
entity_enable_prop(joint_ent, ENTITY_PROP_RELEASE_AT_END_OF_FRAME);
entity_enable_prop(joint_ent, ENTITY_PROP_RELEASE_BEFORE_PUBLISH);
}
}
}
@ -1581,10 +1581,6 @@ INTERNAL f32 toi(struct collider_shape *c0, struct collider_shape *c1,
++iteration;
}
if (iteration > 1) {
DEBUGBREAKABLE;
}
return t;
}
@ -1594,7 +1590,6 @@ INTERNAL f32 determine_earliest_toi(f32 dt, f32 tolerance, u32 max_iterations)
f32 smallest_t = 1;
struct entity_store *store = G.tick.entity_store;
//struct entity *root = G.root;
for (u64 e0_index = 0; e0_index < store->reserved; ++e0_index) {
struct entity *e0 = &store->entities[e0_index];
@ -1646,7 +1641,37 @@ INTERNAL f32 determine_earliest_toi(f32 dt, f32 tolerance, u32 max_iterations)
return smallest_t;
}
/* ========================== *
* Release entities
* ========================== */
INTERNAL void release_entities_with_prop(enum entity_prop prop)
{
struct temp_arena scratch = scratch_begin_no_conflict();
struct entity_store *store = G.tick.entity_store;
struct entity **ents_to_release = arena_dry_push(scratch.arena, struct entity *);
u64 ents_to_release_count = 0;
for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
struct entity *ent = &store->entities[entity_index];
if (!ent->valid) continue;
if (entity_has_prop(ent, prop)) {
*arena_push(scratch.arena, struct entity *) = ent;
++ents_to_release_count;
}
}
for (u64 i = 0; i < ents_to_release_count; ++i) {
struct entity *ent = ents_to_release[i];
if (ent->valid) {
/* Release */
entity_release(store, ent);
}
}
scratch_end(scratch);
}
/* ========================== *
* Update
@ -2271,7 +2296,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
#if GAME_PHYSICS_ENABLE_TOI
const f32 min_toi = 0.000001f;
const f32 tolerance = 0.00001f;
const u32 max_iterations = 128;
const u32 max_iterations = 16;
earliest_toi = max_f32(determine_earliest_toi(remaining_dt, tolerance, max_iterations), min_toi);
#else
(UNUSED)toi;
@ -2335,7 +2360,18 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
(UNUSED)bullet;
(UNUSED)target;
entity_enable_prop(bullet, ENTITY_PROP_RELEASE_AT_END_OF_FRAME);
#if 0
{
/* Set bullet position to hit position */
struct xform xf = entity_get_xform(bullet);
xf.og = event->point;
entity_set_xform(bullet, xf);
/* Release after publish so user sees bullet in final postiion */
entity_enable_prop(bullet, ENTITY_PROP_RELEASE_AFTER_PUBLISH);
}
#else
entity_enable_prop(bullet, ENTITY_PROP_RELEASE_BEFORE_PUBLISH);
#endif
/* Create test blood */
/* TODO: Remove this */
@ -2451,37 +2487,13 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
}
/* ========================== *
* Release entities
* Release entities pre-publish
* ========================== */
/* TODO: Breadth first iteration to only release parent entities (since
* child entities will be released along with parent anyway) */
{
struct temp_arena temp = arena_temp_begin(scratch.arena);
struct entity **ents_to_release = arena_dry_push(temp.arena, struct entity *);
u64 ents_to_release_count = 0;
for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
struct entity *ent = &store->entities[entity_index];
if (!ent->valid) continue;
if (entity_has_prop(ent, ENTITY_PROP_RELEASE_AT_END_OF_FRAME)) {
*arena_push(temp.arena, struct entity *) = ent;
++ents_to_release_count;
}
}
for (u64 i = 0; i < ents_to_release_count; ++i) {
struct entity *ent = ents_to_release[i];
if (ent->valid) {
/* Release */
entity_release(store, ent);
}
}
arena_temp_end(temp);
}
release_entities_with_prop(ENTITY_PROP_RELEASE_BEFORE_PUBLISH);
/* ========================== *
* Publish tick
@ -2491,6 +2503,12 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
publish_game_tick();
__profframe("Game");
/* ========================== *
* Release entities post-publish
* ========================== */
release_entities_with_prop(ENTITY_PROP_RELEASE_AFTER_PUBLISH);
/* ========================== *
* End frame cache scopes
* ========================== */