From 987842e13027993f7eb3d52d63b6f84a8ee51295 Mon Sep 17 00:00:00 2001 From: jacob Date: Fri, 30 Aug 2024 18:22:48 -0500 Subject: [PATCH] working depth points for colliding gjk --- src/user.c | 4 +- src/util.c | 314 ++++++++++++++--------------------------------------- 2 files changed, 81 insertions(+), 237 deletions(-) diff --git a/src/user.c b/src/user.c index 853e0410..42a6a876 100644 --- a/src/user.c +++ b/src/user.c @@ -1035,8 +1035,8 @@ INTERNAL void user_update(void) /* Draw point */ { - u32 color = COLOR_RED; - f32 thickness = 4; + u32 color = COLOR_PURPLE; + f32 thickness = 5; struct v2 point = xform_mul_v2(G.world_view, ent->point); draw_solid_circle(G.viewport_canvas, point, thickness, color, 10); } diff --git a/src/util.c b/src/util.c index 2f481df6..d3a11533 100644 --- a/src/util.c +++ b/src/util.c @@ -183,177 +183,6 @@ b32 gjk_boolean(struct v2_array poly0, struct v2_array poly1, u32 max_steps) return colliding; } - - - - - - - -#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(); - b32 colliding = false; - struct v2 pen = V2(0, 0); - u32 step = 0; - - /* 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 menkowski_result m = menkowski_point(poly0, poly1, dir); - s.a = m.p; - s.a0 = m.p0; - s.a1 = m.p1; - s.len = 1; - - /* Second point is support point towards origin */ - if (step++ >= max_steps) goto abort; - { - dir = v2_neg(s.a); - m = menkowski_point(poly0, poly1, dir); - if (v2_dot(dir, m.p) >= 0) { - s.b = s.a; - s.b0 = s.a0; - s.b1 = s.a1; - s.a = m.p; - s.a0 = m.p0; - s.a1 = m.p1; - s.len = 2; - } - } - - if (s.len == 2) { - while (true) { - if (step++ >= max_steps) goto abort; - - /* Third point is support point in direction of line normal towards origin */ - dir = perp_towards_point(s.a, s.b, V2(0, 0)); - m = menkowski_point(poly0, poly1, dir); - if (v2_dot(dir, m.p) < 0) { - colliding = false; - break; - } - - s.c = s.b; - s.c0 = s.b0; - s.c1 = s.b1; - s.b = s.a; - s.b0 = s.a0; - s.b1 = s.a1; - s.a = m.p; - s.a0 = m.p0; - s.a1 = m.p1; - s.len = 3; - - dir = v2_neg(perp_towards_point(s.a, s.b, s.c)); /* Normal dir 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) */ - 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, a_to_origin) >= 0) { - /* Point is in region ac, remove b from simplex */ - s.b = s.c; - s.len = 2; - } else { - /* Point is in simplex */ - colliding = true; - break; - } - } - } - } - - if (colliding) { /* Calculate penetration depth using epa */ - ASSERT(s.len == 3); - struct v2 *proto = arena_dry_push(scratch.arena, struct v2); - { - struct v2 *tmp = arena_push_array(scratch.arena, struct v2, 3); - tmp[0] = s.a; - tmp[1] = s.b; - tmp[2] = s.c; - } - u32 proto_count = 3; - - 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 */ - m = menkowski_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(m.p, proto[i])) { - unique = false; - break; - } - } - if (!unique) { - break; - } - } - - /* Insert point into prototype array */ - /* FIXME: Preserve winding order */ - 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] = m.p; - } - pen = v2_mul(v2_norm(pen), pen_len); - } - - abort: - struct gjk_extended_result res = { 0 }; - res.colliding = colliding; - res.colliding_pen = pen; - res.final_simplex = s; - scratch_end(scratch); - return res; -} - -#else - 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(); @@ -377,24 +206,22 @@ 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; - dir = v2_neg(s.a.p); - m = menkowski_point(poly0, poly1, dir); + m = menkowski_point(poly0, poly1, v2_neg(s.a.p)); if (v2_eq(m.p, s.a.p)) { - colliding = false; break; } s.b = s.a; s.a = m; s.len = 2; + + /* Third point is support point in direction of line normal towards origin */ + dir = perp_towards_point(s.a.p, s.b.p, V2(0, 0)); } - /* Third point is support point in direction of line normal towards origin */ 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, 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; } @@ -405,76 +232,89 @@ struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array p if (step++ >= max_steps) goto abort; - 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; + struct v2 rab_dir = v2_neg(perp_towards_point(s.a.p, s.b.p, s.c.p)); + struct v2 rac_dir = v2_neg(perp_towards_point(s.a.p, s.c.p, s.b.p)); + struct v2 rbc_dir = v2_neg(perp_towards_point(s.b.p, s.c.p, s.a.p)); + + b32 rab = v2_dot(rab_dir, v2_neg(s.a.p)) >= 0; + b32 rac = v2_dot(rac_dir, v2_neg(s.a.p)) >= 0; + b32 rbc = v2_dot(rbc_dir, v2_neg(s.b.p)) >= 0; + if (!rab && !rac && !rbc) { colliding = true; break; } - /* 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; - } + /* Remove point or edge and determine next direction 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 */ + dir = rab_dir; + s.len = 2; + } else if (rac) { + /* Remove b */ + dir = rac_dir; + s.b = s.c; + s.len = 2; + } else if (rbc) { + /* Remove a */ + dir = rbc_dir; + 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; } prev_point = m.p; } if (colliding) { - ASSERT(s.len == 3); - struct v2 *proto = arena_dry_push(scratch.arena, struct v2); - { - struct v2 *tmp = arena_push_array(scratch.arena, struct v2, 3); - tmp[0] = s.a.p; - tmp[1] = s.b.p; - tmp[2] = s.c.p; - } - u32 proto_count = 3; + struct gjk_menkowski_point *proto = arena_dry_push(scratch.arena, struct gjk_menkowski_point); + u32 proto_count = 0; + + { + 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; + } + } - struct v2 pen = V2(0, 0); - f32 pen_len = F32_INFINITY; while (true) { - pen = V2(0, 0); - pen_len = F32_INFINITY; + if (step++ >= max_steps) goto abort; + + struct v2 pen = V2(0, 0); + f32 pen_len = F32_INFINITY; /* Find dir from origin to closest edge */ + u32 pen_ps_index = 0; 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 ps = proto[ps_index].p; + struct v2 pe = proto[pe_index].p; struct v2 vse = v2_sub(pe, ps); struct v2 vso = v2_neg(ps); @@ -485,6 +325,7 @@ struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array p /* TODO: sq cmp */ f32 pd_len = v2_len(pd); if (pd_len < pen_len) { + pen_ps_index = ps_index; pen_pe_index = pe_index; pen = pd; pen_len = pd_len; @@ -500,12 +341,15 @@ struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array p { b32 unique = true; for (u32 i = 0; i < proto_count; ++i) { - if (v2_eq(m.p, proto[i])) { + if (v2_eq(m.p, proto[i].p)) { unique = false; break; } } if (!unique) { + s.a = proto[pen_ps_index]; + s.b = proto[pen_pe_index]; + s.len = 2; break; } } @@ -519,10 +363,12 @@ struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array p u32 shift_to = i; proto[shift_to] = proto[shift_from]; } - proto[pen_pe_index] = m.p; + proto[pen_pe_index] = m; } - pen = v2_mul(v2_norm(pen), pen_len); - } else { + + } + + { if (s.len == 1) { shape0_p = s.a.p0; shape1_p = s.a.p1; @@ -546,7 +392,7 @@ struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array p } } - abort: +abort: struct gjk_extended_result res = { 0 }; res.colliding = colliding; res.p0 = shape0_p; @@ -555,5 +401,3 @@ struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array p scratch_end(scratch); return res; } - -#endif