use shortcut-gjk for gjk_contact_points
This commit is contained in:
parent
41d56ac13c
commit
7b37f78b1e
14
src/game.c
14
src/game.c
@ -129,10 +129,10 @@ INTERNAL void spawn_test_entities(void)
|
||||
//struct v2 pos = V2(0.374142020941, -0.246118023992); /* Touching glitch spot */
|
||||
//struct v2 size = V2(1, 1);
|
||||
struct v2 size = V2(0.5, 0.5);
|
||||
//f32 r = PI / 4;
|
||||
f32 r = PI / 4;
|
||||
//f32 r = PI / 3;
|
||||
//f32 r = PI / 2;
|
||||
f32 r = 0;
|
||||
//f32 r = 0;
|
||||
f32 skew = 0;
|
||||
|
||||
struct entity *e = entity_alloc(root);
|
||||
@ -155,8 +155,8 @@ INTERNAL void spawn_test_entities(void)
|
||||
|
||||
entity_enable_prop(e, ENTITY_PROP_PHYSICAL);
|
||||
e->mass_unscaled = 100;
|
||||
e->inertia_unscaled = 1;
|
||||
//e->inertia_unscaled = F32_INFINITY;
|
||||
//e->inertia_unscaled = 1;
|
||||
e->inertia_unscaled = F32_INFINITY;
|
||||
e->linear_ground_friction = 1000;
|
||||
e->angular_ground_friction = 100;
|
||||
|
||||
@ -200,8 +200,8 @@ INTERNAL void spawn_test_entities(void)
|
||||
|
||||
entity_enable_prop(e, ENTITY_PROP_PHYSICAL);
|
||||
#if 0
|
||||
e->mass_unscaled = 1000;
|
||||
e->inertia_unscaled = 1000;
|
||||
e->mass_unscaled = 500;
|
||||
e->inertia_unscaled = 500;
|
||||
#else
|
||||
e->mass_unscaled = F32_INFINITY;
|
||||
e->inertia_unscaled = F32_INFINITY;
|
||||
@ -566,7 +566,7 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias)
|
||||
//f32 bias_factor = 0.2;
|
||||
f32 bias_factor = 0.1;
|
||||
|
||||
f32 bias_slop = 0.001;
|
||||
f32 bias_slop = 0.0005;
|
||||
//f32 bias_slop = 0.001;
|
||||
//f32 bias_slop = 0.00;
|
||||
|
||||
|
||||
321
src/gjk.c
321
src/gjk.c
@ -56,13 +56,16 @@ b32 gjk_boolean(struct v2_array shape0, struct v2_array shape1)
|
||||
s.b = s.a;
|
||||
s.a = p;
|
||||
|
||||
dir = v2_perp_towards_dir(v2_sub(s.b, s.a), v2_neg(v2_sub(s.c, s.a))); /* Normal of ab pointing away from c */
|
||||
struct v2 vab = v2_sub(s.b, s.a);
|
||||
struct v2 vac = v2_sub(s.c, s.a);
|
||||
struct v2 a_to_origin = v2_neg(s.a);
|
||||
|
||||
dir = v2_perp_towards_dir(vab, v2_neg(vac)); /* Normal of ab pointing away from c */
|
||||
if (v2_dot(dir, a_to_origin) >= 0) {
|
||||
/* Point is in region ab, remove c from simplex (will happen automatically next iteration) */
|
||||
} else {
|
||||
/* Point is not in region ab */
|
||||
dir = v2_perp_towards_dir(v2_sub(s.c, s.a), v2_neg(v2_sub(s.b, s.a))); /* Normal of ac pointing away from b */
|
||||
dir = v2_perp_towards_dir(vac, v2_neg(vab)); /* Normal 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;
|
||||
@ -147,6 +150,281 @@ struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_arra
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/* TODO: Update this function to only calculate contact pairs if colliding. Will allow for shortcut-style GJK.
|
||||
* `colliding` & `has_2nd_pair` become "num_pairs", where 0 = no collision. */
|
||||
struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, struct v2_array shape1)
|
||||
{
|
||||
struct temp_arena scratch = scratch_begin_no_conflict();
|
||||
struct gjk_contact_points_result res = { 0 };
|
||||
|
||||
/* TODO: Verify epsilon */
|
||||
/* FIXME: Infinite loop when epsilon too low and shapes is too far from 0 (precision issue) */
|
||||
const f32 epsilon = 0.0000100;
|
||||
struct gjk_simplex s = { 0 };
|
||||
b32 colliding = false;
|
||||
struct gjk_menkowski_point *proto = NULL;
|
||||
u32 proto_count = 0;
|
||||
|
||||
struct gjk_contact_pair pair0 = { 0 };
|
||||
struct gjk_contact_pair pair1 = { 0 };
|
||||
u32 num_pairs = 0;
|
||||
|
||||
/* Used by GJK & EPA */
|
||||
struct v2 dir = { 0 };
|
||||
struct gjk_menkowski_point m = { 0 };
|
||||
|
||||
#if GJK_DEBUG
|
||||
u32 dbg_step = 0;
|
||||
#endif
|
||||
|
||||
/* ========================== *
|
||||
* GJK
|
||||
* ========================== */
|
||||
|
||||
{
|
||||
/* First point is support point in shape's general directions to eachother */
|
||||
s.a = menkowski_point_extended(shape0, shape1, v2_sub(shape1.points[0], shape0.points[0]));
|
||||
s.len = 1;
|
||||
|
||||
/* Second point is support point towards origin */
|
||||
dir = v2_neg(s.a.p);
|
||||
m = menkowski_point_extended(shape0, shape1, dir);
|
||||
if (v2_dot(dir, m.p) >= 0) {
|
||||
s.b = s.a;
|
||||
s.a = m;
|
||||
s.len = 2;
|
||||
while (true) {
|
||||
/* Third point is support point in direction of line normal towards origin */
|
||||
dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p));
|
||||
m = menkowski_point_extended(shape0, shape1, dir);
|
||||
if (v2_dot(dir, m.p) < 0) {
|
||||
/* New point did not cross origin, collision impossible */
|
||||
break;
|
||||
}
|
||||
|
||||
s.c = s.b;
|
||||
s.b = s.a;
|
||||
s.a = m;
|
||||
s.len = 3;
|
||||
|
||||
struct v2 vab = v2_sub(s.b.p, s.a.p);
|
||||
struct v2 vac = v2_sub(s.c.p, s.a.p);
|
||||
struct v2 a_to_origin = v2_neg(s.a.p);
|
||||
|
||||
dir = v2_perp_towards_dir(vab, v2_neg(vac)); /* Normal of ab pointing away from c */
|
||||
if (v2_dot(dir, a_to_origin) >= 0) {
|
||||
/* Point is in region ab, remove c from simplex */
|
||||
s.len = 2;
|
||||
} else {
|
||||
/* Point is not in region ab */
|
||||
dir = v2_perp_towards_dir(vac, v2_neg(vab)); /* Normal 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) {
|
||||
/* ========================== *
|
||||
* Epa
|
||||
* ========================== */
|
||||
|
||||
proto = arena_dry_push(scratch.arena, struct gjk_menkowski_point);
|
||||
proto_count = 0;
|
||||
{
|
||||
ASSERT(s.len == 3);
|
||||
struct gjk_menkowski_point *tmp = arena_push_array(scratch.arena, struct gjk_menkowski_point, 3);
|
||||
tmp[0] = s.a;
|
||||
tmp[1] = s.b;
|
||||
tmp[2] = s.c;
|
||||
proto_count = 3;
|
||||
}
|
||||
|
||||
while (true) {
|
||||
f32 pen_len_sq = F32_INFINITY;
|
||||
|
||||
/* Find dir from origin to closest edge */
|
||||
/* FIXME: Winding order of ps & pe index */
|
||||
u32 pen_ps_index = 0;
|
||||
u32 pen_pe_index = 0;
|
||||
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].p;
|
||||
struct v2 pe = proto[pe_index].p;
|
||||
|
||||
struct v2 vse = v2_sub(pe, ps);
|
||||
struct v2 vso = v2_neg(ps);
|
||||
|
||||
f32 d1 = v2_dot(vso, vse);
|
||||
f32 d2 = v2_dot(vse, vse);
|
||||
struct v2 vsd = v2_mul(vse, (d1 / d2));
|
||||
struct v2 pd = v2_add(ps, vsd);
|
||||
|
||||
f32 pd_len_sq = v2_len_sq(pd);
|
||||
if (pd_len_sq < pen_len_sq) {
|
||||
pen_ps_index = ps_index;
|
||||
pen_pe_index = pe_index;
|
||||
pen_len_sq = pd_len_sq;
|
||||
dir = pd;
|
||||
}
|
||||
}
|
||||
|
||||
/* TODO: Move to break (debugging) */
|
||||
s.a = proto[pen_ps_index];
|
||||
s.b = proto[pen_pe_index];
|
||||
s.len = 2;
|
||||
|
||||
/* Find new point in dir */
|
||||
m = menkowski_point_extended(shape0, shape1, dir);
|
||||
|
||||
/* Check unique */
|
||||
/* TODO: Better */
|
||||
DBGSTEP;
|
||||
{
|
||||
b32 unique = true;
|
||||
for (u32 i = 0; i < proto_count; ++i) {
|
||||
struct v2 edge_start = proto[i].p;
|
||||
struct v2 edge_end = i < proto_count - 1 ? proto[i + 1].p : proto[0].p;
|
||||
if (math_fabs(v2_wedge(v2_sub(edge_end, edge_start), v2_sub(m.p, edge_start))) < epsilon) {
|
||||
unique = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!unique) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* Insert point into prototype */
|
||||
/* FIXME: Preserve winding order */
|
||||
arena_push(scratch.arena, struct gjk_menkowski_point);
|
||||
++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;
|
||||
}
|
||||
|
||||
/* ========================== *
|
||||
* Resolve points
|
||||
* ========================== */
|
||||
|
||||
if (s.len == 1) {
|
||||
num_pairs = 1;
|
||||
pair0.p0 = s.a.p0;
|
||||
pair0.p1 = s.a.p1;
|
||||
} else {
|
||||
ASSERT(s.len == 2);
|
||||
/* TODO: Epsilon */
|
||||
if (!v2_eq(s.a.p0, s.b.p0) && !v2_eq(s.a.p1, s.b.p1)) {
|
||||
/* Closest features are both faces, return clipped edge points of each face */
|
||||
num_pairs = 2;
|
||||
struct v2 a = s.a.p0;
|
||||
struct v2 b = s.b.p0;
|
||||
struct v2 c = s.a.p1;
|
||||
struct v2 d = s.b.p1;
|
||||
|
||||
struct v2 vab = v2_sub(b, a);
|
||||
struct v2 vac = v2_sub(c, a);
|
||||
struct v2 vad = v2_sub(d, a);
|
||||
|
||||
struct v2 vcd = v2_sub(d, c);
|
||||
struct v2 vca = v2_sub(a, c);
|
||||
struct v2 vcb = v2_sub(b, c);
|
||||
|
||||
f32 inv_vab_len_sq = 1.f / v2_len_sq(vab);
|
||||
f32 inv_vcd_len_sq = 1.f / v2_len_sq(vcd);
|
||||
|
||||
pair0.p0 = v2_add(a, v2_mul(vab, clamp_f32(v2_dot(vab, vac) * inv_vab_len_sq, 0, 1)));
|
||||
pair0.p1 = v2_add(c, v2_mul(vcd, clamp_f32(v2_dot(vcd, vcb) * inv_vcd_len_sq, 0, 1)));
|
||||
pair1.p0 = v2_add(a, v2_mul(vab, clamp_f32(v2_dot(vab, vad) * inv_vab_len_sq, 0, 1)));
|
||||
pair1.p1 = v2_add(c, v2_mul(vcd, clamp_f32(v2_dot(vcd, vca) * inv_vcd_len_sq, 0, 1)));
|
||||
} else {
|
||||
/* Determine ratio between edge a & b that projected origin lies */
|
||||
num_pairs = 1;
|
||||
struct v2 vab = v2_sub(s.b.p, s.a.p);
|
||||
f32 ratio = clamp_f32(v2_dot(vab, v2_neg(s.a.p)) / v2_dot(vab, vab), 0, 1);
|
||||
pair0.p0 = v2_add(s.a.p0, v2_mul(v2_sub(s.b.p0, s.a.p0), ratio));
|
||||
pair0.p1 = v2_add(s.a.p1, v2_mul(v2_sub(s.b.p1, s.a.p1), ratio));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
res.solved = true;
|
||||
abort:
|
||||
|
||||
if (proto_count > 0) {
|
||||
for (u32 i = 0; i < min_u32(proto_count, ARRAY_COUNT(res.prototype.points)); ++i) {
|
||||
res.prototype.points[i] = proto[i].p;
|
||||
}
|
||||
res.prototype.len = proto_count;
|
||||
} else {
|
||||
if (s.len >= 1) {
|
||||
res.prototype.points[0] = s.a.p;
|
||||
if (s.len >= 2) {
|
||||
res.prototype.points[1] = s.b.p;
|
||||
if (s.len >= 3) {
|
||||
res.prototype.points[2] = s.c.p;
|
||||
}
|
||||
}
|
||||
}
|
||||
res.prototype.len = s.len;
|
||||
}
|
||||
res.pairs[0] = pair0;
|
||||
res.pairs[1] = pair1;
|
||||
res.num_pairs = num_pairs;
|
||||
res.simplex = s;
|
||||
scratch_end(scratch);
|
||||
return res;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/* ========================== *
|
||||
* GJK closest / deepest contact pairs (unused)
|
||||
* ========================== */
|
||||
|
||||
#if 0
|
||||
/* TODO: Update this function to only calculate contact pairs if colliding. Will allow for shortcut-style GJK.
|
||||
* `colliding` & `has_2nd_pair` become "num_pairs", where 0 = no collision. */
|
||||
struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, struct v2_array shape1)
|
||||
@ -442,44 +720,7 @@ abort:
|
||||
scratch_end(scratch);
|
||||
return res;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
/* ========================== *
|
||||
* GJK closest / deepest point (unused)
|
||||
|
||||
@ -1001,15 +1001,15 @@ INTERNAL void user_update(void)
|
||||
struct contact contact = ent->contacts[i];
|
||||
{
|
||||
u32 color = entity_has_prop(e0, ENTITY_PROP_PLAYER_CONTROLLED) ? RGBA_32_F(1, 0, 0, 0.50) : RGBA_32_F(0, 1, 1, 0.50);
|
||||
//struct v2 point = xform_mul_v2(e0_xf, contact.p0_local);
|
||||
struct v2 point = contact.p0_initial_world;
|
||||
struct v2 point = xform_mul_v2(e0_xf, contact.p0_local);
|
||||
//struct v2 point = contact.p0_initial_world;
|
||||
point = xform_mul_v2(G.world_view, point);
|
||||
draw_solid_circle(G.viewport_canvas, point, radius, color, 10);
|
||||
}
|
||||
{
|
||||
u32 color = entity_has_prop(e1, ENTITY_PROP_PLAYER_CONTROLLED) ? RGBA_32_F(1, 0, 0, 0.50) : RGBA_32_F(0, 1, 1, 0.50);
|
||||
//struct v2 point = xform_mul_v2(e1_xf, contact.p1_local);
|
||||
struct v2 point = contact.p1_initial_world;
|
||||
struct v2 point = xform_mul_v2(e1_xf, contact.p1_local);
|
||||
//struct v2 point = contact.p1_initial_world;
|
||||
point = xform_mul_v2(G.world_view, point);
|
||||
draw_solid_circle(G.viewport_canvas, point, radius, color, 10);
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user