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

fix gjk hang

Browse Source
This commit is contained in:
jacob 2024-08-30 14:40:40 -05:00
parent 8fd92c55a1
commit 2fe3502cfc
3 changed files with 57 additions and 95 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/common.h
View File

@ -345,11 +345,20 @@ typedef i32 b32;
GLOBAL const u32 _f32_infinity_u32 = 0x7f800000; GLOBAL const u32 _f32_infinity_u32 = 0x7f800000;
GLOBAL const f32 *_f32_infinity = (f32 *)&_f32_infinity_u32; GLOBAL const f32 *_f32_infinity = (f32 *)&_f32_infinity_u32;
#define F32_INFINITY (*_f32_infinity)
GLOBAL const u64 _f64_infinity_u64 = 0x7ff0000000000000ULL; GLOBAL const u64 _f64_infinity_u64 = 0x7ff0000000000000ULL;
GLOBAL const f64 *_f64_infinity = (f64 *)&_f64_infinity_u64; GLOBAL const f64 *_f64_infinity = (f64 *)&_f64_infinity_u64;
#define F32_INFINITY (*_f32_infinity)
#define F64_INFINITY (*_f64_infinity) #define F64_INFINITY (*_f64_infinity)
GLOBAL const u32 _f32_nan_u32 = 0x7f800001;
GLOBAL const f32 *_f32_nan = (f32 *)&_f32_nan_u32;
#define F32_NAN (*_f32_nan)
GLOBAL const u64 _f64_nan_u64 = 0x7ff8000000000001;
GLOBAL const f64 *_f64_nan = (f64 *)&_f64_nan_u64;
#define F64_NAN (*_f64_nan)
#define F32_IS_NAN(x) (x != x) #define F32_IS_NAN(x) (x != x)
#define F64_IS_NAN(x) (x != x) #define F64_IS_NAN(x) (x != x)

12
src/user.c
View File

@ -1036,9 +1036,9 @@ INTERNAL void user_update(void)
{ {
f32 thickness = 2; f32 thickness = 2;
u32 line_color = colliding ? COLOR_WHITE: COLOR_YELLOW; u32 line_color = colliding ? COLOR_WHITE: COLOR_YELLOW;
u32 color_first = RGBA_32_F(1, 0, 0, 0.5); u32 color_first = RGBA_32_F(1, 0, 0, 0.75);
u32 color_second = RGBA_32_F(0, 1, 0, 0.5); u32 color_second = RGBA_32_F(0, 1, 0, 0.75);
u32 color_third = RGBA_32_F(0, 0, 1, 0.5); u32 color_third = RGBA_32_F(0, 0, 1, 0.75);
struct gjk_extended_simplex simplex = ent->simplex; struct gjk_extended_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 };
@ -1047,15 +1047,15 @@ INTERNAL void user_update(void)
if (simplex.len >= 1) { if (simplex.len >= 1) {
u32 color = simplex.len == 1 ? color_first : (simplex.len == 2 ? color_second : color_third); u32 color = simplex.len == 1 ? color_first : (simplex.len == 2 ? color_second : color_third);
draw_solid_circle(G.viewport_canvas, simplex_array.points[0], thickness * 2, color, 10); draw_solid_circle(G.viewport_canvas, simplex_array.points[0], thickness * 3, color, 10);
} }
if (simplex.len >= 2) { if (simplex.len >= 2) {
u32 color = simplex.len == 2 ? color_first : color_second; u32 color = simplex.len == 2 ? color_first : color_second;
draw_solid_circle(G.viewport_canvas, simplex_array.points[1], thickness * 2, color, 10); draw_solid_circle(G.viewport_canvas, simplex_array.points[1], thickness * 3, color, 10);
} }
if (simplex.len >= 3) { if (simplex.len >= 3) {
u32 color = color_first; u32 color = color_first;
draw_solid_circle(G.viewport_canvas, simplex_array.points[2], thickness * 2, color, 10); draw_solid_circle(G.viewport_canvas, simplex_array.points[2], thickness * 3, color, 10);
} }
if (simplex.len >= 2) { if (simplex.len >= 2) {
draw_solid_poly_line(G.viewport_canvas, simplex_array, simplex.len > 2, thickness, line_color); draw_solid_poly_line(G.viewport_canvas, simplex_array, simplex.len > 2, thickness, line_color);

59
src/util.c
View File

@ -371,27 +371,14 @@ struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array p
s.a = m; s.a = m;
s.len = 1; s.len = 1;
#if 0 struct v2 prev_point = V2(F32_NAN, F32_NAN);
/* 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) { while (true) {
/* Second point is support point towards origin */ /* Second point is support point towards origin */
if (s.len == 1) { if (s.len == 1) {
if (step++ >= max_steps) goto abort; if (step++ >= max_steps) goto abort;
dir = v2_neg(s.a.p); dir = v2_neg(s.a.p);
m = menkowski_point(poly0, poly1, dir); m = menkowski_point(poly0, poly1, dir);
if (v2_eq(s.a.p, m.p)) { if (v2_eq(m.p, s.a.p)) {
colliding = false; colliding = false;
break; break;
} }
@ -405,7 +392,7 @@ struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array p
if (step++ >= max_steps) goto abort; if (step++ >= max_steps) goto abort;
dir = perp_towards_point(s.a.p, s.b.p, V2(0, 0)); dir = perp_towards_point(s.a.p, s.b.p, V2(0, 0));
m = menkowski_point(poly0, poly1, dir); m = menkowski_point(poly0, poly1, dir);
if (v2_eq(m.p, s.a.p) || v2_eq(m.p, s.b.p)) { if (v2_eq(m.p, prev_point) || v2_eq(m.p, s.a.p) || v2_eq(m.p, s.b.p) || v2_eq(m.p, s.c.p)) {
colliding = false; colliding = false;
break; break;
} }
@ -417,33 +404,6 @@ struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array p
if (step++ >= max_steps) goto abort; if (step++ >= max_steps) goto abort;
#if 0
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;
}
}
}
#else
b32 rab = v2_dot(perp_towards_point(s.a.p, s.b.p, s.c.p), v2_neg(s.a.p)) < 0; 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 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; b32 rbc = v2_dot(perp_towards_point(s.b.p, s.c.p, s.a.p), v2_neg(s.b.p)) < 0;
@ -452,7 +412,8 @@ struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array p
break; break;
} }
/* Simplex vornoi regions */ /* Remove point or edge based on vornoi regions */
{
b32 ra = rab && rac; b32 ra = rab && rac;
b32 rb = rab && rbc; b32 rb = rab && rbc;
b32 rc = rac && rbc; b32 rc = rac && rbc;
@ -484,18 +445,10 @@ struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array p
s.a = s.c; s.a = s.c;
s.len = 1; s.len = 1;
} }
#endif
} }
#if 0 prev_point = m.p;
b32 epa_specific = false;
struct v2 epa_specific_dir = V2(0, 0);
if (!colliding) {
epa_specific = true;
epa_specific_dir =
} }
#endif
if (colliding) { if (colliding) {
ASSERT(s.len == 3); ASSERT(s.len == 3);