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

create collider_closest_points function

Browse Source
This commit is contained in:
jacob 2025-01-06 11:41:59 -06:00
parent 2260b1bc36
commit 1e81a7ea41
5 changed files with 519 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

491
src/collider.c
View File

@ -76,9 +76,13 @@ INTERNAL u32 collider_support_point_index(struct collider_shape *a, struct xform
return furthest;
}
INTERNAL struct v2 menkowski_point(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1, struct v2 dir)
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)
{
return v2_sub(collider_support_point(shape0, xf0, dir), collider_support_point(shape1, xf1, v2_neg(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;
}
struct collider_collision_points_result collider_collision_points(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1)
@ -112,7 +116,7 @@ struct collider_collision_points_result collider_collision_points(struct collide
b32 simplex_is_closest_edge = false;
struct collider_simplex s = ZI;
struct v2 *proto = NULL;
struct collider_menkowski_point *proto = NULL;
u32 proto_count = 0;
struct v2 normal = ZI;
@ -120,7 +124,7 @@ struct collider_collision_points_result collider_collision_points(struct collide
u32 num_points = 0;
struct v2 dir = ZI;
struct v2 m = ZI;
struct collider_menkowski_point m = ZI;
#if COLLIDER_DEBUG
u32 dbg_step = 0;
@ -136,7 +140,7 @@ struct collider_collision_points_result collider_collision_points(struct collide
/* 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 = menkowski_point(shape0, shape1, xf0, xf1, dir);
s.a = get_menkowski_point(shape0, shape1, xf0, xf1, dir);
s.len = 1;
struct v2 removed_a = ZI;
@ -145,12 +149,12 @@ struct collider_collision_points_result collider_collision_points(struct collide
while (true) {
if (s.len == 1) {
/* Second point is support point towards origin */
dir = v2_neg(s.a);
dir = v2_neg(s.a.p);
DBGSTEP;
m = menkowski_point(shape0, shape1, xf0, xf1, dir);
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, s.a)) < min_unique_pt_dist_sq) {
if (v2_len_sq(v2_sub(m.p, s.a.p)) < min_unique_pt_dist_sq) {
simplex_is_closest_edge = true;
break;
}
@ -159,21 +163,21 @@ struct collider_collision_points_result collider_collision_points(struct collide
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));
dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p));
}
{
DBGSTEP;
m = menkowski_point(shape0, shape1, xf0, xf1, dir);
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, s.a)) < min_unique_pt_dist_sq ||
v2_len_sq(v2_sub(m, s.b)) < min_unique_pt_dist_sq ||
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, removed_a)) < min_unique_pt_dist_sq) ||
(num_removed >= 2 && v2_len_sq(v2_sub(m, removed_b)) < min_unique_pt_dist_sq))
(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, s.a), v2_sub(m, s.a))) < 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;
@ -184,9 +188,9 @@ struct collider_collision_points_result collider_collision_points(struct collide
s.a = m;
s.len = 3;
if ((math_fabs(v2_wedge(v2_sub(s.b, s.a), v2_neg(s.a))) <= min_unique_pt_dist_sq) ||
(math_fabs(v2_wedge(v2_sub(s.c, s.b), v2_neg(s.b))) <= min_unique_pt_dist_sq) ||
(math_fabs(v2_wedge(v2_sub(s.c, s.a), v2_neg(s.a))) <= min_unique_pt_dist_sq)) {
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;
@ -195,39 +199,39 @@ struct collider_collision_points_result collider_collision_points(struct collide
/* Determine region of the simplex in which the origin lies */
DBGSTEP;
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 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));
f32 rac_dot = v2_dot(rac_dir, v2_neg(s.a));
f32 rbc_dot = v2_dot(rbc_dir, v2_neg(s.b));
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)) / v2_len_sq(vab);
f32 vac_dot = v2_dot(vac, v2_neg(s.a)) / v2_len_sq(vac);
f32 vbc_dot = v2_dot(vbc, v2_neg(s.b)) / v2_len_sq(vbc);
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;
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;
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;
removed_a = s.a.p;
s.len = 2;
s.a = s.b;
s.b = s.c;
@ -235,21 +239,21 @@ struct collider_collision_points_result collider_collision_points(struct collide
} else if (vab_dot <= 0 && vac_dot <= 0) {
/* Region a, remove bc */
num_removed = 2;
removed_a = s.b;
removed_b = s.c;
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;
removed_b = s.c;
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;
removed_b = s.b;
removed_a = s.a.p;
removed_b = s.b.p;
s.len = 1;
s.a = s.c;
} else {
@ -265,18 +269,18 @@ struct collider_collision_points_result collider_collision_points(struct collide
* Epa (to find collision normal from inside shape)
* ========================== */
proto = arena_dry_push(scratch.arena, struct v2);
proto = arena_dry_push(scratch.arena, struct collider_menkowski_point);
proto_count = 0;
{
ASSERT(s.len == 3);
struct v2 *tmp = arena_push_array(scratch.arena, struct v2, 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, s.a), v2_sub(s.b, s.a));
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) {
@ -285,20 +289,20 @@ struct collider_collision_points_result collider_collision_points(struct collide
/* Find dir from origin to closest edge */
/* FIXME: Winding order of ps & pe index */
f32 closest_len_sq = F32_INFINITY;
struct v2 closest_a = ZI;
struct v2 closest_b = ZI;
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 v2 a = proto[a_index];
struct v2 b = proto[b_index];
struct collider_menkowski_point a = proto[a_index];
struct collider_menkowski_point b = proto[b_index];
struct v2 vab = v2_sub(b, a);
struct v2 vao = v2_neg(a);
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, v2_mul(vab, proj_ratio));
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) {
@ -308,11 +312,11 @@ struct collider_collision_points_result collider_collision_points(struct collide
closest_len_sq = proj_len_sq;
}
}
struct v2 vab = v2_sub(closest_b, closest_a);
struct v2 vab = v2_sub(closest_b.p, closest_a.p);
/* Find new point in dir */
dir = v2_mul(v2_perp(vab), winding);
m = menkowski_point(shape0, shape1, xf0, xf1, dir);
m = get_menkowski_point(shape0, shape1, xf0, xf1, dir);
/* TODO: Remove this (debugging) */
{
@ -332,8 +336,8 @@ struct collider_collision_points_result collider_collision_points(struct collide
//const f32 validity_epsilon = min_unique_pt_dist_sq; /* Arbitrary */
const f32 validity_epsilon = 0.0000000001f; /* Arbitrary */
struct v2 vam = v2_sub(m, closest_a);
struct v2 vbm = v2_sub(closest_b, closest_a);
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);
@ -365,7 +369,7 @@ struct collider_collision_points_result collider_collision_points(struct collide
}
} else if (simplex_is_closest_edge) {
if (s.len == 1) {
struct v2 p = v2_neg(s.a);
struct v2 p = v2_neg(s.a.p);
if (v2_len_sq(p) <= (tolerance * tolerance)) {
res.path = 2;
normal = v2_norm(dir);
@ -375,10 +379,10 @@ struct collider_collision_points_result collider_collision_points(struct collide
/* 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);
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, v2_mul(vab, ratio));
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);
@ -647,7 +651,7 @@ struct collider_collision_points_result collider_collision_points(struct collide
#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];
res.prototype.points[i] = proto[i].p;
}
res.prototype.len = len;
res.normal = normal;
@ -659,6 +663,358 @@ struct collider_collision_points_result collider_collision_points(struct collide
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 temp_arena scratch = scratch_begin_no_conflict(); /* TODO: Only begin scratch for EPA */
struct collider_closest_points_result res = ZI;
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);
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));
}
{
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;
} else {
ASSERT(s.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);
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_mul(p0, ratio);
p0 = v2_add(p0, s.a.s0);
/* Shape 1 */
p1 = v2_sub(s.b.s1, s.a.s1);
p1 = v2_mul(p1, ratio);
p1 = v2_add(p1, s.a.s1);
}
res.solved = true;
#if COLLIDER_DEBUG
abort :
#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;
}
/* ========================== *
* Debug functions
* TODO: Remove these
@ -671,9 +1027,9 @@ struct v2_array menkowski(struct arena *arena, struct collider_shape *shape0, st
for (u64 i = 0; i < detail; ++i) {
f32 angle = ((f32)i / detail) * (2 * PI);
struct v2 dir = v2_from_angle(angle);
struct v2 p = menkowski_point(shape0, shape1, xf0, xf1, dir);
if (res.count == 0 || !v2_eq(p, res.points[res.count - 1])) {
*arena_push(arena, struct v2) = p;
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;
}
}
@ -700,19 +1056,6 @@ struct v2_array cloud(struct arena *arena, struct collider_shape *shape0, struct
return res;
}
/* ========================== *
* Boolean GJK (unused)
* ========================== */
@ -728,18 +1071,18 @@ b32 collider_collision_boolean(struct collider_shape *shape0, struct collider_sh
/* 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 = menkowski_point(shape0, shape1, dir);
s.a = get_menkowski_point(shape0, shape1, dir);
/* Second point is support point towards origin */
dir = v2_neg(s.a);
p = menkowski_point(shape0, shape1, dir);
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 = menkowski_point(shape0, shape1, dir);
p = get_menkowski_point(shape0, shape1, dir);
if (v2_dot(dir, p) < 0) {
/* New point did not cross origin, collision impossible */
break;

29
src/collider.h
View File

@ -12,15 +12,15 @@ struct v2 collider_support_point(struct collider_shape *a, struct xform xf, stru
b32 collider_collision_boolean(struct collider_shape *shape0, struct collider_shape *shape1);
#endif
struct collider_simplex {
u32 len;
struct v2 a, b, c;
struct collider_menkowski_point {
struct v2 p; /* Menkowski difference point */
struct v2 s0; /* Support point of first shape in dir */
struct v2 s1; /* Support point of second shape in -dir */
};
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 */
struct collider_simplex {
u32 len;
struct collider_menkowski_point a, b, c;
};
struct collider_collision_point {
@ -31,7 +31,6 @@ struct collider_collision_point {
struct collider_prototype { struct v2 points[64]; u32 len; };
struct collider_collision_points_result {
struct v2 normal;
struct collider_collision_point points[2];
u32 num_points;
@ -46,6 +45,20 @@ 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_prototype prototype;
};
struct collider_closest_points_result collider_closest_points(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1);
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 cloud(struct arena *arena, struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1);

3
src/entity.h
View File

@ -113,6 +113,9 @@ struct collision_debug {
struct contact_point points[2];
u32 num_points;
struct v2 closest0;
struct v2 closest1;
struct xform xf0;
struct xform xf1;
};

65
src/game.c
View File

@ -650,6 +650,7 @@ INTERNAL void create_contacts(void)
struct string fdkey = STRING_FROM_BUFFER(BUFFER_FROM_STRUCT(&lookup_hash));
struct entity_handle *dbg_ent_handle = fixed_dict_get(&G.collision_debug_lookup.dict, fdkey);
if (!dbg_ent_handle) {
/* FIXME: Handle never released */
dbg_ent_handle = arena_push_zero(&G.collision_debug_lookup.arena, struct entity_handle);
}
@ -689,6 +690,13 @@ INTERNAL void create_contacts(void)
dbg->xf0 = e0_xf;
dbg->xf1 = e1_xf;
/* Update closest points */
{
struct collider_closest_points_result closest_points_res = collider_closest_points(&e0_collider, &e1_collider, e0_xf, e1_xf);
dbg->closest0 = closest_points_res.p0;
dbg->closest1 = closest_points_res.p1;
}
}
#endif
}
@ -1450,17 +1458,70 @@ INTERNAL void integrate_positions_from_velocities(f32 dt)
/* ========================== *
* TESTING TOI
* ========================== */
#if 0
/* Takes 2 shapes and their xforms at t=0 and t=1.
* Returns time of impact in range [0, 1]. */
INTERNAL f32 toi(struct collider_shape c0, struct collider_shape c1,
struct xform xf0_t0, struct xform xf1_t0,
struct xform xf0_t1, struct xform xf1_t1,
f32 tolerance)
{
/* Find direction p0 -> p1 at t=0 */
struct v2 dir;
struct v2 dir_neg;
{
struct collider_closest_points_result closest_points_res = collider_closest_points(&c0, &c1, xf0_t0, xf1_t0);
if (closest_points_res.colliding) {
/* Shapes are penetrating at t=0 */
return 0;
}
dir = v2_norm(v2_sub(closest_points_res.p1, closest_points_res.p0));
dir_neg = v2_neg(dir);
}
/* Safety check that shapes penetrate at t=1 */
f32 sep;
{
struct v2 p0 = collider_support_point(&c0, xf0_t1, dir);
struct v2 p1 = collider_support_point(&c1, xf1_t1, dir_neg);
sep = v2_dot(dir, v2_sub(p1, p0));
if (sep > tolerance) {
/* Shapes are not penetrating at t=1 */
return 1;
}
}
/* Root finding (bisection) */
f32 t0 = 0.0;
f32 t1 = 1.0;
f32 t = 0.5f;
while (math_fabs(sep) > tolerance) {
struct xform xf0 = xform_lerp(xf0_t0, xf0_t1, t);
struct xform xf1 = xform_lerp(xf1_t0, xf1_t1, t);
struct v2 p0 = collider_support_point(&c0, xf0, dir);
struct v2 p1 = collider_support_point(&c1, xf1, dir_neg);
sep = v2_dot(dir, v2_sub(p1, p0));
/* Update bracket */
if (sep > 0) {
t0 = t;
} else {
t1 = t;
}
t = (t1 - t0) / 2.0;
}
return t;
}
#endif

21
src/user.c
View File

@ -1188,7 +1188,7 @@ INTERNAL void user_update(void)
u32 color_third = RGBA_32_F(0, 0, 1, 0.75);
struct collider_simplex simplex = collider_res.simplex;
struct v2 simplex_points[] = { simplex.a, simplex.b, simplex.c };
struct v2 simplex_points[] = { simplex.a.p, simplex.b.p, simplex.c.p };
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 };
@ -1299,22 +1299,31 @@ INTERNAL void user_update(void)
u32 color_line = RGBA_32_F(1, 0, 1, 0.5);
u32 color_a = RGBA_32_F(1, 0, 0, 0.5);
u32 color_b = RGBA_32_F(0, 1, 0, 0.5);
struct collider_collision_points_result res = collider_res;
{
struct v2 a = xform_mul_v2(G.world_view, res.a0);
struct v2 b = xform_mul_v2(G.world_view, res.b0);
struct v2 a = xform_mul_v2(G.world_view, collider_res.a0);
struct v2 b = xform_mul_v2(G.world_view, collider_res.b0);
draw_solid_line(G.viewport_canvas, a, b, thickness, color_line);
draw_solid_circle(G.viewport_canvas, a, radius, color_a, 10);
draw_solid_circle(G.viewport_canvas, b, radius, color_b, 10);
}
{
struct v2 a = xform_mul_v2(G.world_view, res.a1);
struct v2 b = xform_mul_v2(G.world_view, res.b1);
struct v2 a = xform_mul_v2(G.world_view, collider_res.a1);
struct v2 b = xform_mul_v2(G.world_view, collider_res.b1);
draw_solid_line(G.viewport_canvas, a, b, thickness, color_line);
draw_solid_circle(G.viewport_canvas, a, radius, color_a, 10);
draw_solid_circle(G.viewport_canvas, b, radius, color_b, 10);
}
}
/* Draw closest points */
{
f32 radius = 4;
u32 color = RGBA_32_F(1, 1, 0, 0.5);
struct v2 a = xform_mul_v2(G.world_view, data->closest0);
struct v2 b = xform_mul_v2(G.world_view, data->closest1);
draw_solid_circle(G.viewport_canvas, a, radius, color, 10);
draw_solid_circle(G.viewport_canvas, b, radius, color, 10);
}
#endif
}
#endif