From 2fe3502cfc6629b4ec9874b0fdcc90ee5518fa56 Mon Sep 17 00:00:00 2001 From: jacob Date: Fri, 30 Aug 2024 14:40:40 -0500 Subject: [PATCH] fix gjk hang --- src/common.h | 21 ++++++--- src/user.c | 12 +++--- src/util.c | 119 ++++++++++++++++----------------------------------- 3 files changed, 57 insertions(+), 95 deletions(-) diff --git a/src/common.h b/src/common.h index 16d3e78f..2ab5dfa0 100644 --- a/src/common.h +++ b/src/common.h @@ -343,12 +343,21 @@ typedef i32 b32; #define I32_MIN ((i32)-0x80000000) #define I64_MIN ((i64)-0x8000000000000000LL) -GLOBAL const u32 _f32_infinity_u32 = 0x7f800000; -GLOBAL const f32 *_f32_infinity = (f32 *)&_f32_infinity_u32; -GLOBAL const u64 _f64_infinity_u64 = 0x7ff0000000000000ULL; -GLOBAL const f64 *_f64_infinity = (f64 *)&_f64_infinity_u64; -#define F32_INFINITY (*_f32_infinity) -#define F64_INFINITY (*_f64_infinity) +GLOBAL const u32 _f32_infinity_u32 = 0x7f800000; +GLOBAL const f32 *_f32_infinity = (f32 *)&_f32_infinity_u32; +#define F32_INFINITY (*_f32_infinity) + +GLOBAL const u64 _f64_infinity_u64 = 0x7ff0000000000000ULL; +GLOBAL const f64 *_f64_infinity = (f64 *)&_f64_infinity_u64; +#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 F64_IS_NAN(x) (x != x) diff --git a/src/user.c b/src/user.c index 352949cc..a56da567 100644 --- a/src/user.c +++ b/src/user.c @@ -1036,9 +1036,9 @@ INTERNAL void user_update(void) { 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); + u32 color_first = RGBA_32_F(1, 0, 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); struct gjk_extended_simplex simplex = ent->simplex; 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) { 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) { 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) { 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) { draw_solid_poly_line(G.viewport_canvas, simplex_array, simplex.len > 2, thickness, line_color); diff --git a/src/util.c b/src/util.c index eb59afad..9056cccf 100644 --- a/src/util.c +++ b/src/util.c @@ -371,27 +371,14 @@ struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array p 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 - + struct v2 prev_point = V2(F32_NAN, F32_NAN); 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)) { + if (v2_eq(m.p, s.a.p)) { colliding = false; 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; 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)) { + 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; 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 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 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; @@ -452,50 +412,43 @@ struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array p 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; + /* Remove point or edge 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 */ - 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; + 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; + } } -#endif - } -#if 0 - b32 epa_specific = false; - struct v2 epa_specific_dir = V2(0, 0); - - if (!colliding) { - epa_specific = true; - epa_specific_dir = + prev_point = m.p; } -#endif if (colliding) { ASSERT(s.len == 3);