diff --git a/src/entity.h b/src/entity.h index 01e17029..27278b17 100644 --- a/src/entity.h +++ b/src/entity.h @@ -93,12 +93,15 @@ struct entity { /* TODO: Remove this (testing) */ b32 colliding; struct entity_handle colliding_with; - struct v2 point; struct gjk_simplex simplex; struct gjk_prototype prototype; struct v2 pendir; b32 solved; + struct v2 point0; + struct v2 point1; + b32 has_2nd_point; + b32 test_torque_applied; b32 test_collided; diff --git a/src/game.c b/src/game.c index 70e7cdd8..6b4645d0 100644 --- a/src/game.c +++ b/src/game.c @@ -128,8 +128,8 @@ INTERNAL void spawn_test_entities(void) struct v2 size = V2(1, 1); //f32 r = PI / 4; //f32 r = PI / 3; - //f32 r = PI / 2; - f32 r = 0; + f32 r = PI / 2; + //f32 r = 0; f32 skew = 0; struct entity *e = entity_alloc(root); @@ -910,8 +910,11 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) } b32 colliding = false; - struct v2 p0 = V2(0, 0); - struct v2 p1 = V2(0, 0); + struct v2 pair0p0 = V2(0, 0); + struct v2 pair0p1 = V2(0, 0); + struct v2 pair1p0 = V2(0, 0); + struct v2 pair1p1 = V2(0, 0); + b32 has_2nd_pair = false; struct entity *colliding_with = entity_nil(); struct gjk_simplex simplex = { 0 }; struct gjk_prototype prototype = { 0 }; @@ -938,11 +941,14 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) }; } - struct gjk_extended_result res = gjk_extended(e0_poly, e1_poly); + struct gjk_contact_points_result res = gjk_contact_points(e0_poly, e1_poly); colliding = res.colliding; - p0 = res.p0; - p1 = res.p1; + pair0p0 = res.pair0p0; + pair0p1 = res.pair0p1; + pair1p0 = res.pair1p0; + pair1p1 = res.pair1p1; + has_2nd_pair = res.has_2nd_pair; colliding_with = e1; simplex = res.simplex; prototype = res.prototype; @@ -951,7 +957,9 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) e0->colliding = colliding; e0->colliding_with = colliding_with->handle; - e0->point = p0; + e0->point0 = pair0p0; + e0->point1 = pair1p0; + e0->has_2nd_point = has_2nd_pair; e0->simplex = simplex; e0->prototype = prototype; e0->pendir = velocity; @@ -960,11 +968,13 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) if (colliding_with->valid) { colliding_with->colliding = colliding; colliding_with->colliding_with = e0->handle; - colliding_with->point = p1; + colliding_with->point0 = pair0p1; + colliding_with->point1 = pair1p1; + colliding_with->has_2nd_point = has_2nd_pair; } { -#if 1 +#if 0 if (colliding) { struct entity *e1 = colliding_with; struct xform e1_xf = entity_get_xform(e1); diff --git a/src/gjk.c b/src/gjk.c index 7151cc70..e184ec19 100644 --- a/src/gjk.c +++ b/src/gjk.c @@ -86,13 +86,423 @@ INTERNAL struct gjk_menkowski_point menkowski_point_extended(struct v2_array pol return res; } + + + + + + + + + + + + + + + + + + + +/* ========================== * + * Debug functions + * TODO: Remove these + * ========================== */ + +/* TODO: Remove this (debugging) */ +struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1) +{ + struct v2_array res = { .points = arena_dry_push(arena, struct v2) }; + u64 rays = 500; + for (u64 i = 0; i < rays; ++i) { + f32 angle = ((f32)i / rays) * (2 * PI); + struct v2 dir = v2_from_angle(angle); + struct v2 p = menkowski_point_extended(poly0, poly1, dir).p; + if (res.count == 0 || !v2_eq(p, res.points[res.count - 1])) { + *arena_push(arena, struct v2) = p; + ++res.count; + } + } + return res; +} + +/* TODO: Remove this (debugging) */ +struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_array poly1) +{ + struct v2_array res = { .points = arena_dry_push(arena, struct v2) }; + for (u64 i = 0; i < poly0.count; ++i) { + struct v2 p0 = poly0.points[i]; + for (u64 j = 0; j < poly1.count; ++j) { + struct v2 p1 = poly1.points[j]; + *arena_push(arena, struct v2) = v2_sub(p0, p1); + ++res.count; + } + } + + return res; +} + + + + +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 */ + const f32 epsilon = 0.0000100; + struct gjk_simplex s = { 0 }; + b32 colliding = false; + struct gjk_menkowski_point *proto = NULL; + u32 proto_count = 0; + + struct v2 pair0p0 = { 0 }; + struct v2 pair0p1 = { 0 }; + struct v2 pair1p0 = { 0 }; + struct v2 pair1p1 = { 0 }; + b32 has_2nd_pair = false; + +#if GJK_DEBUG + u32 dbg_step = 0; +#endif + + /* ========================== * + * GJK collision check + * Construct encapsulating simplex OR closest feature if not colliding + * ========================== */ + + struct v2 dir = { 0 }; + struct gjk_menkowski_point m = { 0 }; + /* Determine encapsulating simplex if colliding, or closest edge / point to origin on simplex */ + { + /* 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; + + while (!colliding) { + if (s.len == 1) { + /* Second point is support point towards origin */ + dir = v2_neg(s.a.p); + + DBGSTEP; + m = menkowski_point_extended(shape0, shape1, dir); + if (v2_eq(m.p, s.a.p)) { + /* Point is the same */ + break; + } + s.b = s.a; + s.a = m; + s.len = 2; + if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) < epsilon) { + /* New ab lies on origin */ + break; + } + + /* 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)); + } + + { + DBGSTEP; + m = menkowski_point_extended(shape0, shape1, dir); + if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_sub(m.p, s.a.p))) < epsilon) { + /* New point is on existing line ab */ + break; + } + s.c = s.b; + s.b = s.a; + s.a = m; + s.len = 3; + if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) < epsilon) { + /* New ab lies on origin */ + s.len = 2; + break; + } else if (math_fabs(v2_wedge(v2_sub(s.c.p, s.a.p), v2_neg(s.a.p))) < epsilon) { + /* New ac lies on origin */ + s.b = s.c; + s.len = 2; + break; + } + } + + /* Determine voronoi region of the simplex in which the origin lies */ + i32 voronoi_mask = 0; + struct v2 vab = v2_sub(s.b.p, s.a.p); + struct v2 vac = v2_sub(s.c.p, s.a.p); + struct v2 vbc = v2_sub(s.c.p, s.b.p); + struct v2 rab_dir = v2_perp_towards_dir(vab, v2_neg(vac)); + struct v2 rac_dir = v2_perp_towards_dir(vac, v2_neg(vab)); + struct v2 rbc_dir = v2_perp_towards_dir(vbc, vab); + voronoi_mask |= (v2_dot(rab_dir, v2_neg(s.a.p)) >= 0) << 0; /* Regions ab, a, and b*/ + voronoi_mask |= (v2_dot(rac_dir, v2_neg(s.a.p)) >= 0) << 1; /* Regions ac, a, and c */ + voronoi_mask |= (v2_dot(rbc_dir, v2_neg(s.b.p)) >= 0) << 2; /* Regions bc, b, and c */ + /* Remove point or edge and determine next direction based on voronoi region */ + switch (voronoi_mask) { + default: + { /* No region, must be in simplex */ + colliding = true; + } break; + case 1: + { /* Region ab, remove c */ + dir = rab_dir; /* Next third point is in direction of region ab */ + s.len = 2; + } break; + case 2: + { /* Region ac, remove b */ + dir = rac_dir; /* Next third point is in direction of region ac */ + s.b = s.c; + s.len = 2; + } break; + case 4: + { /* Region bc, remove a */ + dir = rbc_dir; /* Next third point is in direction of region bc */ + s.a = s.b; + s.b = s.c; + s.len = 2; + } break; + case 3: + { /* Region a, remove bc */ + s.len = 1; + } break; + case 5: + { /* Region b, remove ac */ + s.a = s.b; + s.len = 1; + } break; + case 6: + { /* Region c, remove ab */ + s.a = s.c; + s.len = 1; + } 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; + } + + /* ========================== * + * Expand simplex towards closest edge to origin inside menkowski + * ========================== */ + + 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) { + pair0p0 = s.a.p0; + pair0p1 = s.a.p1; + } else if (s.len == 2) { + + /* TODO: Epsilon */ + if (v2_eq(s.a.p0, s.b.p0) || v2_eq(s.a.p1, s.b.p1)) { + /* FIXME: Winding order dependent? */ + ASSERT(s.len == 2); + f32 ratio; + { + /* Determine ratio between edge a & b that projected origin lies */ + struct v2 vab = v2_sub(s.b.p, s.a.p); + struct v2 vao = v2_neg(s.a.p); + ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1); + } + /* Shape 0 */ + pair0p0 = v2_sub(s.b.p0, s.a.p0); + pair0p0 = v2_mul(pair0p0, ratio); + pair0p0 = v2_add(pair0p0, s.a.p0); + /* Shape 1 */ + pair0p1 = v2_sub(s.b.p1, s.a.p1); + pair0p1 = v2_mul(pair0p1, ratio); + pair0p1 = v2_add(pair0p1, s.a.p1); + } else { + /* Face -> face, need 2nd pair */ + + has_2nd_pair = true; + + /* TODO: Rename abcd variables */ + + 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); + + pair0p0 = v2_add(a, v2_mul(vab, clamp_f32(v2_dot(vab, vac) * inv_vab_len_sq, 0, 1))); + pair1p0 = v2_add(a, v2_mul(vab, clamp_f32(v2_dot(vab, vad) * inv_vab_len_sq, 0, 1))); + pair0p1 = v2_add(c, v2_mul(vcd, clamp_f32(v2_dot(vcd, vca) * inv_vcd_len_sq, 0, 1))); + pair1p1 = v2_add(c, v2_mul(vcd, clamp_f32(v2_dot(vcd, vcb) * inv_vcd_len_sq, 0, 1))); + } + } + + 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.colliding = colliding; + res.pair0p0 = pair0p0; + res.pair0p1 = pair0p1; + res.pair1p0 = pair1p0; + res.pair1p1 = pair1p1; + res.has_2nd_pair = has_2nd_pair; + res.simplex = s; + scratch_end(scratch); + return res; +} + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +/* ========================== * + * GJK closest / deepest point (unused) + * ========================== */ + +#if 0 struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1) { struct temp_arena scratch = scratch_begin_no_conflict(); struct gjk_extended_result res = { 0 }; /* TODO: Verify epsilon */ - f32 epsilon = 0.0000100; + const f32 epsilon = 0.0000100; struct gjk_simplex s = { 0 }; b32 colliding = false; struct v2 shape0_p = { 0 }; @@ -347,39 +757,7 @@ abort: scratch_end(scratch); return res; } - -/* TODO: Remove this (debugging) */ -struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1) -{ - struct v2_array res = { .points = arena_dry_push(arena, struct v2) }; - u64 rays = 500; - for (u64 i = 0; i < rays; ++i) { - f32 angle = ((f32)i / rays) * (2 * PI); - struct v2 dir = v2_from_angle(angle); - struct v2 p = menkowski_point_extended(poly0, poly1, dir).p; - if (res.count == 0 || !v2_eq(p, res.points[res.count - 1])) { - *arena_push(arena, struct v2) = p; - ++res.count; - } - } - return res; -} - -/* TODO: Remove this (debugging) */ -struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_array poly1) -{ - struct v2_array res = { .points = arena_dry_push(arena, struct v2) }; - for (u64 i = 0; i < poly0.count; ++i) { - struct v2 p0 = poly0.points[i]; - for (u64 j = 0; j < poly1.count; ++j) { - struct v2 p1 = poly1.points[j]; - *arena_push(arena, struct v2) = v2_sub(p0, p1); - ++res.count; - } - } - - return res; -} +#endif diff --git a/src/gjk.h b/src/gjk.h index c0e94f7d..16d8228c 100644 --- a/src/gjk.h +++ b/src/gjk.h @@ -22,6 +22,27 @@ struct gjk_simplex { b32 gjk_boolean(struct v2_array shape0, struct v2_array shape1); struct gjk_prototype { struct v2 points[64]; u32 len; }; +struct gjk_contact_points_result { + b32 colliding; + + + struct v2 pair0p0, pair0p1; + struct v2 pair1p0, pair1p1; + b32 has_2nd_pair; + + /* For debugging */ + b32 solved; + struct gjk_simplex simplex; + struct gjk_prototype prototype; +}; + +struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, struct v2_array shape1); + +struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1); + +struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_array poly1); + +#if 0 struct gjk_extended_result { b32 colliding; struct v2 p0, p1; /* Closest points (or penetrating points if colliding) on each shape */ @@ -32,18 +53,9 @@ struct gjk_extended_result { struct gjk_prototype prototype; }; -/* Returns shape whether shapes are colliding well as closest / penetrating points on each shape. - * - * If shapes are colliding and `penetration_dir` is non-zero, the shortest - * direction to resolve the collision will be used to calculate penetrating - * points. Otherwise, the penetrating points will be calculated using the - * supplied direction. - */ +/* Returns whether shapes are colliding well as closest / penetrating points on each shape. */ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1); - -struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1); - -struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_array poly1); +#endif #if 0 diff --git a/src/user.c b/src/user.c index 602e8964..5816e507 100644 --- a/src/user.c +++ b/src/user.c @@ -1113,12 +1113,19 @@ INTERNAL void user_update(void) } #endif - /* Draw point */ + /* Draw points */ { - u32 color = entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED) ? RGBA_32_F(1, 0, 0, 0.75) : RGBA_32_F(0, 1, 1, 0.75); f32 radius = 5; - struct v2 point = xform_mul_v2(G.world_view, ent->point); - draw_solid_circle(G.viewport_canvas, point, radius, color, 10); + { + u32 color = entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED) ? RGBA_32_F(1, 0, 0, 0.75) : RGBA_32_F(0, 1, 1, 0.75); + struct v2 point = xform_mul_v2(G.world_view, ent->point0); + draw_solid_circle(G.viewport_canvas, point, radius, color, 10); + } + if (ent->has_2nd_point) { + u32 color = entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED) ? RGBA_32_F(1, 1, 0, 0.75) : RGBA_32_F(1, 0, 1, 0.75); + struct v2 point = xform_mul_v2(G.world_view, ent->point1); + draw_solid_circle(G.viewport_canvas, point, radius, color, 10); + } } }