fix gjk hang
This commit is contained in:
parent
8fd92c55a1
commit
2fe3502cfc
21
src/common.h
21
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)
|
||||
|
||||
12
src/user.c
12
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);
|
||||
|
||||
119
src/util.c
119
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);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user