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

working gjk penetration direction test

Browse Source
This commit is contained in:
jacob 2024-09-03 12:04:22 -05:00
parent d735ff8fc8
commit beccc17b7f
4 changed files with 283 additions and 101 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/game.c
View File

@ -451,7 +451,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
}
/* Movement */
if (v2_len_squared(move) > 1) {
if (v2_len_sq(move) > 1) {
/* Cap movement vector magnitude at 1 */
move = v2_norm(move);
}

4
src/math.h
View File

@ -630,7 +630,7 @@ INLINE f32 v2_len(struct v2 a)
return math_sqrt(a.x * a.x + a.y * a.y);
}
INLINE f32 v2_len_squared(struct v2 a)
INLINE f32 v2_len_sq(struct v2 a)
{
return a.x * a.x + a.y * a.y;
}
@ -649,7 +649,7 @@ INLINE struct v2 v2_perp_ccw(struct v2 a)
INLINE struct v2 v2_norm(struct v2 a)
{
f32 l = v2_len_squared(a);
f32 l = v2_len_sq(a);
if (l != 0) {
f32 denom = 1.f / math_sqrt(l);
a.x *= denom;

348
src/util.c
View File

@ -45,31 +45,6 @@ struct string util_file_name_from_path(struct string path)
/* TODO: Remove / move this */
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 gjk_menkowski_point menkowski_point(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 v2 perp_towards_point(struct v2 start, struct v2 end, struct v2 p)
{
struct v2 perp = v2_perp_cw(v2_sub(end, start));
@ -77,21 +52,11 @@ struct v2 perp_towards_point(struct v2 start, struct v2 end, struct v2 p)
return v2_mul(perp, sign);
}
/* TODO: Remove this (debugging) */
struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1)
struct v2 perp_towards_dir(struct v2 start, struct v2 end, struct v2 dir)
{
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;
struct v2 perp = v2_perp_cw(v2_sub(end, start));
i32 sign = 1 - ((v2_dot(perp, v2_add(dir, start)) < 0) << 1);
return v2_mul(perp, sign);
}
/* Returns 1 if winding forward, -1 if backward */
@ -119,40 +84,55 @@ i32 poly_get_winding_order(struct v2_array poly)
b32 gjk_boolean(struct v2_array poly0, struct v2_array poly1, u32 max_steps)
INTERNAL struct v2 poly_support(struct v2_array a, struct v2 dir)
{
b32 colliding = false;
u32 step = 0;
/* 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;
}
/* Simplex */
INTERNAL struct v2 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)));
}
INTERNAL struct gjk_menkowski_point extended_menkowski_point(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;
}
b32 gjk_boolean(struct v2_array poly0, struct v2_array poly1)
{
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_sub(poly1.points[0], poly0.points[0])).p;
/* 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]));
/* 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).p;
struct v2 p = menkowski_point(poly0, poly1, dir);
if (v2_dot(dir, p) >= 0) {
s.b = s.a;
s.a = p;
valid_line = true;
}
}
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).p;
dir = perp_towards_point(s.a, s.b, V2(0, 0));
p = menkowski_point(poly0, poly1, dir);
if (v2_dot(dir, p) < 0) {
colliding = false;
/* New point did not cross origin, collision impossible */
break;
}
@ -160,29 +140,39 @@ b32 gjk_boolean(struct v2_array poly0, struct v2_array poly1, u32 max_steps)
s.b = s.a;
s.a = p;
dir = v2_neg(perp_towards_point(s.a, s.b, s.c)); /* Normal dir of ab pointing away from c */
dir = v2_neg(perp_towards_point(s.a, s.b, s.c)); /* 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_neg(perp_towards_point(s.a, s.c, s.b)); /* Normal dir of ac pointing away from b */
dir = v2_neg(perp_towards_point(s.a, s.c, s.b)); /* 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 */
colliding = true;
break;
return true;
}
}
}
}
abort:
return colliding;
return false;
}
#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();
@ -200,7 +190,7 @@ struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array p
/* 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);
struct gjk_menkowski_point m = extended_menkowski_point(poly0, poly1, dir);
s.a = m;
s.len = 1;
@ -208,7 +198,7 @@ struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array p
/* Second point is support point towards origin */
if (s.len == 1) {
if (step++ >= max_steps) goto abort;
m = menkowski_point(poly0, poly1, v2_neg(s.a.p));
m = extended_menkowski_point(poly0, poly1, v2_neg(s.a.p));
if (v2_eq(m.p, s.a.p)) {
break;
}
@ -222,7 +212,7 @@ struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array p
}
if (step++ >= max_steps) goto abort;
m = menkowski_point(poly0, poly1, dir);
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 */
@ -292,26 +282,25 @@ struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array p
{
struct gjk_menkowski_point *tmp = arena_push_array(scratch.arena, struct gjk_menkowski_point, 3);
proto_count = s.len;
if (proto_count >= 1) {
tmp[0] = s.a;
}
if (proto_count >= 2) {
tmp[1] = s.b;
}
if (proto_count >= 3) {
tmp[2] = s.c;
}
}
}
while (true) {
if (step++ >= max_steps) goto abort;
struct v2 pen = V2(0, 0);
f32 pen_len = F32_INFINITY;
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 = 1;
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;
@ -324,18 +313,22 @@ struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array p
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) {
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 = pd_len;
pen_len_sq = pd_len_sq;
}
}
/* Find new point in dir */
m = menkowski_point(poly0, poly1, pen);
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 */
@ -350,14 +343,11 @@ struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array p
}
}
if (!unique) {
s.a = proto[pen_ps_index];
s.b = proto[pen_pe_index];
s.len = 2;
break;
}
}
/* Insert point into prototype array */
/* Insert point into prototype */
/* FIXME: Preserve winding order */
arena_push(scratch.arena, struct v2);
++proto_count;
@ -403,3 +393,195 @@ abort:
scratch_end(scratch);
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
/* 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 = extended_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;
}

4
src/util.h
View File

@ -206,11 +206,11 @@ struct gjk_extended_result {
struct gjk_extended_simplex final_simplex;
};
struct v2 poly_support(struct v2_array a, struct v2 dir);
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_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);
b32 gjk_boolean(struct v2_array poly0, struct v2_array poly1);
struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array poly1, u32 max_steps);
#endif