diff --git a/src/game.c b/src/game.c index 9275a6c0..85c5effc 100644 --- a/src/game.c +++ b/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; diff --git a/src/gjk.c b/src/gjk.c index 2cd50a06..6cc94add 100644 --- a/src/gjk.c +++ b/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) diff --git a/src/user.c b/src/user.c index d2604192..c54862a9 100644 --- a/src/user.c +++ b/src/user.c @@ -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); }