join logic for gjk expansion into one function
This commit is contained in:
parent
beccc17b7f
commit
c3b96d1597
@ -94,7 +94,7 @@ struct entity {
|
|||||||
/* TODO: Remove this (testing) */
|
/* TODO: Remove this (testing) */
|
||||||
b32 colliding;
|
b32 colliding;
|
||||||
struct entity_handle colliding_with;
|
struct entity_handle colliding_with;
|
||||||
struct gjk_extended_simplex simplex;
|
struct gjk_simplex simplex;
|
||||||
struct v2 point;
|
struct v2 point;
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
17
src/game.c
17
src/game.c
@ -778,7 +778,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
|
|||||||
struct v2 point0 = V2(0, 0);
|
struct v2 point0 = V2(0, 0);
|
||||||
struct v2 point1 = V2(0, 0);
|
struct v2 point1 = V2(0, 0);
|
||||||
struct entity *colliding_with = entity_nil();
|
struct entity *colliding_with = entity_nil();
|
||||||
struct gjk_extended_simplex final_simplex = { 0 };
|
struct gjk_simplex final_simplex = { 0 };
|
||||||
for (u64 e1_index = 0; e1_index < store->reserved; ++e1_index) {
|
for (u64 e1_index = 0; e1_index < store->reserved; ++e1_index) {
|
||||||
struct entity *e1 = &store->entities[e1_index];
|
struct entity *e1 = &store->entities[e1_index];
|
||||||
if (e1 == e0) continue;
|
if (e1 == e0) continue;
|
||||||
@ -799,12 +799,25 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
|
|||||||
};
|
};
|
||||||
}
|
}
|
||||||
|
|
||||||
struct gjk_extended_result res = gjk_extended(e0_poly, e1_poly, G.gjk_steps);
|
#if 0
|
||||||
|
struct gjk_simplex res = gjk_extended(e0_poly, e1_poly, G.gjk_steps);
|
||||||
colliding = res.colliding;
|
colliding = res.colliding;
|
||||||
point0 = res.p0;
|
point0 = res.p0;
|
||||||
point1 = res.p1;
|
point1 = res.p1;
|
||||||
colliding_with = e1;
|
colliding_with = e1;
|
||||||
final_simplex = res.final_simplex;
|
final_simplex = res.final_simplex;
|
||||||
|
#else
|
||||||
|
struct v2 pendir = V2(0, 0);
|
||||||
|
//struct v2 pendir = V2(0, 99999);
|
||||||
|
|
||||||
|
struct gjk_extended_result res = gjk_extended(e0_poly, e1_poly, pendir);
|
||||||
|
|
||||||
|
colliding = res.colliding;
|
||||||
|
point0 = res.p0;
|
||||||
|
point1 = res.p1;
|
||||||
|
colliding_with = e1;
|
||||||
|
//final_simplex = res.simplex;
|
||||||
|
#endif
|
||||||
|
|
||||||
if (colliding) {
|
if (colliding) {
|
||||||
|
|
||||||
|
|||||||
@ -1050,7 +1050,7 @@ INTERNAL void user_update(void)
|
|||||||
u32 color_second = RGBA_32_F(0, 1, 0, 0.75);
|
u32 color_second = RGBA_32_F(0, 1, 0, 0.75);
|
||||||
u32 color_third = RGBA_32_F(0, 0, 1, 0.75);
|
u32 color_third = RGBA_32_F(0, 0, 1, 0.75);
|
||||||
|
|
||||||
struct gjk_extended_simplex simplex = ent->simplex;
|
struct gjk_simplex simplex = ent->simplex;
|
||||||
struct v2 simplex_points[] = { simplex.a.p, simplex.b.p, simplex.c.p };
|
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]);
|
for (u64 i = 0; i < ARRAY_COUNT(simplex_points); ++i) simplex_points[i] = xform_mul_v2(G.world_view, simplex_points[i]);
|
||||||
struct v2_array simplex_array = { .count = simplex.len, .points = simplex_points };
|
struct v2_array simplex_array = { .count = simplex.len, .points = simplex_points };
|
||||||
|
|||||||
686
src/util.c
686
src/util.c
@ -100,37 +100,28 @@ INTERNAL struct v2 poly_support(struct v2_array a, struct v2 dir)
|
|||||||
return furthest;
|
return furthest;
|
||||||
}
|
}
|
||||||
|
|
||||||
INTERNAL struct v2 menkowski_point(struct v2_array poly0, struct v2_array poly1, struct v2 dir)
|
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(poly1, v2_neg(dir)));
|
return v2_sub(poly_support(poly0, dir), poly_support(shape1, v2_neg(dir)));
|
||||||
}
|
}
|
||||||
|
|
||||||
INTERNAL struct gjk_menkowski_point extended_menkowski_point(struct v2_array poly0, struct v2_array poly1, struct v2 dir)
|
b32 gjk_boolean(struct v2_array shape0, struct v2_array shape1)
|
||||||
{
|
|
||||||
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;
|
|
||||||
}
|
|
||||||
|
|
||||||
b32 gjk_boolean(struct v2_array poly0, struct v2_array poly1)
|
|
||||||
{
|
{
|
||||||
struct { struct v2 a, b, c; } s = { 0 };
|
struct { struct v2 a, b, c; } s = { 0 };
|
||||||
|
|
||||||
/* First point is support point in shape's general directions to eachother */
|
/* First point is support point in shape's general directions to eachother */
|
||||||
s.a = menkowski_point(poly0, poly1, v2_sub(poly1.points[0], poly0.points[0]));
|
s.a = menkowski_point(shape0, shape1, v2_sub(shape1.points[0], shape0.points[0]));
|
||||||
|
|
||||||
/* Second point is support point towards origin */
|
/* Second point is support point towards origin */
|
||||||
struct v2 dir = v2_neg(s.a);
|
struct v2 dir = v2_neg(s.a);
|
||||||
struct v2 p = menkowski_point(poly0, poly1, dir);
|
struct v2 p = menkowski_point(shape0, shape1, dir);
|
||||||
if (v2_dot(dir, p) >= 0) {
|
if (v2_dot(dir, p) >= 0) {
|
||||||
s.b = s.a;
|
s.b = s.a;
|
||||||
s.a = p;
|
s.a = p;
|
||||||
while (true) {
|
while (true) {
|
||||||
/* Third point is support point in direction of line normal towards origin */
|
/* Third point is support point in direction of line normal towards origin */
|
||||||
dir = perp_towards_point(s.a, s.b, V2(0, 0));
|
dir = perp_towards_point(s.a, s.b, V2(0, 0));
|
||||||
p = menkowski_point(poly0, poly1, dir);
|
p = menkowski_point(shape0, shape1, dir);
|
||||||
if (v2_dot(dir, p) < 0) {
|
if (v2_dot(dir, p) < 0) {
|
||||||
/* New point did not cross origin, collision impossible */
|
/* New point did not cross origin, collision impossible */
|
||||||
break;
|
break;
|
||||||
@ -171,400 +162,343 @@ b32 gjk_boolean(struct v2_array poly0, struct v2_array poly1)
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
#if 0
|
|
||||||
|
|
||||||
struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array poly1, u32 max_steps)
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
INTERNAL struct gjk_menkowski_point menkowski_point_extended(struct v2_array poly0, struct v2_array poly1, struct v2 dir)
|
||||||
{
|
{
|
||||||
struct temp_arena scratch = scratch_begin_no_conflict();
|
struct gjk_menkowski_point res;
|
||||||
b32 colliding = false;
|
res.p0 = poly_support(poly0, dir);
|
||||||
struct v2 shape0_p = V2(0, 0);
|
res.p1 = poly_support(poly1, v2_neg(dir));
|
||||||
struct v2 shape1_p = V2(0, 0);
|
res.p = v2_sub(res.p0, res.p1);
|
||||||
u32 step = 0;
|
return res;
|
||||||
|
}
|
||||||
|
|
||||||
|
struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1, struct v2 penetration_dir)
|
||||||
|
{
|
||||||
|
struct gjk_extended_result res = { 0 };
|
||||||
|
|
||||||
/* TODO: Verify epsilon */
|
/* TODO: Verify epsilon */
|
||||||
f32 unique_epsilon = 0.00001;
|
f32 unique_epsilon = 0.00001;
|
||||||
|
b32 use_penetration_dir = false;
|
||||||
|
struct gjk_simplex s = { 0 };
|
||||||
|
|
||||||
/* Simplex */
|
/* ========================== *
|
||||||
struct gjk_extended_simplex s = { 0 };
|
* Collision check
|
||||||
|
* ========================== */
|
||||||
|
struct v2 dir = { 0 };
|
||||||
|
struct gjk_menkowski_point m = { 0 };
|
||||||
|
{
|
||||||
|
/* 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;
|
||||||
|
|
||||||
/* First point is support point towards shape centers */
|
|
||||||
if (step++ >= max_steps) goto abort;
|
|
||||||
struct v2 dir = v2_sub(poly1.points[0], poly0.points[0]);
|
|
||||||
struct gjk_menkowski_point m = extended_menkowski_point(poly0, poly1, dir);
|
|
||||||
s.a = m;
|
|
||||||
s.len = 1;
|
|
||||||
|
|
||||||
while (true) {
|
|
||||||
/* Second point is support point towards origin */
|
/* Second point is support point towards origin */
|
||||||
if (s.len == 1) {
|
dir = v2_neg(s.a.p);
|
||||||
if (step++ >= max_steps) goto abort;
|
m = menkowski_point_extended(shape0, shape1, dir);
|
||||||
m = extended_menkowski_point(poly0, poly1, v2_neg(s.a.p));
|
if (v2_dot(dir, m.p) >= 0) {
|
||||||
if (v2_eq(m.p, s.a.p)) {
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
s.b = s.a;
|
s.b = s.a;
|
||||||
s.a = m;
|
s.a = m;
|
||||||
s.len = 2;
|
s.len = 2;
|
||||||
|
|
||||||
/* Third point is support point in direction of line normal towards origin */
|
while (true) {
|
||||||
dir = perp_towards_point(s.a.p, s.b.p, V2(0, 0));
|
/* Third point is support point in direction of line normal towards origin */
|
||||||
}
|
dir = perp_towards_point(s.a.p, s.b.p, V2(0, 0));
|
||||||
|
m = menkowski_point_extended(shape0, shape1, dir);
|
||||||
if (step++ >= max_steps) goto abort;
|
if (v2_dot(dir, m.p) < 0) {
|
||||||
m = extended_menkowski_point(poly0, poly1, dir);
|
/* New point did not cross origin, collision impossible */
|
||||||
|
break;
|
||||||
if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_sub(m.p, s.a.p))) < unique_epsilon) {
|
|
||||||
/* New point is already on the current line */
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
s.c = s.b;
|
|
||||||
s.b = s.a;
|
|
||||||
s.a = m;
|
|
||||||
s.len = 3;
|
|
||||||
|
|
||||||
if (step++ >= max_steps) goto abort;
|
|
||||||
|
|
||||||
struct v2 rab_dir = v2_neg(perp_towards_point(s.a.p, s.b.p, s.c.p));
|
|
||||||
struct v2 rac_dir = v2_neg(perp_towards_point(s.a.p, s.c.p, s.b.p));
|
|
||||||
struct v2 rbc_dir = v2_neg(perp_towards_point(s.b.p, s.c.p, s.a.p));
|
|
||||||
|
|
||||||
b32 rab = v2_dot(rab_dir, v2_neg(s.a.p)) >= 0;
|
|
||||||
b32 rac = v2_dot(rac_dir, v2_neg(s.a.p)) >= 0;
|
|
||||||
b32 rbc = v2_dot(rbc_dir, v2_neg(s.b.p)) >= 0;
|
|
||||||
|
|
||||||
if (!rab && !rac && !rbc) {
|
|
||||||
colliding = true;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Remove point or edge and determine next direction based on vornoi regions */
|
|
||||||
b32 ra = rab && rac;
|
|
||||||
b32 rb = rab && rbc;
|
|
||||||
b32 rc = rac && rbc;
|
|
||||||
rab = rab && !ra && !rb;
|
|
||||||
rac = rac && !ra && !rc;
|
|
||||||
rbc = rbc && !rb && !rc;
|
|
||||||
if (rab) {
|
|
||||||
/* Remove c */
|
|
||||||
dir = rab_dir;
|
|
||||||
s.len = 2;
|
|
||||||
} else if (rac) {
|
|
||||||
/* Remove b */
|
|
||||||
dir = rac_dir;
|
|
||||||
s.b = s.c;
|
|
||||||
s.len = 2;
|
|
||||||
} else if (rbc) {
|
|
||||||
/* Remove a */
|
|
||||||
dir = rbc_dir;
|
|
||||||
s.a = s.b;
|
|
||||||
s.b = s.c;
|
|
||||||
s.len = 2;
|
|
||||||
} else if (ra) {
|
|
||||||
/* Remove bc */
|
|
||||||
s.len = 1;
|
|
||||||
} else if (rb) {
|
|
||||||
/* Remove ac */
|
|
||||||
s.a = s.b;
|
|
||||||
s.len = 1;
|
|
||||||
} else if (rc) {
|
|
||||||
/* Remove ab */
|
|
||||||
s.a = s.c;
|
|
||||||
s.len = 1;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if (colliding) {
|
|
||||||
struct gjk_menkowski_point *proto = arena_dry_push(scratch.arena, struct gjk_menkowski_point);
|
|
||||||
u32 proto_count = 0;
|
|
||||||
|
|
||||||
{
|
|
||||||
struct gjk_menkowski_point *tmp = arena_push_array(scratch.arena, struct gjk_menkowski_point, 3);
|
|
||||||
proto_count = s.len;
|
|
||||||
tmp[0] = s.a;
|
|
||||||
if (proto_count >= 2) {
|
|
||||||
tmp[1] = s.b;
|
|
||||||
if (proto_count >= 3) {
|
|
||||||
tmp[2] = s.c;
|
|
||||||
}
|
}
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
while (true) {
|
s.c = s.b;
|
||||||
if (step++ >= max_steps) goto abort;
|
s.b = s.a;
|
||||||
|
s.a = m;
|
||||||
|
s.len = 3;
|
||||||
|
|
||||||
struct v2 pen = V2(0, 0);
|
dir = v2_neg(perp_towards_point(s.a.p, s.b.p, s.c.p)); /* Normal of ab pointing away from c */
|
||||||
f32 pen_len_sq = F32_INFINITY;
|
struct v2 a_to_origin = v2_neg(s.a.p);
|
||||||
|
if (v2_dot(dir, a_to_origin) >= 0) {
|
||||||
/* Find dir from origin to closest edge */
|
/* Point is in region ab, remove c from simplex */
|
||||||
/* FIXME: Winding order of ps & pe index */
|
s.len = 2;
|
||||||
u32 pen_ps_index = 0;
|
} else {
|
||||||
u32 pen_pe_index = 0;
|
/* Point is not in region ab */
|
||||||
for (u32 i = 0; i < proto_count; ++i) {
|
dir = v2_neg(perp_towards_point(s.a.p, s.c.p, s.b.p)); /* Normal of ac pointing away from b */
|
||||||
u32 ps_index = i;
|
if (v2_dot(dir, a_to_origin) >= 0) {
|
||||||
u32 pe_index = (i < proto_count - 1) ? (i + 1) : 0;
|
/* Point is in region ac, remove b from simplex */
|
||||||
struct v2 ps = proto[ps_index].p;
|
s.b = s.c;
|
||||||
struct v2 pe = proto[pe_index].p;
|
s.len = 2;
|
||||||
|
} else {
|
||||||
struct v2 vse = v2_sub(pe, ps);
|
res.colliding = true;
|
||||||
struct v2 vso = v2_neg(ps);
|
|
||||||
|
|
||||||
struct v2 vsd = v2_mul(vse, (v2_dot(vso, vse) / v2_dot(vse, vse)));
|
|
||||||
struct v2 pd = v2_add(ps, vsd);
|
|
||||||
|
|
||||||
f32 pd_len_sq = v2_len_sq(pd);
|
|
||||||
if (pd_len_sq < pen_len_sq) {
|
|
||||||
pen_ps_index = ps_index;
|
|
||||||
pen_pe_index = pe_index;
|
|
||||||
pen = pd;
|
|
||||||
pen_len_sq = pd_len_sq;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Find new point in dir */
|
|
||||||
m = extended_menkowski_point(poly0, poly1, pen);
|
|
||||||
|
|
||||||
/* TODO: Move to break */
|
|
||||||
s.a = proto[pen_ps_index];
|
|
||||||
s.b = proto[pen_pe_index];
|
|
||||||
s.len = 2;
|
|
||||||
|
|
||||||
/* 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))) < unique_epsilon) {
|
|
||||||
unique = false;
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
if (!unique) {
|
}
|
||||||
break;
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (res.colliding) {
|
||||||
|
use_penetration_dir = !v2_eq(penetration_dir, V2(0, 0));
|
||||||
|
if (use_penetration_dir) {
|
||||||
|
/* ========================== *
|
||||||
|
* Penetration dir expansion
|
||||||
|
* ========================== */
|
||||||
|
|
||||||
|
while (true) {
|
||||||
|
/* Second point is support point towards penetration_dir */
|
||||||
|
if (s.len == 1) {
|
||||||
|
dir = v2_sub(v2_mul(penetration_dir, v2_dot(penetration_dir, s.a.p) / v2_dot(penetration_dir, penetration_dir)), s.a.p);
|
||||||
|
m = menkowski_point_extended(shape0, shape1, dir);
|
||||||
|
if (v2_eq(m.p, s.a.p)) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
s.b = s.a;
|
||||||
|
s.a = m;
|
||||||
|
s.len = 2;
|
||||||
|
|
||||||
|
/* Third point is support point in direction of line normal towards `a` projected onto penetration_dir */
|
||||||
|
dir = perp_towards_dir(s.a.p, s.b.p, penetration_dir);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (s.len == 2) {
|
||||||
|
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))) < unique_epsilon) {
|
||||||
|
/* New point is already on the current line */
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
s.c = s.b;
|
||||||
|
s.b = s.a;
|
||||||
|
s.a = m;
|
||||||
|
s.len = 3;
|
||||||
|
}
|
||||||
|
|
||||||
|
i32 a_wedgesign = math_fsign(v2_wedge(penetration_dir, s.a.p));
|
||||||
|
i32 b_wedgesign = math_fsign(v2_wedge(penetration_dir, s.b.p));
|
||||||
|
i32 c_wedgesign = math_fsign(v2_wedge(penetration_dir, s.c.p));
|
||||||
|
if (a_wedgesign != b_wedgesign) {
|
||||||
|
/* Remove c */
|
||||||
|
dir = perp_towards_dir(s.a.p, s.b.p, penetration_dir);
|
||||||
|
s.len = 2;
|
||||||
|
} else if (b_wedgesign != c_wedgesign) {
|
||||||
|
/* Remove b */
|
||||||
|
dir = perp_towards_dir(s.a.p, s.c.p, penetration_dir);
|
||||||
|
s.b = s.c;
|
||||||
|
s.len = 2;
|
||||||
|
} else {
|
||||||
|
/* Remove a */
|
||||||
|
dir = perp_towards_dir(s.b.p, s.c.p, penetration_dir);
|
||||||
|
s.a = s.b;
|
||||||
|
s.b = s.c;
|
||||||
|
s.len = 2;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
/* ========================== *
|
||||||
|
* Epa expansion
|
||||||
|
* ========================== */
|
||||||
|
|
||||||
|
ASSERT(s.len == 3);
|
||||||
|
struct temp_arena scratch = scratch_begin_no_conflict();
|
||||||
|
|
||||||
|
struct gjk_menkowski_point *proto = arena_dry_push(scratch.arena, struct gjk_menkowski_point);
|
||||||
|
u32 proto_count = 0;
|
||||||
|
|
||||||
|
{
|
||||||
|
struct gjk_menkowski_point *tmp = arena_push_array(scratch.arena, struct gjk_menkowski_point, 3);
|
||||||
|
proto_count = s.len;
|
||||||
|
tmp[0] = s.a;
|
||||||
|
if (proto_count >= 2) {
|
||||||
|
tmp[1] = s.b;
|
||||||
|
if (proto_count >= 3) {
|
||||||
|
tmp[2] = s.c;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Insert point into prototype */
|
while (true) {
|
||||||
/* FIXME: Preserve winding order */
|
struct v2 pen = V2(0, 0);
|
||||||
arena_push(scratch.arena, struct v2);
|
f32 pen_len_sq = F32_INFINITY;
|
||||||
++proto_count;
|
|
||||||
for (u32 i = proto_count - 1; i > pen_pe_index; --i) {
|
/* Find dir from origin to closest edge */
|
||||||
u32 shift_from = (i > 0) ? i - 1 : proto_count - 1;
|
/* FIXME: Winding order of ps & pe index */
|
||||||
u32 shift_to = i;
|
u32 pen_ps_index = 0;
|
||||||
proto[shift_to] = proto[shift_from];
|
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);
|
||||||
|
|
||||||
|
struct v2 vsd = v2_mul(vse, (v2_dot(vso, vse) / v2_dot(vse, vse)));
|
||||||
|
struct v2 pd = v2_add(ps, vsd);
|
||||||
|
|
||||||
|
f32 pd_len_sq = v2_len_sq(pd);
|
||||||
|
if (pd_len_sq < pen_len_sq) {
|
||||||
|
pen_ps_index = ps_index;
|
||||||
|
pen_pe_index = pe_index;
|
||||||
|
pen = pd;
|
||||||
|
pen_len_sq = pd_len_sq;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Find new point in dir */
|
||||||
|
m = menkowski_point_extended(shape0, shape1, pen);
|
||||||
|
|
||||||
|
/* 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))) < unique_epsilon) {
|
||||||
|
unique = false;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (!unique) {
|
||||||
|
s.a = proto[pen_ps_index];
|
||||||
|
s.b = proto[pen_pe_index];
|
||||||
|
s.len = 2;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Insert point into prototype */
|
||||||
|
/* FIXME: Preserve winding order */
|
||||||
|
arena_push(scratch.arena, struct v2);
|
||||||
|
++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;
|
||||||
|
}
|
||||||
|
|
||||||
|
scratch_end(scratch);
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
/* ========================== *
|
||||||
|
* Closest point expansion
|
||||||
|
* ========================== */
|
||||||
|
|
||||||
|
if (s.len == 2) {
|
||||||
|
/* Third point is support point in direction of line normal towards `a` projected onto penetration_dir */
|
||||||
|
dir = perp_towards_dir(s.a.p, s.b.p, penetration_dir);
|
||||||
|
}
|
||||||
|
|
||||||
|
while (true) {
|
||||||
|
/* Second point is support point towards origin */
|
||||||
|
if (s.len == 1) {
|
||||||
|
m = menkowski_point_extended(shape0, shape1, v2_neg(s.a.p));
|
||||||
|
if (v2_eq(m.p, s.a.p)) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
s.b = s.a;
|
||||||
|
s.a = m;
|
||||||
|
s.len = 2;
|
||||||
|
|
||||||
|
/* Third point is support point in direction of line normal towards origin */
|
||||||
|
dir = perp_towards_point(s.a.p, s.b.p, V2(0, 0));
|
||||||
|
}
|
||||||
|
|
||||||
|
if (s.len == 2) {
|
||||||
|
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))) < unique_epsilon) {
|
||||||
|
/* New point is already on the current line */
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
s.c = s.b;
|
||||||
|
s.b = s.a;
|
||||||
|
s.a = m;
|
||||||
|
s.len = 3;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Remove point or edge and determine next direction based on vornoi regions */
|
||||||
|
i32 code = 0;
|
||||||
|
struct v2 rab_dir = v2_neg(perp_towards_point(s.a.p, s.b.p, s.c.p));
|
||||||
|
struct v2 rac_dir = v2_neg(perp_towards_point(s.a.p, s.c.p, s.b.p));
|
||||||
|
struct v2 rbc_dir = v2_neg(perp_towards_point(s.b.p, s.c.p, s.a.p));
|
||||||
|
code |= ((v2_dot(rab_dir, v2_neg(s.a.p)) >= 0) << 0); /* Regions ab, a, and b*/
|
||||||
|
code |= ((v2_dot(rac_dir, v2_neg(s.a.p)) >= 0) << 1); /* Regions ac, a, and c */
|
||||||
|
code |= ((v2_dot(rbc_dir, v2_neg(s.b.p)) >= 0) << 2); /* Regions bc, b, and c */
|
||||||
|
switch (code) {
|
||||||
|
case 1: { /* Region ab, remove c */
|
||||||
|
dir = rab_dir;
|
||||||
|
s.len = 2;
|
||||||
|
} break;
|
||||||
|
case 2: { /* Region ac, remove b */
|
||||||
|
dir = rac_dir;
|
||||||
|
s.b = s.c;
|
||||||
|
s.len = 2;
|
||||||
|
} break;
|
||||||
|
case 4: { /* Region bc, remove a */
|
||||||
|
dir = rbc_dir;
|
||||||
|
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;
|
||||||
}
|
}
|
||||||
proto[pen_pe_index] = m;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
{
|
/* Resolve points */
|
||||||
if (s.len == 1) {
|
if (s.len == 1) {
|
||||||
shape0_p = s.a.p0;
|
res.p0 = s.a.p0;
|
||||||
shape1_p = s.a.p1;
|
res.p1 = s.a.p1;
|
||||||
} else if (s.len == 2) {
|
} else if (s.len == 2) {
|
||||||
|
/* FIXME: Winding order dependent? */
|
||||||
|
f32 ratio;
|
||||||
|
if (use_penetration_dir) {
|
||||||
|
/* Determine ratio between edge a & b that penetration dir intersection lies */
|
||||||
|
f32 wedgea = math_fabs(v2_wedge(penetration_dir, s.a.p));
|
||||||
|
f32 wedgeb = math_fabs(v2_wedge(penetration_dir, s.b.p));
|
||||||
|
ratio = wedgea / (wedgea + wedgeb);
|
||||||
|
} else {
|
||||||
/* Determine ratio between edge a & b that projected origin lies */
|
/* Determine ratio between edge a & b that projected origin lies */
|
||||||
struct v2 vab = v2_sub(s.b.p, s.a.p);
|
struct v2 vab = v2_sub(s.b.p, s.a.p);
|
||||||
struct v2 vao = v2_neg(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);
|
ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1);
|
||||||
/* Determine point on shape 0 */
|
}
|
||||||
{
|
/* Determine point on shape 0 */
|
||||||
shape0_p = v2_sub(s.b.p0, s.a.p0);
|
{
|
||||||
shape0_p = v2_mul(shape0_p, ratio);
|
res.p0 = v2_sub(s.b.p0, s.a.p0);
|
||||||
shape0_p = v2_add(shape0_p, s.a.p0);
|
res.p0 = v2_mul(res.p0, ratio);
|
||||||
}
|
res.p0 = v2_add(res.p0, s.a.p0);
|
||||||
/* Determine point on shape 1 */
|
}
|
||||||
{
|
/* Determine point on shape 1 */
|
||||||
shape1_p = v2_sub(s.b.p1, s.a.p1);
|
{
|
||||||
shape1_p = v2_mul(shape1_p, ratio);
|
res.p1 = v2_sub(s.b.p1, s.a.p1);
|
||||||
shape1_p = v2_add(shape1_p, s.a.p1);
|
res.p1 = v2_mul(res.p1, ratio);
|
||||||
}
|
res.p1 = v2_add(res.p1, s.a.p1);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
abort:
|
|
||||||
struct gjk_extended_result res = { 0 };
|
|
||||||
res.colliding = colliding;
|
|
||||||
res.p0 = shape0_p;
|
|
||||||
res.p1 = shape1_p;
|
|
||||||
res.final_simplex = s;
|
|
||||||
scratch_end(scratch);
|
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
#else
|
|
||||||
|
|
||||||
INTERNAL b32 ray_intersects_line(struct v2 p0, struct v2 p1, struct v2 ray_dir)
|
|
||||||
{
|
|
||||||
return (v2_dot(ray_dir, p0) >= 0 || v2_dot(ray_dir, p1) >= 0) && math_fsign(v2_wedge(ray_dir, p0)) != math_fsign(v2_wedge(ray_dir, p1));
|
|
||||||
}
|
|
||||||
|
|
||||||
struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array poly1, u32 max_steps)
|
|
||||||
{
|
|
||||||
struct v2 movedir = V2(0, 999999);
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
struct temp_arena scratch = scratch_begin_no_conflict();
|
|
||||||
b32 colliding = false;
|
|
||||||
struct v2 shape0_p = V2(0, 0);
|
|
||||||
struct v2 shape1_p = V2(0, 0);
|
|
||||||
u32 step = 0;
|
|
||||||
|
|
||||||
/* TODO: Verify epsilon */
|
|
||||||
f32 unique_epsilon = 0.00001;
|
|
||||||
|
|
||||||
/* Simplex */
|
|
||||||
struct gjk_extended_simplex s = { 0 };
|
|
||||||
|
|
||||||
/* First point is support point towards shape centers */
|
|
||||||
if (step++ >= max_steps) goto abort;
|
|
||||||
struct v2 dir = v2_sub(poly1.points[0], poly0.points[0]);
|
|
||||||
struct gjk_menkowski_point m = extended_menkowski_point(poly0, poly1, dir);
|
|
||||||
s.a = m;
|
|
||||||
s.len = 1;
|
|
||||||
|
|
||||||
while (true) {
|
|
||||||
/* Second point is support point towards movedir */
|
|
||||||
if (s.len == 1) {
|
|
||||||
if (step++ >= max_steps) goto abort;
|
|
||||||
#if 0
|
|
||||||
m = extended_menkowski_point(poly0, poly1, movedir);
|
|
||||||
if (v2_eq(m.p, s.a.p)) {
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
#else
|
|
||||||
dir = v2_sub(v2_mul(movedir, v2_dot(movedir, s.a.p) / v2_dot(movedir, movedir)), s.a.p);
|
|
||||||
m = extended_menkowski_point(poly0, poly1, dir);
|
|
||||||
if (v2_eq(m.p, s.a.p)) {
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
s.b = s.a;
|
|
||||||
s.a = m;
|
|
||||||
s.len = 2;
|
|
||||||
|
|
||||||
/* Third point is support point in direction of line normal towards `a` projected onto movedir */
|
|
||||||
dir = perp_towards_dir(s.a.p, s.b.p, movedir);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (step++ >= max_steps) goto abort;
|
|
||||||
m = extended_menkowski_point(poly0, poly1, dir);
|
|
||||||
|
|
||||||
if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_sub(m.p, s.a.p))) < unique_epsilon) {
|
|
||||||
/* New point is already on the current line */
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
s.c = s.b;
|
|
||||||
s.b = s.a;
|
|
||||||
s.a = m;
|
|
||||||
s.len = 3;
|
|
||||||
|
|
||||||
if (step++ >= max_steps) goto abort;
|
|
||||||
|
|
||||||
/* TODO: Can group dots & wedges from intersect checks */
|
|
||||||
b32 abi = ray_intersects_line(s.a.p, s.b.p, movedir);
|
|
||||||
b32 aci = ray_intersects_line(s.a.p, s.c.p, movedir);
|
|
||||||
b32 bci = ray_intersects_line(s.b.p, s.c.p, movedir);
|
|
||||||
|
|
||||||
|
|
||||||
struct v2 ab_dir = perp_towards_dir(s.a.p, s.b.p, movedir);
|
|
||||||
struct v2 ac_dir = perp_towards_dir(s.a.p, s.c.p, movedir);
|
|
||||||
struct v2 bc_dir = perp_towards_dir(s.b.p, s.c.p, movedir);
|
|
||||||
|
|
||||||
if (abi) {
|
|
||||||
/* Remove c */
|
|
||||||
dir = ab_dir;
|
|
||||||
s.len = 2;
|
|
||||||
} else if (aci) {
|
|
||||||
/* Remove b */
|
|
||||||
dir = ac_dir;
|
|
||||||
s.b = s.c;
|
|
||||||
s.len = 2;
|
|
||||||
} else if (bci) {
|
|
||||||
/* Remove a */
|
|
||||||
dir = bc_dir;
|
|
||||||
s.a = s.b;
|
|
||||||
s.b = s.c;
|
|
||||||
s.len = 2;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
#if 0
|
|
||||||
{
|
|
||||||
if (s.len == 1) {
|
|
||||||
shape0_p = s.a.p0;
|
|
||||||
shape1_p = s.a.p1;
|
|
||||||
} else if (s.len == 2) {
|
|
||||||
/* Determine ratio between edge a & b that projected origin lies */
|
|
||||||
/* FIXME: Winding order dependent? */
|
|
||||||
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);
|
|
||||||
/* Determine point on 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);
|
|
||||||
}
|
|
||||||
/* Determine point on 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);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#else
|
|
||||||
{
|
|
||||||
if (s.len == 1) {
|
|
||||||
shape0_p = s.a.p0;
|
|
||||||
shape1_p = s.a.p1;
|
|
||||||
} else if (s.len == 2) {
|
|
||||||
/* Determine ratio between edge a & b that ray intersection lies */
|
|
||||||
/* FIXME: Winding order dependent? */
|
|
||||||
f32 wedgea = math_fabs(v2_wedge(movedir, s.a.p));
|
|
||||||
f32 wedgeb = math_fabs(v2_wedge(movedir, s.b.p));
|
|
||||||
f32 ratio = wedgea / (wedgea + wedgeb);
|
|
||||||
/* Determine point on 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);
|
|
||||||
}
|
|
||||||
/* Determine point on 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);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
abort:
|
|
||||||
struct gjk_extended_result res = { 0 };
|
|
||||||
res.colliding = colliding;
|
|
||||||
res.p0 = shape0_p;
|
|
||||||
res.p1 = shape1_p;
|
|
||||||
res.final_simplex = s;
|
|
||||||
scratch_end(scratch);
|
|
||||||
return res;
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
@ -577,7 +511,7 @@ struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_
|
|||||||
for (u64 i = 0; i < rays; ++i) {
|
for (u64 i = 0; i < rays; ++i) {
|
||||||
f32 angle = ((f32)i / rays) * (2 * PI);
|
f32 angle = ((f32)i / rays) * (2 * PI);
|
||||||
struct v2 dir = v2_from_angle(angle);
|
struct v2 dir = v2_from_angle(angle);
|
||||||
struct v2 p = extended_menkowski_point(poly0, poly1, dir).p;
|
struct v2 p = menkowski_point_extended(poly0, poly1, dir).p;
|
||||||
if (res.count == 0 || !v2_eq(p, res.points[res.count - 1])) {
|
if (res.count == 0 || !v2_eq(p, res.points[res.count - 1])) {
|
||||||
*arena_push(arena, struct v2) = p;
|
*arena_push(arena, struct v2) = p;
|
||||||
++res.count;
|
++res.count;
|
||||||
|
|||||||
14
src/util.h
14
src/util.h
@ -184,33 +184,27 @@ INLINE void sleep_frame(sys_timestamp_t last_frame_time, f64 target_dt)
|
|||||||
* Collision testing
|
* Collision testing
|
||||||
* ========================== */
|
* ========================== */
|
||||||
|
|
||||||
/* TODO: Remove this */
|
|
||||||
|
|
||||||
struct gjk_menkowski_point {
|
struct gjk_menkowski_point {
|
||||||
struct v2 p0; /* Support point of first shape in dir */
|
struct v2 p0; /* Support point of first shape in dir */
|
||||||
struct v2 p1; /* Support point of second shape in -dir */
|
struct v2 p1; /* Support point of second shape in -dir */
|
||||||
struct v2 p; /* Menkowski difference point */
|
struct v2 p; /* Menkowski difference point */
|
||||||
};
|
};
|
||||||
|
|
||||||
struct gjk_extended_simplex {
|
struct gjk_simplex {
|
||||||
u32 len;
|
u32 len;
|
||||||
struct gjk_menkowski_point a, b, c;
|
struct gjk_menkowski_point a, b, c;
|
||||||
};
|
};
|
||||||
|
|
||||||
struct gjk_extended_result {
|
struct gjk_extended_result {
|
||||||
b32 colliding;
|
b32 colliding;
|
||||||
struct v2 p0; /* Closest / deepest point on first shape's edge */
|
struct v2 p0, p1;
|
||||||
struct v2 p1; /* Closest / deepest point on second shape's edge */
|
|
||||||
|
|
||||||
/* For debugging */
|
|
||||||
struct gjk_extended_simplex final_simplex;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
struct v2 perp_towards_point(struct v2 start, struct v2 end, struct v2 p);
|
struct v2 perp_towards_point(struct v2 start, struct v2 end, struct v2 p);
|
||||||
struct v2 perp_towards_dir(struct v2 start, struct v2 end, struct v2 dir);
|
struct v2 perp_towards_dir(struct v2 start, struct v2 end, struct v2 dir);
|
||||||
struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1);
|
struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1);
|
||||||
i32 poly_get_winding_order(struct v2_array poly);
|
i32 poly_get_winding_order(struct v2_array poly);
|
||||||
b32 gjk_boolean(struct v2_array poly0, struct v2_array poly1);
|
b32 gjk_boolean(struct v2_array shape0, struct v2_array shape1);
|
||||||
struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array poly1, u32 max_steps);
|
struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1, struct v2 penetration_dir);
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user