working epa test
This commit is contained in:
parent
818ffa7eba
commit
2150d0e3c4
@ -55,11 +55,6 @@ struct entity_store {
|
||||
|
||||
|
||||
|
||||
/* TODO: Remove this */
|
||||
struct simplex {
|
||||
struct v2 a, b, c;
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
@ -98,6 +93,7 @@ struct entity {
|
||||
|
||||
/* TODO: Remove this (testing) */
|
||||
b32 colliding;
|
||||
struct entity_handle colliding_with;
|
||||
struct simplex simplex;
|
||||
struct v2 pen;
|
||||
|
||||
|
||||
184
src/game.c
184
src/game.c
@ -204,182 +204,6 @@ INTERNAL void spawn_test_entities(void)
|
||||
}
|
||||
}
|
||||
|
||||
/* ========================== *
|
||||
* Collision test
|
||||
* ========================== */
|
||||
|
||||
INTERNAL struct v2 poly_furthest_point(struct v2_array a, struct v2 dir)
|
||||
{
|
||||
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 poly_support_point(struct v2_array poly0, struct v2_array poly1, struct v2 dir)
|
||||
{
|
||||
return v2_sub(poly_furthest_point(poly0, dir), poly_furthest_point(poly1, v2_neg(dir)));
|
||||
}
|
||||
|
||||
INTERNAL struct v2 normal_towards_point(struct v2 start, struct v2 end, struct v2 p)
|
||||
{
|
||||
struct v2 dir = v2_norm(v2_perp_cw(v2_sub(end, start)));
|
||||
i32 sign = 1 - ((v2_dot(dir, v2_sub(p, start)) < 0) << 1);
|
||||
return v2_mul(dir, sign);
|
||||
}
|
||||
|
||||
struct gjk_result {
|
||||
b32 colliding;
|
||||
struct simplex final_simplex;
|
||||
};
|
||||
|
||||
INTERNAL struct gjk_result gjk(struct v2_array poly0, struct v2_array poly1)
|
||||
{
|
||||
struct v2 poly0_center = math_poly_center(poly0);
|
||||
struct v2 poly1_center = math_poly_center(poly1);
|
||||
|
||||
/* Simplex */
|
||||
struct simplex s = { 0 };
|
||||
u32 s_len = 0;
|
||||
|
||||
/* Append first point to simplex */
|
||||
struct v2 dir = v2_norm(v2_sub(poly1_center, poly0_center));
|
||||
s.c = poly_support_point(poly0, poly1, dir);
|
||||
s_len = 1;
|
||||
|
||||
dir = v2_norm(v2_neg(s.c)); /* Next point is towards origin */
|
||||
|
||||
b32 colliding = false;
|
||||
while (true) {
|
||||
/* Determine support point */
|
||||
struct v2 p = poly_support_point(poly0, poly1, dir);
|
||||
if (v2_dot(dir, p) < 0) {
|
||||
/* Point did not cross origin */
|
||||
colliding = false;
|
||||
break;
|
||||
}
|
||||
|
||||
if (s_len < 3) {
|
||||
/* Line case */
|
||||
/* Next dir is line normal towards origin */
|
||||
if (s_len == 1) {
|
||||
s.b = p;
|
||||
} else if (s_len == 2) {
|
||||
s.a = p;
|
||||
}
|
||||
++s_len;
|
||||
dir = normal_towards_point(s.a, s.b, V2(0, 0));
|
||||
} else {
|
||||
/* Triangle case */
|
||||
s.c = s.b;
|
||||
s.b = s.a;
|
||||
s.a = p;
|
||||
|
||||
/* Ensure point is unique */
|
||||
if (v2_eq(s.a, s.b)
|
||||
|| v2_eq(s.b, s.c)
|
||||
|| v2_eq(s.a, s.c)) {
|
||||
colliding = false;
|
||||
break;
|
||||
}
|
||||
|
||||
struct v2 a_to_origin_rel = v2_neg(s.a);
|
||||
|
||||
dir = v2_neg(normal_towards_point(s.a, s.b, s.c)); /* Normal dir of ab pointing away from c */
|
||||
if (v2_dot(dir, a_to_origin_rel) >= 0) {
|
||||
/* Point is in region ab, remove c from simplex */
|
||||
} else {
|
||||
dir = v2_neg(normal_towards_point(s.a, s.c, s.b)); /* Normal dir of ac pointing away from b */
|
||||
if (v2_dot(dir, a_to_origin_rel) >= 0) {
|
||||
/* Point is in region ac, remove b from simplex */
|
||||
s.b = s.c;
|
||||
} else {
|
||||
/* Point must be in simplex */
|
||||
colliding = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return (struct gjk_result) {
|
||||
.colliding = colliding,
|
||||
.final_simplex = s
|
||||
};
|
||||
}
|
||||
|
||||
INTERNAL struct v2 epa(struct v2_array poly0, struct v2_array poly1, struct simplex simplex)
|
||||
{
|
||||
struct temp_arena scratch = scratch_begin_no_conflict();
|
||||
|
||||
struct v2 *poly = 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 poly_count = 3;
|
||||
|
||||
struct v2 pen = V2(0, 0);
|
||||
f32 pen_len = F32_INFINITY;
|
||||
while (true) {
|
||||
/* Find dir from origin to closest edge */
|
||||
struct v2 ps = V2(0, 0);
|
||||
struct v2 pe = V2(0, 0);
|
||||
for (u32 i = 0; i < poly_count; ++i) {
|
||||
ps = poly[i];
|
||||
pe = poly[(i < poly_count - 1) ? i : 0];
|
||||
|
||||
struct v2 vso = v2_neg(ps);
|
||||
struct v2 vse = v2_norm(v2_sub(pe, ps));
|
||||
|
||||
f32 dot = v2_dot(vso, vse);
|
||||
|
||||
struct v2 vsd = v2_mul(vse, dot);
|
||||
struct v2 pd = v2_add(ps, vsd);
|
||||
|
||||
/* TODO: sq cmp */
|
||||
f32 pd_len = v2_len(pd);
|
||||
if (pd_len < pen_len) {
|
||||
pen = pd;
|
||||
pen_len = pd_len;
|
||||
}
|
||||
}
|
||||
|
||||
/* Find new point in dir */
|
||||
#if 0
|
||||
/* FIXME: Maintain convexity */
|
||||
struct v2 p = poly_support_point(poly0, poly1, pen);
|
||||
if (v2_eq(p, ps) || v2_eq(p, pe)) {
|
||||
break;
|
||||
} else {
|
||||
*arena_push(scratch.arena, struct v2) = p;
|
||||
++poly_count;
|
||||
}
|
||||
#else
|
||||
/* FIXME: Maintain convexity */
|
||||
struct v2 p = poly_support_point(poly0, poly1, pen);
|
||||
if (v2_eq(p, ps) || v2_eq(p, pe)) {
|
||||
break;
|
||||
} else {
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
pen = v2_mul(v2_norm(pen), pen_len);
|
||||
|
||||
scratch_end(scratch);
|
||||
return pen;
|
||||
}
|
||||
|
||||
/* ========================== *
|
||||
* Update
|
||||
* ========================== */
|
||||
@ -905,6 +729,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
|
||||
b32 colliding = false;
|
||||
struct simplex simplex = { 0 };
|
||||
struct v2 pen = 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;
|
||||
@ -927,13 +752,13 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
|
||||
|
||||
struct gjk_result res = gjk(e0_poly, e1_poly);
|
||||
colliding = res.colliding;
|
||||
colliding_with = e1->handle;
|
||||
simplex = res.final_simplex;
|
||||
pen = epa(e0_poly, e1_poly, simplex);
|
||||
|
||||
if (colliding) {
|
||||
/* Pen movement test */
|
||||
pen = epa(e0_poly, e1_poly, simplex);
|
||||
|
||||
#if 0
|
||||
#if 1
|
||||
{
|
||||
struct xform xf = e1_xf;
|
||||
|
||||
@ -950,6 +775,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
|
||||
}
|
||||
|
||||
e0->colliding = colliding;
|
||||
e0->colliding_with = colliding_with;
|
||||
e0->simplex = simplex;
|
||||
e0->pen = pen;
|
||||
}
|
||||
|
||||
63
src/user.c
63
src/user.c
@ -391,6 +391,25 @@ INTERNAL void debug_draw_movement(struct entity *ent)
|
||||
draw_solid_arrow_ray(G.viewport_canvas, pos, vel_ray, thickness, arrow_len, color_vel);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
INTERNAL void user_update(void)
|
||||
{
|
||||
struct temp_arena scratch = scratch_begin_no_conflict();
|
||||
@ -957,9 +976,49 @@ INTERNAL void user_update(void)
|
||||
/* Draw player collision */
|
||||
if (entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED)) {
|
||||
b32 colliding = ent->colliding;
|
||||
struct entity *e1 = entity_from_handle(store, ent->colliding_with);
|
||||
|
||||
/* Draw menkowski */
|
||||
{
|
||||
struct quad ent_quad;
|
||||
struct v2_array ent_poly;
|
||||
struct quad e1_quad;
|
||||
struct v2_array e1_poly;
|
||||
{
|
||||
{
|
||||
struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, ent->sprite);
|
||||
struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("shape"), ent->animation_frame);
|
||||
ent_quad = xform_mul_quad(ent->sprite_local_xform, quad_from_rect(slice.rect));
|
||||
ent_quad = xform_mul_quad(entity_get_xform(ent), ent_quad);
|
||||
ent_poly = (struct v2_array) {
|
||||
.count = ARRAY_COUNT(ent_quad.e),
|
||||
.points = ent_quad.e
|
||||
};
|
||||
}
|
||||
{
|
||||
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(entity_get_xform(e1), e1_quad);
|
||||
e1_poly = (struct v2_array) {
|
||||
.count = ARRAY_COUNT(e1_quad.e),
|
||||
.points = e1_quad.e
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
u32 color = RGBA_32_F(0, 0, 0.75, 1);
|
||||
f32 thickness = 2;
|
||||
(UNUSED)thickness;
|
||||
|
||||
struct v2_array m = menkowski(temp.arena, ent_poly, e1_poly);
|
||||
for (u64 i = 0; i < m.count; ++i) m.points[i] = xform_mul_v2(G.world_view, m.points[i]);
|
||||
draw_solid_poly_line(G.viewport_canvas, m, true, thickness, color);
|
||||
//draw_solid_poly(G.viewport_canvas, m, color);
|
||||
}
|
||||
|
||||
/* Draw simplex */
|
||||
{
|
||||
if (colliding) {
|
||||
f32 thickness = 2;
|
||||
u32 color = colliding ? COLOR_WHITE: COLOR_YELLOW;
|
||||
struct simplex simplex = ent->simplex;
|
||||
@ -970,7 +1029,7 @@ INTERNAL void user_update(void)
|
||||
}
|
||||
|
||||
/* Draw pen */
|
||||
{
|
||||
if (colliding) {
|
||||
f32 thickness = 2;
|
||||
u32 color = COLOR_RED;
|
||||
|
||||
|
||||
234
src/util.c
234
src/util.c
@ -34,3 +34,237 @@ struct string util_file_name_from_path(struct string path)
|
||||
};
|
||||
}
|
||||
#endif
|
||||
|
||||
/* ========================== *
|
||||
* Collision test
|
||||
* ========================== */
|
||||
|
||||
#include "math.h"
|
||||
#include "arena.h"
|
||||
#include "scratch.h"
|
||||
|
||||
/* TODO: Remove / move this */
|
||||
|
||||
struct v2 poly_furthest_point(struct v2_array a, struct v2 dir)
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
struct v2 poly_support_point(struct v2_array poly0, struct v2_array poly1, struct v2 dir)
|
||||
{
|
||||
return v2_sub(poly_furthest_point(poly0, dir), poly_furthest_point(poly1, v2_neg(dir)));
|
||||
}
|
||||
|
||||
struct v2 normal_towards_point(struct v2 start, struct v2 end, struct v2 p)
|
||||
{
|
||||
struct v2 dir = v2_norm(v2_perp_cw(v2_sub(end, start)));
|
||||
i32 sign = 1 - ((v2_dot(dir, v2_sub(p, start)) < 0) << 1);
|
||||
return v2_mul(dir, sign);
|
||||
}
|
||||
|
||||
struct gjk_result gjk(struct v2_array poly0, struct v2_array poly1)
|
||||
{
|
||||
struct v2 poly0_center = math_poly_center(poly0);
|
||||
struct v2 poly1_center = math_poly_center(poly1);
|
||||
|
||||
/* Simplex */
|
||||
struct simplex s = { 0 };
|
||||
u32 s_len = 0;
|
||||
|
||||
/* Append first point to simplex */
|
||||
struct v2 dir = v2_norm(v2_sub(poly1_center, poly0_center));
|
||||
s.c = poly_support_point(poly0, poly1, dir);
|
||||
s_len = 1;
|
||||
|
||||
dir = v2_norm(v2_neg(s.c)); /* Next point is towards origin */
|
||||
|
||||
b32 colliding = false;
|
||||
while (true) {
|
||||
/* Determine support point */
|
||||
struct v2 p = poly_support_point(poly0, poly1, dir);
|
||||
if (v2_dot(dir, p) < 0) {
|
||||
/* Point did not cross origin */
|
||||
colliding = false;
|
||||
break;
|
||||
}
|
||||
|
||||
if (s_len < 3) {
|
||||
/* Line case */
|
||||
/* Next dir is line normal towards origin */
|
||||
if (s_len == 1) {
|
||||
s.b = p;
|
||||
} else if (s_len == 2) {
|
||||
s.a = p;
|
||||
}
|
||||
++s_len;
|
||||
dir = normal_towards_point(s.a, s.b, V2(0, 0));
|
||||
} else {
|
||||
/* Triangle case */
|
||||
s.c = s.b;
|
||||
s.b = s.a;
|
||||
s.a = p;
|
||||
|
||||
/* Ensure point is unique */
|
||||
/* TODO: Is this necessary? */
|
||||
if (v2_eq(s.a, s.b)
|
||||
|| v2_eq(s.b, s.c)
|
||||
|| v2_eq(s.a, s.c)) {
|
||||
colliding = false;
|
||||
break;
|
||||
}
|
||||
|
||||
struct v2 a_to_origin_rel = v2_neg(s.a);
|
||||
|
||||
dir = v2_neg(normal_towards_point(s.a, s.b, s.c)); /* Normal dir of ab pointing away from c */
|
||||
if (v2_dot(dir, a_to_origin_rel) >= 0) {
|
||||
/* Point is in region ab, remove c from simplex */
|
||||
} else {
|
||||
dir = v2_neg(normal_towards_point(s.a, s.c, s.b)); /* Normal dir of ac pointing away from b */
|
||||
if (v2_dot(dir, a_to_origin_rel) >= 0) {
|
||||
/* Point is in region ac, remove b from simplex */
|
||||
s.b = s.c;
|
||||
} else {
|
||||
/* Point must be in simplex */
|
||||
colliding = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return (struct gjk_result)
|
||||
{
|
||||
.colliding = colliding,
|
||||
.final_simplex = s
|
||||
};
|
||||
}
|
||||
|
||||
struct v2 epa(struct v2_array poly0, struct v2_array poly1, struct simplex simplex)
|
||||
{
|
||||
struct temp_arena scratch = scratch_begin_no_conflict();
|
||||
|
||||
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;
|
||||
|
||||
struct v2 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 */
|
||||
#if 1
|
||||
/* FIXME: Ensure convexity */
|
||||
|
||||
struct v2 p = poly_support_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(p, proto[i])) {
|
||||
unique = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!unique) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* Insert point into prototype array */
|
||||
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 = poly_support_point(poly0, poly1, pen);
|
||||
if (v2_eq(p, ps) || v2_eq(p, pe)) {
|
||||
break;
|
||||
} else {
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
pen = v2_mul(v2_norm(pen), pen_len);
|
||||
|
||||
scratch_end(scratch);
|
||||
return pen;
|
||||
}
|
||||
|
||||
/* 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;
|
||||
}
|
||||
}
|
||||
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 = poly_support_point(poly0, poly1, dir);
|
||||
if (res.count == 0 || !v2_eq(p, res.points[res.count - 1])) {
|
||||
*arena_push(arena, struct v2) = p;
|
||||
++res.count;
|
||||
}
|
||||
}
|
||||
return res;
|
||||
#endif
|
||||
}
|
||||
|
||||
22
src/util.h
22
src/util.h
@ -180,4 +180,26 @@ INLINE void sleep_frame(sys_timestamp_t last_frame_time, f64 target_dt)
|
||||
}
|
||||
}
|
||||
|
||||
/* ========================== *
|
||||
* Collision testing
|
||||
* ========================== */
|
||||
|
||||
/* TODO: Remove this */
|
||||
|
||||
struct simplex {
|
||||
struct v2 a, b, c;
|
||||
};
|
||||
|
||||
struct gjk_result {
|
||||
b32 colliding;
|
||||
struct simplex final_simplex;
|
||||
};
|
||||
|
||||
struct v2 poly_furthest_point(struct v2_array a, struct v2 dir);
|
||||
struct v2 poly_support_point(struct v2_array poly0, struct v2_array poly1, struct v2 dir);
|
||||
struct v2 normal_towards_point(struct v2 start, struct v2 end, struct v2 p);
|
||||
struct gjk_result gjk(struct v2_array poly0, struct v2_array poly1);
|
||||
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);
|
||||
|
||||
#endif
|
||||
|
||||
Loading…
Reference in New Issue
Block a user