working depth points for colliding gjk
This commit is contained in:
parent
ed1a788821
commit
987842e130
@ -1035,8 +1035,8 @@ INTERNAL void user_update(void)
|
|||||||
|
|
||||||
/* Draw point */
|
/* Draw point */
|
||||||
{
|
{
|
||||||
u32 color = COLOR_RED;
|
u32 color = COLOR_PURPLE;
|
||||||
f32 thickness = 4;
|
f32 thickness = 5;
|
||||||
struct v2 point = xform_mul_v2(G.world_view, ent->point);
|
struct v2 point = xform_mul_v2(G.world_view, ent->point);
|
||||||
draw_solid_circle(G.viewport_canvas, point, thickness, color, 10);
|
draw_solid_circle(G.viewport_canvas, point, thickness, color, 10);
|
||||||
}
|
}
|
||||||
|
|||||||
250
src/util.c
250
src/util.c
@ -183,177 +183,6 @@ b32 gjk_boolean(struct v2_array poly0, struct v2_array poly1, u32 max_steps)
|
|||||||
return colliding;
|
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 gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array poly1, u32 max_steps)
|
||||||
{
|
{
|
||||||
struct temp_arena scratch = scratch_begin_no_conflict();
|
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 */
|
/* 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);
|
m = menkowski_point(poly0, poly1, v2_neg(s.a.p));
|
||||||
m = menkowski_point(poly0, poly1, dir);
|
|
||||||
if (v2_eq(m.p, s.a.p)) {
|
if (v2_eq(m.p, s.a.p)) {
|
||||||
colliding = false;
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
s.b = s.a;
|
s.b = s.a;
|
||||||
s.a = m;
|
s.a = m;
|
||||||
s.len = 2;
|
s.len = 2;
|
||||||
}
|
|
||||||
|
|
||||||
/* Third point is support point in direction of line normal towards origin */
|
/* 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));
|
dir = perp_towards_point(s.a.p, s.b.p, V2(0, 0));
|
||||||
|
}
|
||||||
|
|
||||||
|
if (step++ >= max_steps) goto abort;
|
||||||
m = menkowski_point(poly0, poly1, dir);
|
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)) {
|
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;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -405,32 +232,38 @@ 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;
|
||||||
|
|
||||||
b32 rab = v2_dot(perp_towards_point(s.a.p, s.b.p, s.c.p), v2_neg(s.a.p)) < 0;
|
struct v2 rab_dir = v2_neg(perp_towards_point(s.a.p, s.b.p, s.c.p));
|
||||||
b32 rac = v2_dot(perp_towards_point(s.a.p, s.c.p, s.b.p), v2_neg(s.a.p)) < 0;
|
struct v2 rac_dir = v2_neg(perp_towards_point(s.a.p, s.c.p, s.b.p));
|
||||||
b32 rbc = v2_dot(perp_towards_point(s.b.p, s.c.p, s.a.p), v2_neg(s.b.p)) < 0;
|
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) {
|
if (!rab && !rac && !rbc) {
|
||||||
colliding = true;
|
colliding = true;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Remove point or edge based on vornoi regions */
|
/* Remove point or edge and determine next direction 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;
|
||||||
rab = rab && !ra && !rb;
|
rab = rab && !ra && !rb;
|
||||||
rac = rac && !ra && !rc;
|
rac = rac && !ra && !rc;
|
||||||
rbc = rbc && !rb && !rc;
|
rbc = rbc && !rb && !rc;
|
||||||
|
|
||||||
if (rab) {
|
if (rab) {
|
||||||
/* Remove c */
|
/* Remove c */
|
||||||
|
dir = rab_dir;
|
||||||
s.len = 2;
|
s.len = 2;
|
||||||
} else if (rac) {
|
} else if (rac) {
|
||||||
/* Remove b */
|
/* Remove b */
|
||||||
|
dir = rac_dir;
|
||||||
s.b = s.c;
|
s.b = s.c;
|
||||||
s.len = 2;
|
s.len = 2;
|
||||||
} else if (rbc) {
|
} else if (rbc) {
|
||||||
/* Remove a */
|
/* Remove a */
|
||||||
|
dir = rbc_dir;
|
||||||
s.a = s.b;
|
s.a = s.b;
|
||||||
s.b = s.c;
|
s.b = s.c;
|
||||||
s.len = 2;
|
s.len = 2;
|
||||||
@ -446,35 +279,42 @@ 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;
|
||||||
}
|
}
|
||||||
}
|
|
||||||
|
|
||||||
prev_point = m.p;
|
prev_point = m.p;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (colliding) {
|
if (colliding) {
|
||||||
ASSERT(s.len == 3);
|
struct gjk_menkowski_point *proto = arena_dry_push(scratch.arena, struct gjk_menkowski_point);
|
||||||
struct v2 *proto = arena_dry_push(scratch.arena, struct v2);
|
u32 proto_count = 0;
|
||||||
|
|
||||||
{
|
{
|
||||||
struct v2 *tmp = arena_push_array(scratch.arena, struct v2, 3);
|
struct gjk_menkowski_point *tmp = arena_push_array(scratch.arena, struct gjk_menkowski_point, 3);
|
||||||
tmp[0] = s.a.p;
|
proto_count = s.len;
|
||||||
tmp[1] = s.b.p;
|
if (proto_count >= 1) {
|
||||||
tmp[2] = s.c.p;
|
tmp[0] = s.a;
|
||||||
}
|
}
|
||||||
u32 proto_count = 3;
|
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);
|
struct v2 pen = V2(0, 0);
|
||||||
f32 pen_len = F32_INFINITY;
|
f32 pen_len = F32_INFINITY;
|
||||||
while (true) {
|
|
||||||
pen = V2(0, 0);
|
|
||||||
pen_len = F32_INFINITY;
|
|
||||||
|
|
||||||
/* Find dir from origin to closest edge */
|
/* Find dir from origin to closest edge */
|
||||||
|
u32 pen_ps_index = 0;
|
||||||
u32 pen_pe_index = 1;
|
u32 pen_pe_index = 1;
|
||||||
for (u32 i = 0; i < proto_count; ++i) {
|
for (u32 i = 0; i < proto_count; ++i) {
|
||||||
u32 ps_index = i;
|
u32 ps_index = i;
|
||||||
u32 pe_index = (i < proto_count - 1) ? (i + 1) : 0;
|
u32 pe_index = (i < proto_count - 1) ? (i + 1) : 0;
|
||||||
struct v2 ps = proto[ps_index];
|
struct v2 ps = proto[ps_index].p;
|
||||||
struct v2 pe = proto[pe_index];
|
struct v2 pe = proto[pe_index].p;
|
||||||
|
|
||||||
struct v2 vse = v2_sub(pe, ps);
|
struct v2 vse = v2_sub(pe, ps);
|
||||||
struct v2 vso = v2_neg(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 */
|
/* TODO: sq cmp */
|
||||||
f32 pd_len = v2_len(pd);
|
f32 pd_len = v2_len(pd);
|
||||||
if (pd_len < pen_len) {
|
if (pd_len < pen_len) {
|
||||||
|
pen_ps_index = ps_index;
|
||||||
pen_pe_index = pe_index;
|
pen_pe_index = pe_index;
|
||||||
pen = pd;
|
pen = pd;
|
||||||
pen_len = pd_len;
|
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;
|
b32 unique = true;
|
||||||
for (u32 i = 0; i < proto_count; ++i) {
|
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;
|
unique = false;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
if (!unique) {
|
if (!unique) {
|
||||||
|
s.a = proto[pen_ps_index];
|
||||||
|
s.b = proto[pen_pe_index];
|
||||||
|
s.len = 2;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -519,10 +363,12 @@ struct gjk_extended_result gjk_extended(struct v2_array poly0, struct v2_array p
|
|||||||
u32 shift_to = i;
|
u32 shift_to = i;
|
||||||
proto[shift_to] = proto[shift_from];
|
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) {
|
if (s.len == 1) {
|
||||||
shape0_p = s.a.p0;
|
shape0_p = s.a.p0;
|
||||||
shape1_p = s.a.p1;
|
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 };
|
struct gjk_extended_result res = { 0 };
|
||||||
res.colliding = colliding;
|
res.colliding = colliding;
|
||||||
res.p0 = shape0_p;
|
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);
|
scratch_end(scratch);
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user