928 lines
33 KiB
C
928 lines
33 KiB
C
#include "gjk.h"
|
|
#include "math.h"
|
|
#include "arena.h"
|
|
#include "scratch.h"
|
|
|
|
#if GJK_DEBUG
|
|
u32 gjk_debug_steps = U32_MAX;
|
|
#endif
|
|
|
|
#define DBGSTEP if (dbg_step++ >= gjk_debug_steps) goto abort
|
|
|
|
INTERNAL struct v2 poly_support(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 struct v2 menkowski_point(struct v2_array poly0, struct v2_array shape1, struct v2 dir)
|
|
{
|
|
return v2_sub(poly_support(poly0, dir), poly_support(shape1, v2_neg(dir)));
|
|
}
|
|
|
|
b32 gjk_boolean(struct v2_array shape0, struct v2_array shape1)
|
|
{
|
|
struct { struct v2 a, b, c; } s = { 0 };
|
|
|
|
/* 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]));
|
|
|
|
/* Second point is support point towards origin */
|
|
struct v2 dir = v2_neg(s.a);
|
|
struct v2 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;
|
|
|
|
dir = v2_perp_towards_dir(v2_sub(s.b, s.a), v2_neg(v2_sub(s.c, s.a))); /* Normal of ab pointing away from c */
|
|
struct v2 a_to_origin = v2_neg(s.a);
|
|
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(v2_sub(s.c, s.a), v2_neg(v2_sub(s.b, s.a))); /* 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;
|
|
}
|
|
|
|
INTERNAL struct gjk_menkowski_point menkowski_point_extended(struct v2_array poly0, struct v2_array poly1, struct v2 dir)
|
|
{
|
|
struct gjk_menkowski_point res;
|
|
res.p0 = poly_support(poly0, dir);
|
|
res.p1 = poly_support(poly1, v2_neg(dir));
|
|
res.p = v2_sub(res.p0, res.p1);
|
|
return res;
|
|
}
|
|
|
|
struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1)
|
|
{
|
|
struct temp_arena scratch = scratch_begin_no_conflict();
|
|
struct gjk_extended_result res = { 0 };
|
|
|
|
/* TODO: Verify epsilon */
|
|
f32 epsilon = 0.0000100;
|
|
struct gjk_simplex s = { 0 };
|
|
b32 colliding = false;
|
|
struct v2 shape0_p = { 0 };
|
|
struct v2 shape1_p = { 0 };
|
|
struct gjk_menkowski_point *proto = NULL;
|
|
u32 proto_count = 0;
|
|
|
|
#if GJK_DEBUG
|
|
u32 dbg_step = 0;
|
|
#endif
|
|
|
|
/* ========================== *
|
|
* GJK collision check
|
|
* Construct encapsulating simplex OR closest feature if not colliding
|
|
* ========================== */
|
|
|
|
struct v2 dir = { 0 };
|
|
struct gjk_menkowski_point m = { 0 };
|
|
/* Determine encapsulating simplex if colliding, or closest edge / point to origin on simplex */
|
|
{
|
|
/* First point is support point in shape's general directions to eachother */
|
|
s.a = menkowski_point_extended(shape0, shape1, v2_sub(shape1.points[0], shape0.points[0]));
|
|
s.len = 1;
|
|
|
|
while (!colliding) {
|
|
if (s.len == 1) {
|
|
/* Second point is support point towards origin */
|
|
dir = v2_neg(s.a.p);
|
|
|
|
DBGSTEP;
|
|
m = menkowski_point_extended(shape0, shape1, dir);
|
|
if (v2_eq(m.p, s.a.p)) {
|
|
/* Point is the same */
|
|
break;
|
|
}
|
|
s.b = s.a;
|
|
s.a = m;
|
|
s.len = 2;
|
|
if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) < epsilon) {
|
|
/* New ab lies on origin */
|
|
break;
|
|
}
|
|
|
|
/* Third point is support point in direction of line normal towards origin */
|
|
dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p));
|
|
}
|
|
|
|
{
|
|
DBGSTEP;
|
|
m = menkowski_point_extended(shape0, shape1, dir);
|
|
if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_sub(m.p, s.a.p))) < epsilon) {
|
|
/* New point is on existing line ab */
|
|
break;
|
|
}
|
|
s.c = s.b;
|
|
s.b = s.a;
|
|
s.a = m;
|
|
s.len = 3;
|
|
if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) < epsilon) {
|
|
/* New ab lies on origin */
|
|
s.len = 2;
|
|
break;
|
|
} else if (math_fabs(v2_wedge(v2_sub(s.c.p, s.a.p), v2_neg(s.a.p))) < epsilon) {
|
|
/* New ac lies on origin */
|
|
s.b = s.c;
|
|
s.len = 2;
|
|
break;
|
|
}
|
|
}
|
|
|
|
/* Determine voronoi region of the simplex in which the origin lies */
|
|
i32 voronoi_mask = 0;
|
|
struct v2 vab = v2_sub(s.b.p, s.a.p);
|
|
struct v2 vac = v2_sub(s.c.p, s.a.p);
|
|
struct v2 vbc = v2_sub(s.c.p, s.b.p);
|
|
struct v2 rab_dir = v2_perp_towards_dir(vab, v2_neg(vac));
|
|
struct v2 rac_dir = v2_perp_towards_dir(vac, v2_neg(vab));
|
|
struct v2 rbc_dir = v2_perp_towards_dir(vbc, vab);
|
|
voronoi_mask |= (v2_dot(rab_dir, v2_neg(s.a.p)) >= 0) << 0; /* Regions ab, a, and b*/
|
|
voronoi_mask |= (v2_dot(rac_dir, v2_neg(s.a.p)) >= 0) << 1; /* Regions ac, a, and c */
|
|
voronoi_mask |= (v2_dot(rbc_dir, v2_neg(s.b.p)) >= 0) << 2; /* Regions bc, b, and c */
|
|
/* Remove point or edge and determine next direction based on voronoi region */
|
|
switch (voronoi_mask) {
|
|
case 0: { /* No region, must be in simplex */
|
|
colliding = true;
|
|
} break;
|
|
case 1: { /* Region ab, remove c */
|
|
dir = rab_dir; /* Next third point is in direction of region ab */
|
|
s.len = 2;
|
|
} break;
|
|
case 2: { /* Region ac, remove b */
|
|
dir = rac_dir; /* Next third point is in direction of region ac */
|
|
s.b = s.c;
|
|
s.len = 2;
|
|
} break;
|
|
case 4: { /* Region bc, remove a */
|
|
dir = rbc_dir; /* Next third point is in direction of region bc */
|
|
s.a = s.b;
|
|
s.b = s.c;
|
|
s.len = 2;
|
|
} break;
|
|
case 3: { /* Region a, remove bc */
|
|
s.len = 1;
|
|
} break;
|
|
case 5: { /* Region b, remove ac */
|
|
s.a = s.b;
|
|
s.len = 1;
|
|
} break;
|
|
case 6: { /* Region c, remove ab */
|
|
s.a = s.c;
|
|
s.len = 1;
|
|
} break;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (colliding) {
|
|
/* ========================== *
|
|
* Epa
|
|
* ========================== */
|
|
proto = arena_dry_push(scratch.arena, struct gjk_menkowski_point);
|
|
proto_count = 0;
|
|
{
|
|
ASSERT(s.len == 3);
|
|
struct gjk_menkowski_point *tmp = arena_push_array(scratch.arena, struct gjk_menkowski_point, 3);
|
|
tmp[0] = s.a;
|
|
tmp[1] = s.b;
|
|
tmp[2] = s.c;
|
|
proto_count = 3;
|
|
}
|
|
|
|
/* ========================== *
|
|
* Expand simplex towards closest edge to origin inside menkowski
|
|
* ========================== */
|
|
|
|
while (true) {
|
|
f32 pen_len_sq = F32_INFINITY;
|
|
|
|
/* Find dir from origin to closest edge */
|
|
/* FIXME: Winding order of ps & pe index */
|
|
u32 pen_ps_index = 0;
|
|
u32 pen_pe_index = 0;
|
|
for (u32 i = 0; i < proto_count; ++i) {
|
|
u32 ps_index = i;
|
|
u32 pe_index = (i < proto_count - 1) ? (i + 1) : 0;
|
|
struct v2 ps = proto[ps_index].p;
|
|
struct v2 pe = proto[pe_index].p;
|
|
|
|
struct v2 vse = v2_sub(pe, ps);
|
|
struct v2 vso = v2_neg(ps);
|
|
|
|
f32 d1 = v2_dot(vso, vse);
|
|
f32 d2 = v2_dot(vse, vse);
|
|
struct v2 vsd = v2_mul(vse, (d1 / d2));
|
|
struct v2 pd = v2_add(ps, vsd);
|
|
|
|
f32 pd_len_sq = v2_len_sq(pd);
|
|
if (pd_len_sq < pen_len_sq) {
|
|
pen_ps_index = ps_index;
|
|
pen_pe_index = pe_index;
|
|
pen_len_sq = pd_len_sq;
|
|
dir = pd;
|
|
}
|
|
}
|
|
|
|
/* TODO: Move to break (debugging) */
|
|
s.a = proto[pen_ps_index];
|
|
s.b = proto[pen_pe_index];
|
|
s.len = 2;
|
|
|
|
/* Find new point in dir */
|
|
m = menkowski_point_extended(shape0, shape1, dir);
|
|
|
|
/* Check unique */
|
|
/* TODO: Better */
|
|
DBGSTEP;
|
|
{
|
|
b32 unique = true;
|
|
for (u32 i = 0; i < proto_count; ++i) {
|
|
struct v2 edge_start = proto[i].p;
|
|
struct v2 edge_end = i < proto_count - 1 ? proto[i + 1].p : proto[0].p;
|
|
if (math_fabs(v2_wedge(v2_sub(edge_end, edge_start), v2_sub(m.p, edge_start))) < epsilon) {
|
|
unique = false;
|
|
break;
|
|
}
|
|
}
|
|
if (!unique) {
|
|
break;
|
|
}
|
|
}
|
|
|
|
/* Insert point into prototype */
|
|
/* FIXME: Preserve winding order */
|
|
arena_push(scratch.arena, struct gjk_menkowski_point);
|
|
++proto_count;
|
|
for (u32 i = proto_count - 1; i > pen_pe_index; --i) {
|
|
u32 shift_from = (i > 0) ? i - 1 : proto_count - 1;
|
|
u32 shift_to = i;
|
|
proto[shift_to] = proto[shift_from];
|
|
}
|
|
proto[pen_pe_index] = m;
|
|
}
|
|
}
|
|
|
|
/* Resolve points */
|
|
if (s.len == 1) {
|
|
shape0_p = s.a.p0;
|
|
shape1_p = s.a.p1;
|
|
} else if (s.len == 2) {
|
|
/* FIXME: Winding order dependent? */
|
|
ASSERT(s.len == 2);
|
|
f32 ratio;
|
|
{
|
|
/* Determine ratio between edge a & b that projected origin lies */
|
|
struct v2 vab = v2_sub(s.b.p, s.a.p);
|
|
struct v2 vao = v2_neg(s.a.p);
|
|
ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1);
|
|
}
|
|
/* Shape 0 */
|
|
shape0_p = v2_sub(s.b.p0, s.a.p0);
|
|
shape0_p = v2_mul(shape0_p, ratio);
|
|
shape0_p = v2_add(shape0_p, s.a.p0);
|
|
/* Shape 1 */
|
|
shape1_p = v2_sub(s.b.p1, s.a.p1);
|
|
shape1_p = v2_mul(shape1_p, ratio);
|
|
shape1_p = v2_add(shape1_p, s.a.p1);
|
|
}
|
|
|
|
res.solved = true;
|
|
abort:
|
|
if (proto_count > 0) {
|
|
for (u32 i = 0; i < min_u32(proto_count, ARRAY_COUNT(res.prototype.points)); ++i) {
|
|
res.prototype.points[i] = proto[i].p;
|
|
}
|
|
res.prototype.len = proto_count;
|
|
} else {
|
|
if (s.len >= 1) {
|
|
res.prototype.points[0] = s.a.p;
|
|
if (s.len >= 2) {
|
|
res.prototype.points[1] = s.b.p;
|
|
if (s.len >= 3) {
|
|
res.prototype.points[2] = s.c.p;
|
|
}
|
|
}
|
|
}
|
|
res.prototype.len = s.len;
|
|
}
|
|
res.colliding = colliding;
|
|
res.p0 = shape0_p;
|
|
res.p1 = shape1_p;
|
|
res.simplex = s;
|
|
scratch_end(scratch);
|
|
return res;
|
|
}
|
|
|
|
/* 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_extended(poly0, poly1, dir).p;
|
|
if (res.count == 0 || !v2_eq(p, res.points[res.count - 1])) {
|
|
*arena_push(arena, struct v2) = p;
|
|
++res.count;
|
|
}
|
|
}
|
|
return res;
|
|
}
|
|
|
|
/* TODO: Remove this (debugging) */
|
|
struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_array poly1)
|
|
{
|
|
struct v2_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;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/* ========================== *
|
|
* Swept GJK (unused)
|
|
* ========================== */
|
|
|
|
#if 0
|
|
|
|
struct gjk_swept_result {
|
|
b32 colliding;
|
|
struct v2 p0, p1; /* Closest points on each shape */
|
|
|
|
/* For debugging */
|
|
struct gjk_simplex simplex;
|
|
b32 velocity_intersects;
|
|
b32 solved;
|
|
struct gjk_prototype prototype;
|
|
};
|
|
|
|
struct poly_support_swept_result {
|
|
struct v2 p;
|
|
struct v2 original;
|
|
};
|
|
|
|
INTERNAL struct poly_support_swept_result poly_support_swept(struct v2_array a, struct v2 dir, struct v2 linear_velocity)
|
|
{
|
|
/* TODO: Could probably binary search for largest dot since shape is convex */
|
|
struct v2 furthest = V2(0, 0);
|
|
struct v2 furthest_original = V2(0, 0);
|
|
f32 furthest_dot = -F32_INFINITY;
|
|
|
|
for (u32 i = 0; i < a.count; ++i) {
|
|
struct v2 p = a.points[i];
|
|
f32 dot = v2_dot(dir, p);
|
|
if (dot > furthest_dot) {
|
|
furthest = p;
|
|
furthest_original = p;
|
|
furthest_dot = dot;
|
|
}
|
|
}
|
|
|
|
for (u32 i = 0; i < a.count; ++i) {
|
|
struct v2 p = a.points[i];
|
|
struct v2 modified;
|
|
#if 0
|
|
if (v2_dot(linear_velocity, dir) > 0) {
|
|
modified = v2_add(p, linear_velocity);
|
|
} else {
|
|
modified = p;
|
|
}
|
|
#else
|
|
modified = v2_add(p, linear_velocity);
|
|
#endif
|
|
f32 dot = v2_dot(dir, modified);
|
|
if (dot > furthest_dot) {
|
|
furthest = modified;
|
|
furthest_original = p;
|
|
furthest_dot = dot;
|
|
}
|
|
}
|
|
|
|
struct poly_support_swept_result res = { 0 };
|
|
res.p = furthest;
|
|
res.original = furthest_original;
|
|
return res;
|
|
}
|
|
|
|
INTERNAL struct gjk_menkowski_point menkowski_point_extended_swept(struct v2_array poly0, struct v2_array poly1, struct v2 dir, struct v2 linear_velocity)
|
|
{
|
|
struct gjk_menkowski_point res;
|
|
struct poly_support_swept_result res0 = poly_support_swept(poly0, dir, linear_velocity);
|
|
struct poly_support_swept_result res1 = poly_support_swept(poly1, v2_neg(dir), V2(0, 0));
|
|
res.p0 = res0.original;
|
|
res.p1 = res1.original;
|
|
res.p = v2_sub(res0.p, res1.p);
|
|
return res;
|
|
}
|
|
|
|
struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1, struct v2 linear_velocity)
|
|
{
|
|
struct temp_arena scratch = scratch_begin_no_conflict();
|
|
struct gjk_extended_result res = { 0 };
|
|
|
|
/* FIXME: Divs by 0 */
|
|
|
|
/* TODO: Verify epsilon */
|
|
f32 epsilon = 0.00001;
|
|
struct gjk_simplex s = {
|
|
.len = 3,
|
|
.a = V2(F32_NAN, F32_NAN),
|
|
.b = V2(F32_NAN, F32_NAN),
|
|
.c = V2(F32_NAN, F32_NAN)
|
|
};
|
|
b32 colliding = false;
|
|
struct v2 shape0_p = { 0 };
|
|
struct v2 shape1_p = { 0 };
|
|
b32 velocity_intersects = false;
|
|
f32 velocity_intersection = 0;
|
|
|
|
/* TODO: Move this back down */
|
|
struct gjk_menkowski_point *proto = arena_dry_push(scratch.arena, struct gjk_menkowski_point);
|
|
u32 proto_count = 0;
|
|
|
|
#if GJK_DEBUG
|
|
u32 dbg_step = 0;
|
|
#endif
|
|
|
|
/* ========================== *
|
|
* GJK collision check
|
|
* Construct encapsulating simplex OR closest feature if not colliding
|
|
* ========================== */
|
|
|
|
struct v2 dir = { 0 };
|
|
struct gjk_menkowski_point m = { 0 };
|
|
/* Determine encapsulating simplex if colliding, or closest edge / point to origin on simplex */
|
|
{
|
|
/* First point is support point in shape's general directions to eachother */
|
|
s.a = menkowski_point_extended_swept(shape0, shape1, v2_sub(shape1.points[0], shape0.points[0]), linear_velocity);
|
|
s.len = 1;
|
|
|
|
while (!colliding) {
|
|
if (s.len == 1) {
|
|
/* Second point is support point towards origin */
|
|
dir = v2_neg(s.a.p);
|
|
|
|
DBGSTEP;
|
|
m = menkowski_point_extended_swept(shape0, shape1, dir, linear_velocity);
|
|
if (v2_eq(m.p, s.a.p)) {
|
|
/* Point is the same */
|
|
break;
|
|
}
|
|
s.b = s.a;
|
|
s.a = m;
|
|
s.len = 2;
|
|
if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) < epsilon) {
|
|
/* New ab lies on origin */
|
|
break;
|
|
}
|
|
/* Third point is support point in direction of line normal towards origin */
|
|
dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p));
|
|
}
|
|
|
|
{
|
|
DBGSTEP;
|
|
m = menkowski_point_extended_swept(shape0, shape1, dir, linear_velocity);
|
|
if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_sub(m.p, s.a.p))) < epsilon) {
|
|
/* New point is on existing line ab */
|
|
break;
|
|
}
|
|
if (v2_eq(m.p, s.a.p) || v2_eq(m.p, s.b.p) || v2_eq(m.p, s.c.p)) {
|
|
/* New point is existing c */
|
|
s.b = s.a;
|
|
s.a = m;
|
|
s.len = 2;
|
|
break;
|
|
}
|
|
s.c = s.b;
|
|
s.b = s.a;
|
|
s.a = m;
|
|
s.len = 3;
|
|
if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) < epsilon) {
|
|
/* New ab lies on origin */
|
|
s.len = 2;
|
|
break;
|
|
}
|
|
if (math_fabs(v2_wedge(v2_sub(s.c.p, s.a.p), v2_neg(s.a.p))) < epsilon) {
|
|
/* New ac lies on origin */
|
|
s.b = s.c;
|
|
s.len = 2;
|
|
break;
|
|
}
|
|
}
|
|
|
|
DBGSTEP;
|
|
/* Determine voronoi region of the simplex in which the origin lies */
|
|
i32 voronoi_mask = 0;
|
|
struct v2 vab = v2_sub(s.b.p, s.a.p);
|
|
struct v2 vac = v2_sub(s.c.p, s.a.p);
|
|
struct v2 vbc = v2_sub(s.c.p, s.b.p);
|
|
struct v2 rab_dir = v2_perp_towards_dir(vab, v2_neg(vac));
|
|
struct v2 rac_dir = v2_perp_towards_dir(vac, v2_neg(vab));
|
|
struct v2 rbc_dir = v2_perp_towards_dir(vbc, vab);
|
|
voronoi_mask |= (v2_dot(rab_dir, v2_neg(s.a.p)) >= 0) << 0; /* Regions ab, a, and b*/
|
|
voronoi_mask |= (v2_dot(rac_dir, v2_neg(s.a.p)) >= 0) << 1; /* Regions ac, a, and c */
|
|
voronoi_mask |= (v2_dot(rbc_dir, v2_neg(s.b.p)) >= 0) << 2; /* Regions bc, b, and c */
|
|
/* Remove point or edge and determine next direction based on voronoi region */
|
|
switch (voronoi_mask) {
|
|
default: { /* No region, must be in simplex */
|
|
colliding = true;
|
|
} break;
|
|
case 1: { /* Region ab, remove c */
|
|
dir = rab_dir; /* Next third point is in direction of region ab */
|
|
s.len = 2;
|
|
} break;
|
|
case 2: { /* Region ac, remove b */
|
|
dir = rac_dir; /* Next third point is in direction of region ac */
|
|
s.b = s.c;
|
|
s.len = 2;
|
|
} break;
|
|
case 4: { /* Region bc, remove a */
|
|
dir = rbc_dir; /* Next third point is in direction of region bc */
|
|
s.a = s.b;
|
|
s.b = s.c;
|
|
s.len = 2;
|
|
} break;
|
|
case 3: { /* Region a, remove bc */
|
|
s.len = 1;
|
|
} break;
|
|
case 5: { /* Region b, remove ac */
|
|
s.a = s.b;
|
|
s.len = 1;
|
|
} break;
|
|
case 6: { /* Region c, remove ab */
|
|
s.a = s.c;
|
|
s.len = 1;
|
|
} break;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (colliding) {
|
|
/* ========================== *
|
|
* Epa
|
|
* ========================== */
|
|
proto = arena_dry_push(scratch.arena, struct gjk_menkowski_point);
|
|
proto_count = 0;
|
|
{
|
|
ASSERT(s.len == 3);
|
|
struct gjk_menkowski_point *tmp = arena_push_array(scratch.arena, struct gjk_menkowski_point, 3);
|
|
tmp[0] = s.a;
|
|
tmp[1] = s.b;
|
|
tmp[2] = s.c;
|
|
proto_count = 3;
|
|
}
|
|
|
|
if (v2_eq(linear_velocity, V2(0, 0))) {
|
|
/* ========================== *
|
|
* Expand simplex towards closest edge to origin inside menkowski
|
|
* ========================== */
|
|
|
|
while (true) {
|
|
DBGSTEP;
|
|
f32 pen_len_sq = F32_INFINITY;
|
|
|
|
/* Find dir from origin to closest edge */
|
|
/* FIXME: Winding order of ps & pe index */
|
|
u32 pen_ps_index = 0;
|
|
u32 pen_pe_index = 0;
|
|
for (u32 i = 0; i < proto_count; ++i) {
|
|
u32 ps_index = i;
|
|
u32 pe_index = (i < proto_count - 1) ? (i + 1) : 0;
|
|
struct v2 ps = proto[ps_index].p;
|
|
struct v2 pe = proto[pe_index].p;
|
|
|
|
struct v2 vse = v2_sub(pe, ps);
|
|
struct v2 vso = v2_neg(ps);
|
|
|
|
f32 d1 = v2_dot(vso, vse);
|
|
f32 d2 = v2_dot(vse, vse);
|
|
struct v2 vsd = v2_mul(vse, (d1 / d2));
|
|
struct v2 pd = v2_add(ps, vsd);
|
|
|
|
f32 pd_len_sq = v2_len_sq(pd);
|
|
if (pd_len_sq < pen_len_sq) {
|
|
pen_ps_index = ps_index;
|
|
pen_pe_index = pe_index;
|
|
pen_len_sq = pd_len_sq;
|
|
dir = pd;
|
|
}
|
|
}
|
|
|
|
/* TODO: Move to break (debugging) */
|
|
s.a = proto[pen_ps_index];
|
|
s.b = proto[pen_pe_index];
|
|
s.len = 2;
|
|
|
|
/* Find new point in dir */
|
|
m = menkowski_point_extended_swept(shape0, shape1, dir, linear_velocity);
|
|
|
|
/* Check unique */
|
|
/* TODO: Better */
|
|
{
|
|
b32 unique = true;
|
|
for (u32 i = 0; i < proto_count; ++i) {
|
|
struct v2 edge_start = proto[i].p;
|
|
struct v2 edge_end = i < proto_count - 1 ? proto[i + 1].p : proto[0].p;
|
|
if (math_fabs(v2_wedge(v2_sub(edge_end, edge_start), v2_sub(m.p, edge_start))) < epsilon) {
|
|
unique = false;
|
|
break;
|
|
}
|
|
}
|
|
if (!unique) {
|
|
break;
|
|
}
|
|
}
|
|
|
|
/* Insert point into prototype */
|
|
/* FIXME: Preserve winding order */
|
|
arena_push(scratch.arena, struct gjk_menkowski_point);
|
|
++proto_count;
|
|
for (u32 i = proto_count - 1; i > pen_pe_index; --i) {
|
|
u32 shift_from = (i > 0) ? i - 1 : proto_count - 1;
|
|
u32 shift_to = i;
|
|
proto[shift_to] = proto[shift_from];
|
|
}
|
|
proto[pen_pe_index] = m;
|
|
}
|
|
} else {
|
|
/* ========================== *
|
|
* Expand simplex towards furthest edge along velocity
|
|
* ========================== */
|
|
|
|
while (true) {
|
|
DBGSTEP;
|
|
|
|
/* FIXME: Winding order of ps & pe index */
|
|
f32 highest_score = -F32_INFINITY;
|
|
u32 picked_start_index = 0;
|
|
u32 picked_end_index = 0;
|
|
b32 picked_intersects = false;
|
|
f32 picked_intersection = 0;
|
|
for (u32 i = 0; i < proto_count; ++i) {
|
|
u32 ps_index = i;
|
|
u32 pe_index = (i < proto_count - 1) ? (i + 1) : 0;
|
|
struct v2 ps = proto[ps_index].p;
|
|
struct v2 pe = proto[pe_index].p;
|
|
|
|
struct v2 vse = v2_sub(pe, ps);
|
|
struct v2 vso = v2_neg(ps);
|
|
|
|
f32 w = v2_wedge(vse, linear_velocity);
|
|
f32 t1 = 0;
|
|
f32 t2 = 0;
|
|
b32 intersects = false;
|
|
if (w != 0) {
|
|
w = 1 / w;
|
|
t1 = v2_wedge(vso, linear_velocity) * w;
|
|
t2 = v2_wedge(vso, vse) * w;
|
|
intersects = -epsilon < t1 && t1 < 1 + epsilon && -epsilon < t2 && t2 < 1 + epsilon;
|
|
}
|
|
|
|
struct v2 next_dir = dir;
|
|
f32 score = highest_score;
|
|
if (intersects) {
|
|
/* Score is intersection (+1 to ensure intersections get priority) */
|
|
score = 1 + t1;
|
|
next_dir = v2_perp_towards_dir(vse, linear_velocity);
|
|
} else {
|
|
/* Score is inverse projection len */
|
|
struct v2 vsv = v2_sub(linear_velocity, ps);
|
|
struct v2 vsd = v2_mul(vse, v2_dot(vse, vsv) / v2_dot(vse, vse));
|
|
f32 len = v2_len(v2_sub(vsd, vsv));
|
|
score = 1.0 - len;
|
|
next_dir = v2_perp_towards_dir(vse, ps);
|
|
}
|
|
|
|
if (score > highest_score) {
|
|
picked_start_index = ps_index;
|
|
picked_end_index = pe_index;
|
|
highest_score = score;
|
|
picked_intersects = intersects;
|
|
picked_intersection = t1;
|
|
dir = next_dir;
|
|
}
|
|
}
|
|
|
|
/* TODO: Move to break (debugging) */
|
|
velocity_intersects = picked_intersects;
|
|
velocity_intersection = picked_intersection;
|
|
s.a = proto[picked_start_index];
|
|
s.b = proto[picked_end_index];
|
|
s.len = 2;
|
|
|
|
/* Find new point in dir */
|
|
m = menkowski_point_extended_swept(shape0, shape1, dir, linear_velocity);
|
|
|
|
/* Check unique */
|
|
/* TODO: Better */
|
|
/* FIXME: Winding order */
|
|
u32 insert_start_index = 0;
|
|
u32 insert_end_index = 1;
|
|
{
|
|
b32 unique = true;
|
|
for (u32 i = 0; i < proto_count; ++i) {
|
|
u32 start_index = i;
|
|
u32 end_index = i < proto_count - 1 ? i + 1 : 0;
|
|
struct v2 edge_start = proto[start_index].p;
|
|
struct v2 edge_end = proto[end_index].p;
|
|
|
|
struct v2 vso = v2_neg(edge_start);
|
|
struct v2 vse = v2_sub(edge_end, edge_start);
|
|
struct v2 vsm = v2_sub(m.p, edge_start);
|
|
|
|
f32 w = v2_wedge(vse, vsm);
|
|
if (math_fabs(w) > epsilon) {
|
|
w = v2_wedge(vse, m.p);
|
|
if (w != 0) {
|
|
w = 1 / w;
|
|
f32 t1 = v2_wedge(vso, m.p) * w;
|
|
f32 t2 = v2_wedge(vso, vse) * w;
|
|
b32 intersects = 0 <= t1 && t1 <= 1 && 0 <= t2 && t2 <= 1;
|
|
if (intersects) {
|
|
insert_start_index = start_index;
|
|
insert_end_index = end_index;
|
|
}
|
|
}
|
|
} else {
|
|
unique = false;
|
|
break;
|
|
}
|
|
}
|
|
if (!unique) {
|
|
break;
|
|
}
|
|
}
|
|
|
|
(UNUSED)insert_start_index;
|
|
DBGSTEP;
|
|
/* Insert point into prototype */
|
|
/* FIXME: Preserve winding order */
|
|
arena_push(scratch.arena, struct v2);
|
|
++proto_count;
|
|
for (u32 i = proto_count - 1; i > insert_end_index; --i) {
|
|
u32 shift_from = (i > 0) ? i - 1 : proto_count - 1;
|
|
u32 shift_to = i;
|
|
proto[shift_to] = proto[shift_from];
|
|
}
|
|
proto[insert_end_index] = m;
|
|
}
|
|
}
|
|
}
|
|
|
|
/* Resolve points */
|
|
if (s.len == 1) {
|
|
ASSERT(!colliding); /* Is this possible? Would make calculating collision normal tricky. */
|
|
shape0_p = s.a.p0;
|
|
shape1_p = s.a.p1;
|
|
} else {
|
|
ASSERT(s.len == 2);
|
|
/* FIXME: Winding order dependent? */
|
|
f32 ratio;
|
|
if (colliding && !v2_eq(linear_velocity, V2(0, 0))) {
|
|
if (velocity_intersects) {
|
|
/* Ratio between edge a & b that velocity intersection lies */
|
|
ratio = velocity_intersection;
|
|
} else {
|
|
/* Ratio between edge a & b that projected velocity lies */
|
|
struct v2 vab = v2_sub(s.b.p, s.a.p);
|
|
struct v2 vap = v2_sub(linear_velocity, s.a.p);
|
|
ratio = clamp_f32(v2_dot(vab, vap) / v2_dot(vab, vab), 0, 1);
|
|
}
|
|
} else {
|
|
/* Determine ratio between edge a & b that projected origin lies */
|
|
struct v2 vab = v2_sub(s.b.p, s.a.p);
|
|
struct v2 vao = v2_neg(s.a.p);
|
|
ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1);
|
|
}
|
|
/* Shape 0 */
|
|
shape0_p = v2_sub(s.b.p0, s.a.p0);
|
|
shape0_p = v2_mul(shape0_p, ratio);
|
|
shape0_p = v2_add(shape0_p, s.a.p0);
|
|
/* Shape 1 */
|
|
shape1_p = v2_sub(s.b.p1, s.a.p1);
|
|
shape1_p = v2_mul(shape1_p, ratio);
|
|
shape1_p = v2_add(shape1_p, s.a.p1);
|
|
}
|
|
|
|
|
|
res.solved = true;
|
|
abort:
|
|
if (proto_count > 0) {
|
|
for (u32 i = 0; i < min_u32(proto_count, ARRAY_COUNT(res.prototype.points)); ++i) {
|
|
res.prototype.points[i] = proto[i].p;
|
|
}
|
|
res.prototype.len = proto_count;
|
|
} else {
|
|
if (s.len >= 1) {
|
|
res.prototype.points[0] = s.a.p;
|
|
if (s.len >= 2) {
|
|
res.prototype.points[1] = s.b.p;
|
|
if (s.len >= 3) {
|
|
res.prototype.points[2] = s.c.p;
|
|
}
|
|
}
|
|
}
|
|
res.prototype.len = s.len;
|
|
}
|
|
res.colliding = colliding;
|
|
res.p0 = shape0_p;
|
|
res.p1 = shape1_p;
|
|
res.simplex = s;
|
|
res.velocity_intersects = velocity_intersects;
|
|
scratch_end(scratch);
|
|
return res;
|
|
}
|
|
|
|
|
|
/* TODO: Remove this (debugging) */
|
|
struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1, struct v2 linear_velocity)
|
|
{
|
|
struct v2_array res = { .points = arena_dry_push(arena, struct v2) };
|
|
u64 rays = 500;
|
|
for (u64 i = 0; i < rays; ++i) {
|
|
f32 angle = ((f32)i / rays) * (2 * PI);
|
|
struct v2 dir = v2_from_angle(angle);
|
|
struct v2 p = menkowski_point_extended_swept(poly0, poly1, dir, linear_velocity).p;
|
|
if (res.count == 0 || !v2_eq(p, res.points[res.count - 1])) {
|
|
*arena_push(arena, struct v2) = p;
|
|
++res.count;
|
|
}
|
|
}
|
|
return res;
|
|
}
|
|
|
|
/* TODO: Remove this (debugging) */
|
|
struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_array poly1, struct v2 linear_velocity)
|
|
{
|
|
struct v2_array res = { .points = arena_dry_push(arena, struct v2) };
|
|
for (u64 i = 0; i < poly0.count; ++i) {
|
|
struct v2 p0 = poly0.points[i];
|
|
for (u64 j = 0; j < poly1.count; ++j) {
|
|
struct v2 p1 = poly1.points[j];
|
|
struct v2 diff = v2_sub(p0, p1);
|
|
{
|
|
*arena_push(arena, struct v2) = diff;
|
|
++res.count;
|
|
}
|
|
{
|
|
*arena_push(arena, struct v2) = v2_add(diff, linear_velocity);
|
|
++res.count;
|
|
}
|
|
}
|
|
}
|
|
|
|
return res;
|
|
}
|
|
|
|
#endif
|