gjk expanded testing non-colliding closest face
This commit is contained in:
parent
99381f1274
commit
8fd92c55a1
@ -94,7 +94,7 @@ struct entity {
|
||||
/* TODO: Remove this (testing) */
|
||||
b32 colliding;
|
||||
struct entity_handle colliding_with;
|
||||
struct simplex simplex;
|
||||
struct gjk_extended_simplex simplex;
|
||||
struct v2 pen;
|
||||
struct v2 spot;
|
||||
|
||||
|
||||
119
src/game.c
119
src/game.c
@ -682,46 +682,6 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
|
||||
* ========================== */
|
||||
|
||||
#if 0
|
||||
for (u64 e0_index = 0; e0_index < store->reserved; ++e0_index) {
|
||||
struct entity *e0 = &store->entities[e0_index];
|
||||
if (!(e0->valid && entity_has_prop(e0, ENTITY_PROP_ACTIVE))) continue;
|
||||
if (!entity_has_prop(e0, ENTITY_PROP_PHYSICAL)) continue;
|
||||
|
||||
struct xform e0_xf = entity_get_xform(e0);
|
||||
struct quad e0_quad;
|
||||
{
|
||||
struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, e0->sprite);
|
||||
struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("shape"), e0->animation_frame);
|
||||
e0_quad = xform_mul_quad(e0->sprite_local_xform, quad_from_rect(slice.rect));
|
||||
e0_quad = xform_mul_quad(e0_xf, e0_quad);
|
||||
}
|
||||
|
||||
b32 colliding = false;
|
||||
for (u64 e1_index = 0; e1_index < store->reserved; ++e1_index) {
|
||||
struct entity *e1 = &store->entities[e1_index];
|
||||
if (e1 == e0) continue;
|
||||
if (!(e1->valid && entity_has_prop(e1, ENTITY_PROP_ACTIVE))) continue;
|
||||
if (!entity_has_prop(e1, ENTITY_PROP_PHYSICAL)) continue;
|
||||
|
||||
struct xform e1_xf = entity_get_xform(e1);
|
||||
struct quad e1_quad;
|
||||
{
|
||||
struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, e1->sprite);
|
||||
struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("shape"), e1->animation_frame);
|
||||
e1_quad = xform_mul_quad(e1->sprite_local_xform, quad_from_rect(slice.rect));
|
||||
e1_quad = xform_mul_quad(e1_xf, e1_quad);
|
||||
}
|
||||
|
||||
struct gjk_result res = gjk(e0_quad, e1_quad);
|
||||
if (res.colliding) {
|
||||
colliding = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
e0->colliding = colliding;
|
||||
}
|
||||
#else
|
||||
for (u64 e0_index = 0; e0_index < store->reserved; ++e0_index) {
|
||||
struct entity *e0 = &store->entities[e0_index];
|
||||
if (!(e0->valid && entity_has_prop(e0, ENTITY_PROP_ACTIVE))) continue;
|
||||
@ -743,7 +703,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
|
||||
}
|
||||
|
||||
b32 colliding = false;
|
||||
struct simplex simplex = { 0 };
|
||||
struct gjk_extended_simplex simplex = { 0 };
|
||||
struct v2 pen = V2(0, 0);
|
||||
struct v2 spot = V2(0, 0);
|
||||
struct entity_handle colliding_with = { 0 };
|
||||
@ -767,13 +727,86 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
|
||||
};
|
||||
}
|
||||
|
||||
struct gjk_result res = gjk(e0_poly, e1_poly, G.gjk_steps);
|
||||
colliding = gjk_boolean(e0_poly, e1_poly, G.gjk_steps);
|
||||
colliding_with = e1->handle;
|
||||
//simplex = res.final_simplex;
|
||||
|
||||
if (colliding) {
|
||||
//pen = epa_colliding_pen(e0_poly, e1_poly, simplex);
|
||||
#if 0
|
||||
/* Pen movement test */
|
||||
{
|
||||
struct xform xf = e1_xf;
|
||||
xf.og = v2_add(xf.og, pen);
|
||||
entity_set_xform(e1, xf);
|
||||
//e1->verlet_xform.og = v2_add(e1->verlet_xform.og, pen);
|
||||
}
|
||||
#endif
|
||||
} else {
|
||||
spot = e0->spot;
|
||||
}
|
||||
}
|
||||
|
||||
e0->colliding = colliding;
|
||||
e0->colliding_with = colliding_with;
|
||||
e0->simplex = simplex;
|
||||
e0->pen = pen;
|
||||
e0->spot = spot;
|
||||
}
|
||||
#else
|
||||
for (u64 e0_index = 0; e0_index < store->reserved; ++e0_index) {
|
||||
struct entity *e0 = &store->entities[e0_index];
|
||||
if (!(e0->valid && entity_has_prop(e0, ENTITY_PROP_ACTIVE))) continue;
|
||||
if (!entity_has_prop(e0, ENTITY_PROP_PHYSICAL)) continue;
|
||||
if (!entity_has_prop(e0, ENTITY_PROP_PLAYER_CONTROLLED)) continue;
|
||||
|
||||
struct xform e0_xf = entity_get_xform(e0);
|
||||
struct quad e0_quad;
|
||||
struct v2_array e0_poly;
|
||||
{
|
||||
struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, e0->sprite);
|
||||
struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("shape"), e0->animation_frame);
|
||||
e0_quad = xform_mul_quad(e0->sprite_local_xform, quad_from_rect(slice.rect));
|
||||
e0_quad = xform_mul_quad(e0_xf, e0_quad);
|
||||
e0_poly = (struct v2_array) {
|
||||
.count = ARRAY_COUNT(e0_quad.e),
|
||||
.points = e0_quad.e
|
||||
};
|
||||
}
|
||||
|
||||
b32 colliding = false;
|
||||
struct gjk_extended_simplex simplex = { 0 };
|
||||
struct v2 pen = V2(0, 0);
|
||||
struct v2 spot = V2(0, 0);
|
||||
struct entity_handle colliding_with = { 0 };
|
||||
for (u64 e1_index = 0; e1_index < store->reserved; ++e1_index) {
|
||||
struct entity *e1 = &store->entities[e1_index];
|
||||
if (e1 == e0) continue;
|
||||
if (!(e1->valid && entity_has_prop(e1, ENTITY_PROP_ACTIVE))) continue;
|
||||
if (!entity_has_prop(e1, ENTITY_PROP_PHYSICAL)) continue;
|
||||
|
||||
struct xform e1_xf = entity_get_xform(e1);
|
||||
struct quad e1_quad;
|
||||
struct v2_array e1_poly;
|
||||
{
|
||||
struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, e1->sprite);
|
||||
struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("shape"), e1->animation_frame);
|
||||
e1_quad = xform_mul_quad(e1->sprite_local_xform, quad_from_rect(slice.rect));
|
||||
e1_quad = xform_mul_quad(e1_xf, e1_quad);
|
||||
e1_poly = (struct v2_array) {
|
||||
.count = ARRAY_COUNT(e1_quad.e),
|
||||
.points = e1_quad.e
|
||||
};
|
||||
}
|
||||
|
||||
struct gjk_extended_result res = gjk_extended(e0_poly, e1_poly, G.gjk_steps);
|
||||
colliding = res.colliding;
|
||||
colliding_with = e1->handle;
|
||||
simplex = res.final_simplex;
|
||||
pen = res.colliding_pen;
|
||||
|
||||
if (colliding) {
|
||||
pen = epa(e0_poly, e1_poly, simplex);
|
||||
//pen = epa_colliding_pen(e0_poly, e1_poly, simplex);
|
||||
#if 0
|
||||
/* Pen movement test */
|
||||
{
|
||||
|
||||
10
src/user.c
10
src/user.c
@ -1034,14 +1034,14 @@ INTERNAL void user_update(void)
|
||||
|
||||
/* Draw simplex */
|
||||
{
|
||||
f32 thickness = 5;
|
||||
f32 thickness = 2;
|
||||
u32 line_color = colliding ? COLOR_WHITE: COLOR_YELLOW;
|
||||
u32 color_first = RGBA_32_F(1, 0, 0, 0.5);
|
||||
u32 color_second = RGBA_32_F(0, 1, 0, 0.5);
|
||||
u32 color_third = RGBA_32_F(0, 0, 1, 0.5);
|
||||
|
||||
struct simplex simplex = ent->simplex;
|
||||
struct v2 simplex_points[] = { simplex.a, simplex.b, simplex.c };
|
||||
struct gjk_extended_simplex simplex = ent->simplex;
|
||||
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 };
|
||||
|
||||
@ -1051,13 +1051,15 @@ INTERNAL void user_update(void)
|
||||
}
|
||||
if (simplex.len >= 2) {
|
||||
u32 color = simplex.len == 2 ? color_first : color_second;
|
||||
draw_solid_poly_line(G.viewport_canvas, simplex_array, simplex.len > 2, thickness, line_color);
|
||||
draw_solid_circle(G.viewport_canvas, simplex_array.points[1], thickness * 2, color, 10);
|
||||
}
|
||||
if (simplex.len >= 3) {
|
||||
u32 color = color_first;
|
||||
draw_solid_circle(G.viewport_canvas, simplex_array.points[2], thickness * 2, color, 10);
|
||||
}
|
||||
if (simplex.len >= 2) {
|
||||
draw_solid_poly_line(G.viewport_canvas, simplex_array, simplex.len > 2, thickness, line_color);
|
||||
}
|
||||
}
|
||||
|
||||
/* Draw pen */
|
||||
|
||||
555
src/util.c
555
src/util.c
@ -47,6 +47,7 @@ struct string util_file_name_from_path(struct string path)
|
||||
|
||||
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) {
|
||||
@ -60,9 +61,13 @@ struct v2 poly_support(struct v2_array a, struct v2 dir)
|
||||
return furthest;
|
||||
}
|
||||
|
||||
struct v2 menkowski_point(struct v2_array poly0, struct v2_array poly1, struct v2 dir)
|
||||
INTERNAL struct gjk_menkowski_point menkowski_point(struct v2_array poly0, struct v2_array poly1, struct v2 dir)
|
||||
{
|
||||
return v2_sub(poly_support(poly0, dir), poly_support(poly1, v2_neg(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 v2 perp_towards_point(struct v2 start, struct v2 end, struct v2 p)
|
||||
@ -72,40 +77,80 @@ struct v2 perp_towards_point(struct v2 start, struct v2 end, struct v2 p)
|
||||
return v2_mul(perp, sign);
|
||||
}
|
||||
|
||||
struct gjk_result gjk(struct v2_array poly0, struct v2_array poly1, u32 max_steps)
|
||||
/* 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(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;
|
||||
}
|
||||
|
||||
/* Returns 1 if winding forward, -1 if backward */
|
||||
i32 poly_get_winding_order(struct v2_array poly)
|
||||
{
|
||||
i32 res;
|
||||
if (poly.count >= 3) {
|
||||
struct v2 a = poly.points[0];
|
||||
struct v2 b = poly.points[1];
|
||||
struct v2 c = poly.points[2];
|
||||
if (v2_wedge(v2_sub(b, a), v2_sub(c, b)) > 0) {
|
||||
res = 1;
|
||||
} else {
|
||||
res = -1;
|
||||
}
|
||||
} else {
|
||||
res = -1;
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
b32 gjk_boolean(struct v2_array poly0, struct v2_array poly1, u32 max_steps)
|
||||
{
|
||||
b32 colliding = false;
|
||||
u32 step = 0;
|
||||
|
||||
/* Simplex */
|
||||
struct simplex s = { 0 };
|
||||
struct { struct v2 a, b, c; } s = { 0 };
|
||||
|
||||
/* First point is support point towards shape centers */
|
||||
if (step++ >= max_steps) goto abort;
|
||||
{
|
||||
s.a = menkowski_point(poly0, poly1, v2_norm(v2_sub(poly1.points[0], poly0.points[0])));
|
||||
s.len = 1;
|
||||
}
|
||||
s.a = menkowski_point(poly0, poly1, v2_sub(poly1.points[0], poly0.points[0])).p;
|
||||
|
||||
/* Second point is support point towards origin */
|
||||
if (step++ >= max_steps) goto abort;
|
||||
b32 valid_line = false;
|
||||
{
|
||||
struct v2 dir = v2_neg(s.a);
|
||||
struct v2 p = menkowski_point(poly0, poly1, dir);
|
||||
struct v2 p = menkowski_point(poly0, poly1, dir).p;
|
||||
if (v2_dot(dir, p) >= 0) {
|
||||
s.b = s.a;
|
||||
s.a = p;
|
||||
s.len = 2;
|
||||
valid_line = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (s.len == 2) {
|
||||
if (valid_line) {
|
||||
while (true) {
|
||||
if (step++ >= max_steps) goto abort;
|
||||
|
||||
/* Third point is support point in direction of line normal towards origin */
|
||||
struct v2 dir = perp_towards_point(s.a, s.b, V2(0, 0));
|
||||
struct v2 p = menkowski_point(poly0, poly1, dir);
|
||||
struct v2 p = menkowski_point(poly0, poly1, dir).p;
|
||||
if (v2_dot(dir, p) < 0) {
|
||||
colliding = false;
|
||||
break;
|
||||
@ -127,7 +172,6 @@ struct gjk_result gjk(struct v2_array poly0, struct v2_array poly1, u32 max_step
|
||||
s.b = s.c;
|
||||
} else {
|
||||
/* Point is in simplex */
|
||||
s.len = 3;
|
||||
colliding = true;
|
||||
break;
|
||||
}
|
||||
@ -136,150 +180,403 @@ struct gjk_result gjk(struct v2_array poly0, struct v2_array poly1, u32 max_step
|
||||
}
|
||||
|
||||
abort:
|
||||
return (struct gjk_result) {
|
||||
.colliding = colliding,
|
||||
.final_simplex = s
|
||||
};
|
||||
return colliding;
|
||||
}
|
||||
|
||||
|
||||
struct v2 epa(struct v2_array poly0, struct v2_array poly1, struct simplex simplex)
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#if 0
|
||||
struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array poly1, u32 max_steps)
|
||||
{
|
||||
struct temp_arena scratch = scratch_begin_no_conflict();
|
||||
ASSERT(simplex.len == 3);
|
||||
|
||||
struct v2 *proto = arena_dry_push(scratch.arena, struct v2);
|
||||
{
|
||||
struct v2 *tmp = arena_push_array(scratch.arena, struct v2, 3);
|
||||
tmp[0] = simplex.a;
|
||||
tmp[1] = simplex.b;
|
||||
tmp[2] = simplex.c;
|
||||
}
|
||||
u32 proto_count = 3;
|
||||
|
||||
b32 colliding = false;
|
||||
struct v2 pen = V2(0, 0);
|
||||
f32 pen_len = F32_INFINITY;
|
||||
while (true) {
|
||||
pen = V2(0, 0);
|
||||
pen_len = F32_INFINITY;
|
||||
u32 step = 0;
|
||||
|
||||
/* Find dir from origin to closest edge */
|
||||
u32 pen_pe_index = 1;
|
||||
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];
|
||||
struct v2 pe = proto[pe_index];
|
||||
/* Simplex */
|
||||
struct gjk_extended_simplex s = { 0 };
|
||||
|
||||
struct v2 vse = v2_sub(pe, ps);
|
||||
struct v2 vso = v2_neg(ps);
|
||||
/* 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 menkowski_result m = menkowski_point(poly0, poly1, dir);
|
||||
s.a = m.p;
|
||||
s.a0 = m.p0;
|
||||
s.a1 = m.p1;
|
||||
s.len = 1;
|
||||
|
||||
struct v2 vsd = v2_mul(vse, (v2_dot(vso, vse) / v2_dot(vse, vse)));
|
||||
struct v2 pd = v2_add(ps, vsd);
|
||||
|
||||
/* TODO: sq cmp */
|
||||
f32 pd_len = v2_len(pd);
|
||||
if (pd_len < pen_len) {
|
||||
pen_pe_index = pe_index;
|
||||
pen = pd;
|
||||
pen_len = pd_len;
|
||||
}
|
||||
/* Second point is support point towards origin */
|
||||
if (step++ >= max_steps) goto abort;
|
||||
{
|
||||
dir = v2_neg(s.a);
|
||||
m = menkowski_point(poly0, poly1, dir);
|
||||
if (v2_dot(dir, m.p) >= 0) {
|
||||
s.b = s.a;
|
||||
s.b0 = s.a0;
|
||||
s.b1 = s.a1;
|
||||
s.a = m.p;
|
||||
s.a0 = m.p0;
|
||||
s.a1 = m.p1;
|
||||
s.len = 2;
|
||||
}
|
||||
}
|
||||
|
||||
/* Find new point in dir */
|
||||
#if 1
|
||||
/* FIXME: Ensure convexity */
|
||||
if (s.len == 2) {
|
||||
while (true) {
|
||||
if (step++ >= max_steps) goto abort;
|
||||
|
||||
struct v2 p = menkowski_point(poly0, poly1, pen);
|
||||
/* Third point is support point in direction of line normal towards origin */
|
||||
dir = perp_towards_point(s.a, s.b, V2(0, 0));
|
||||
m = menkowski_point(poly0, poly1, dir);
|
||||
if (v2_dot(dir, m.p) < 0) {
|
||||
colliding = false;
|
||||
break;
|
||||
}
|
||||
|
||||
/* Check unique */
|
||||
/* TODO: Better */
|
||||
/* TODO: Epsilon or iteration limit */
|
||||
{
|
||||
b32 unique = true;
|
||||
for (u32 i = 0; i < proto_count; ++i) {
|
||||
if (v2_eq(p, proto[i])) {
|
||||
unique = false;
|
||||
s.c = s.b;
|
||||
s.c0 = s.b0;
|
||||
s.c1 = s.b1;
|
||||
s.b = s.a;
|
||||
s.b0 = s.a0;
|
||||
s.b1 = s.a1;
|
||||
s.a = m.p;
|
||||
s.a0 = m.p0;
|
||||
s.a1 = m.p1;
|
||||
s.len = 3;
|
||||
|
||||
dir = v2_neg(perp_towards_point(s.a, s.b, s.c)); /* Normal dir 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) */
|
||||
s.len = 2;
|
||||
} else {
|
||||
/* Point is not in region ab */
|
||||
dir = v2_neg(perp_towards_point(s.a, s.c, s.b)); /* Normal dir 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;
|
||||
s.len = 2;
|
||||
} else {
|
||||
/* Point is in simplex */
|
||||
colliding = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!unique) {
|
||||
}
|
||||
}
|
||||
|
||||
if (colliding) { /* Calculate penetration depth using epa */
|
||||
ASSERT(s.len == 3);
|
||||
struct v2 *proto = arena_dry_push(scratch.arena, struct v2);
|
||||
{
|
||||
struct v2 *tmp = arena_push_array(scratch.arena, struct v2, 3);
|
||||
tmp[0] = s.a;
|
||||
tmp[1] = s.b;
|
||||
tmp[2] = s.c;
|
||||
}
|
||||
u32 proto_count = 3;
|
||||
|
||||
pen = V2(0, 0);
|
||||
f32 pen_len = F32_INFINITY;
|
||||
while (true) {
|
||||
pen = V2(0, 0);
|
||||
pen_len = F32_INFINITY;
|
||||
|
||||
/* Find dir from origin to closest edge */
|
||||
u32 pen_pe_index = 1;
|
||||
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];
|
||||
struct v2 pe = proto[pe_index];
|
||||
|
||||
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);
|
||||
|
||||
/* TODO: sq cmp */
|
||||
f32 pd_len = v2_len(pd);
|
||||
if (pd_len < pen_len) {
|
||||
pen_pe_index = pe_index;
|
||||
pen = pd;
|
||||
pen_len = pd_len;
|
||||
}
|
||||
}
|
||||
|
||||
/* Find new point in dir */
|
||||
m = menkowski_point(poly0, poly1, pen);
|
||||
|
||||
/* Check unique */
|
||||
/* TODO: Better */
|
||||
/* TODO: Epsilon or iteration limit */
|
||||
{
|
||||
b32 unique = true;
|
||||
for (u32 i = 0; i < proto_count; ++i) {
|
||||
if (v2_eq(m.p, proto[i])) {
|
||||
unique = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!unique) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* Insert point into prototype array */
|
||||
/* 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.p;
|
||||
}
|
||||
pen = v2_mul(v2_norm(pen), pen_len);
|
||||
}
|
||||
|
||||
abort:
|
||||
struct gjk_extended_result res = { 0 };
|
||||
res.colliding = colliding;
|
||||
res.colliding_pen = pen;
|
||||
res.final_simplex = s;
|
||||
scratch_end(scratch);
|
||||
return res;
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array poly1, u32 max_steps)
|
||||
{
|
||||
struct temp_arena scratch = scratch_begin_no_conflict();
|
||||
b32 colliding = false;
|
||||
struct v2 pen = V2(0, 0);
|
||||
u32 step = 0;
|
||||
|
||||
/* 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 = menkowski_point(poly0, poly1, dir);
|
||||
s.a = m;
|
||||
s.len = 1;
|
||||
|
||||
#if 0
|
||||
/* Second point is support point towards origin */
|
||||
{
|
||||
if (step++ >= max_steps) goto abort;
|
||||
dir = v2_neg(s.a.p);
|
||||
m = menkowski_point(poly0, poly1, dir);
|
||||
if (!v2_eq(s.a.p, m.p)) {
|
||||
s.b = s.a;
|
||||
s.a = m;
|
||||
s.len = 2;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
while (true) {
|
||||
/* Second point is support point towards origin */
|
||||
if (s.len == 1) {
|
||||
if (step++ >= max_steps) goto abort;
|
||||
dir = v2_neg(s.a.p);
|
||||
m = menkowski_point(poly0, poly1, dir);
|
||||
if (v2_eq(s.a.p, m.p)) {
|
||||
colliding = false;
|
||||
break;
|
||||
}
|
||||
|
||||
s.b = s.a;
|
||||
s.a = m;
|
||||
s.len = 2;
|
||||
}
|
||||
|
||||
/* Insert point into prototype array */
|
||||
/* 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] = p;
|
||||
#else
|
||||
/* FIXME: Maintain convexity */
|
||||
struct v2 p = menkowski_point(poly0, poly1, pen);
|
||||
if (v2_eq(p, ps) || v2_eq(p, pe)) {
|
||||
break;
|
||||
} else {
|
||||
/* Third point is support point in direction of line normal towards origin */
|
||||
if (step++ >= max_steps) goto abort;
|
||||
dir = perp_towards_point(s.a.p, s.b.p, V2(0, 0));
|
||||
m = menkowski_point(poly0, poly1, dir);
|
||||
if (v2_eq(m.p, s.a.p) || v2_eq(m.p, s.b.p)) {
|
||||
colliding = false;
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
pen = v2_mul(v2_norm(pen), pen_len);
|
||||
|
||||
scratch_end(scratch);
|
||||
return pen;
|
||||
}
|
||||
s.c = s.b;
|
||||
s.b = s.a;
|
||||
s.a = m;
|
||||
s.len = 3;
|
||||
|
||||
if (step++ >= max_steps) goto abort;
|
||||
|
||||
/* TODO: Remove this (debugging) */
|
||||
struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1)
|
||||
{
|
||||
DEBUGBREAKABLE;
|
||||
#if 0
|
||||
struct v2_array res = { .points = arena_dry_push(arena, struct v2) };
|
||||
for (u64 i = 0; i < poly0.count; ++i) {
|
||||
struct v2 a = poly0.points[i];
|
||||
for (u64 j = 0; j < poly1.count; ++j) {
|
||||
struct v2 b = poly1.points[j];
|
||||
*arena_push(arena, struct v2) = v2_sub(a, b);
|
||||
++res.count;
|
||||
dir = v2_neg(perp_towards_point(s.a, s.b, s.c)); /* Normal dir of ab pointing away from c */
|
||||
if (v2_dot(dir, v2_neg(s.a)) >= 0 && v2_dot(v2_neg(s.a), v2_sub(s.b, s.a)) >= 0 && v2_dot(v2_neg(s.b), v2_sub(s.a, s.b))) {
|
||||
/* Point is in region ab, remove c from simplex (will happen automatically next iteration) */
|
||||
s.len = 2;
|
||||
} else {
|
||||
/* Point is not in region ab */
|
||||
dir = v2_neg(perp_towards_point(s.a, s.c, s.b)); /* Normal dir of ac pointing away from b */
|
||||
if (v2_dot(dir, v2_neg(s.a)) >= 0 && v2_dot(v2_neg(s.a), v2_sub(s.c, s.a)) >= 0 && v2_dot(v2_neg(s.c), v2_sub(s.a, s.c))) {
|
||||
/* Point is in region ac, remove b from simplex */
|
||||
s.b = s.c;
|
||||
s.len = 2;
|
||||
} else {
|
||||
dir = v2_neg(perp_towards_point(s.b, s.c, s.a)); /* Normal dir of bc pointing away from a */
|
||||
if (v2_dot(dir, v2_neg(s.b)) >= 0 && v2_dot(v2_neg(s.b), v2_sub(s.c, s.b)) >= 0 && v2_dot(v2_neg(s.c), v2_sub(s.b, s.c))) {
|
||||
/* Point is in region bc, remove a from simplex */
|
||||
s.a = s.b;
|
||||
s.b = s.c;
|
||||
s.len = 2;
|
||||
} else {
|
||||
/* Point is in simplex */
|
||||
colliding = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return res;
|
||||
#else
|
||||
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(poly0, poly1, dir);
|
||||
if (res.count == 0 || !v2_eq(p, res.points[res.count - 1])) {
|
||||
*arena_push(arena, struct v2) = p;
|
||||
++res.count;
|
||||
b32 rab = v2_dot(perp_towards_point(s.a.p, s.b.p, s.c.p), v2_neg(s.a.p)) < 0;
|
||||
b32 rac = v2_dot(perp_towards_point(s.a.p, s.c.p, s.b.p), v2_neg(s.a.p)) < 0;
|
||||
b32 rbc = v2_dot(perp_towards_point(s.b.p, s.c.p, s.a.p), v2_neg(s.b.p)) < 0;
|
||||
if (!rab && !rac && !rbc) {
|
||||
colliding = true;
|
||||
break;
|
||||
}
|
||||
|
||||
/* Simplex 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 */
|
||||
s.len = 2;
|
||||
} else if (rac) {
|
||||
/* Remove b */
|
||||
s.b = s.c;
|
||||
s.len = 2;
|
||||
} else if (rbc) {
|
||||
/* Remove a */
|
||||
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;
|
||||
}
|
||||
}
|
||||
return res;
|
||||
#endif
|
||||
}
|
||||
|
||||
#if 0
|
||||
b32 epa_specific = false;
|
||||
struct v2 epa_specific_dir = V2(0, 0);
|
||||
|
||||
if (!colliding) {
|
||||
epa_specific = true;
|
||||
epa_specific_dir =
|
||||
}
|
||||
#endif
|
||||
|
||||
if (colliding) {
|
||||
ASSERT(s.len == 3);
|
||||
struct v2 *proto = arena_dry_push(scratch.arena, struct v2);
|
||||
{
|
||||
struct v2 *tmp = arena_push_array(scratch.arena, struct v2, 3);
|
||||
tmp[0] = s.a.p;
|
||||
tmp[1] = s.b.p;
|
||||
tmp[2] = s.c.p;
|
||||
}
|
||||
u32 proto_count = 3;
|
||||
|
||||
pen = V2(0, 0);
|
||||
f32 pen_len = F32_INFINITY;
|
||||
while (true) {
|
||||
pen = V2(0, 0);
|
||||
pen_len = F32_INFINITY;
|
||||
|
||||
/* Find dir from origin to closest edge */
|
||||
u32 pen_pe_index = 1;
|
||||
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];
|
||||
struct v2 pe = proto[pe_index];
|
||||
|
||||
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);
|
||||
|
||||
/* TODO: sq cmp */
|
||||
f32 pd_len = v2_len(pd);
|
||||
if (pd_len < pen_len) {
|
||||
pen_pe_index = pe_index;
|
||||
pen = pd;
|
||||
pen_len = pd_len;
|
||||
}
|
||||
}
|
||||
|
||||
/* Find new point in dir */
|
||||
m = menkowski_point(poly0, poly1, pen);
|
||||
|
||||
/* Check unique */
|
||||
/* TODO: Better */
|
||||
/* TODO: Epsilon or iteration limit */
|
||||
{
|
||||
b32 unique = true;
|
||||
for (u32 i = 0; i < proto_count; ++i) {
|
||||
if (v2_eq(m.p, proto[i])) {
|
||||
unique = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!unique) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* Insert point into prototype array */
|
||||
/* 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.p;
|
||||
}
|
||||
pen = v2_mul(v2_norm(pen), pen_len);
|
||||
}
|
||||
|
||||
abort:
|
||||
struct gjk_extended_result res = { 0 };
|
||||
res.colliding = colliding;
|
||||
res.colliding_pen = pen;
|
||||
res.final_simplex = s;
|
||||
scratch_end(scratch);
|
||||
return res;
|
||||
}
|
||||
|
||||
/* Returns 1 if winding forward, -1 if backward */
|
||||
i32 poly_get_winding_order(struct v2_array poly)
|
||||
{
|
||||
i32 res;
|
||||
if (poly.count >= 3) {
|
||||
struct v2 a = poly.points[0];
|
||||
struct v2 b = poly.points[1];
|
||||
struct v2 c = poly.points[2];
|
||||
if (v2_wedge(v2_sub(b, a), v2_sub(c, b)) > 0) {
|
||||
res = 1;
|
||||
} else {
|
||||
res = -1;
|
||||
}
|
||||
} else {
|
||||
res = -1;
|
||||
}
|
||||
return res;
|
||||
}
|
||||
#endif
|
||||
|
||||
22
src/util.h
22
src/util.h
@ -186,22 +186,28 @@ INLINE void sleep_frame(sys_timestamp_t last_frame_time, f64 target_dt)
|
||||
|
||||
/* TODO: Remove this */
|
||||
|
||||
struct simplex {
|
||||
u32 len;
|
||||
struct v2 a, b, c;
|
||||
struct gjk_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 gjk_result {
|
||||
struct gjk_extended_simplex {
|
||||
u32 len;
|
||||
struct gjk_menkowski_point a, b, c;
|
||||
};
|
||||
|
||||
struct gjk_extended_result {
|
||||
b32 colliding;
|
||||
struct simplex final_simplex;
|
||||
struct v2 colliding_pen;
|
||||
struct gjk_extended_simplex final_simplex;
|
||||
};
|
||||
|
||||
struct v2 poly_support(struct v2_array a, struct v2 dir);
|
||||
struct v2 menkowski_point(struct v2_array poly0, struct v2_array poly1, struct v2 dir);
|
||||
struct v2 perp_towards_point(struct v2 start, struct v2 end, struct v2 p);
|
||||
struct gjk_result gjk(struct v2_array poly0, struct v2_array poly1, u32 max_steps);
|
||||
struct v2 epa(struct v2_array poly0, struct v2_array poly1, struct simplex simplex);
|
||||
struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1);
|
||||
i32 poly_get_winding_order(struct v2_array poly);
|
||||
b32 gjk_boolean(struct v2_array poly0, struct v2_array poly1, u32 max_steps);
|
||||
struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array poly1, u32 max_steps);
|
||||
|
||||
#endif
|
||||
|
||||
Loading…
Reference in New Issue
Block a user