From d52effe5c1f2107964cdd8c69bad89dc5e1d6149 Mon Sep 17 00:00:00 2001 From: jacob Date: Tue, 8 Oct 2024 12:15:11 -0500 Subject: [PATCH] wip collider shapes --- src/collider.c | 1318 ++++++++++++++++++++++++++++++++++++++++++++++-- src/collider.h | 25 +- src/common.h | 10 + src/config.h | 2 + src/draw.c | 20 + src/draw.h | 1 + src/entity.h | 11 +- src/game.c | 152 +++--- src/math.h | 31 +- src/user.c | 90 ++-- 10 files changed, 1460 insertions(+), 200 deletions(-) diff --git a/src/collider.c b/src/collider.c index ff6409e3..9d709ef7 100644 --- a/src/collider.c +++ b/src/collider.c @@ -4,33 +4,52 @@ #include "scratch.h" #if COLLIDER_DEBUG -u32 collider_debug_steps = U32_MAX; +//u32 collider_debug_steps = U32_MAX; +u32 collider_debug_steps = 50; #endif #define DBGSTEP if (dbg_step++ >= collider_debug_steps) goto abort -INTERNAL struct v2 poly_support_point(struct v2_array a, struct v2 dir) +struct v2 collider_support_point(struct collider_shape *a, struct xform xf, struct v2 dir) { + struct v2 *points = a->points; + u32 count = a->count; + f32 radius = a->radius; + + struct xform xf_inv = xform_invert(xf); /* TODO: Calculate this outside of function */ + dir = xform_basis_mul_v2(xf_inv, dir); + /* TODO: Could probably binary search for largest dot since shape is convex */ - struct v2 furthest = a.points[0]; + struct v2 furthest = points[0]; f32 furthest_dot = v2_dot(dir, furthest); - for (u32 i = 1; i < a.count; ++i) { - struct v2 p = a.points[i]; + for (u32 i = 1; i < count; ++i) { + struct v2 p = points[i]; f32 dot = v2_dot(dir, p); if (dot > furthest_dot) { furthest = p; furthest_dot = dot; } } + + if (radius > 0.0) { + dir = v2_with_len(dir, radius); + furthest = v2_add(furthest, dir); + } + + furthest = xform_mul_v2(xf, furthest); + return furthest; } -INTERNAL u32 poly_support_point_index(struct v2_array a, struct v2 dir) +INTERNAL u32 collider_support_point_index(struct collider_shape *a, struct xform xf, struct v2 dir) { + /* TODO: Could probably binary search for largest dot since shape is convex */ + struct v2 *points = a->points; + u32 count = a->count; u32 furthest = 0; - f32 furthest_dot = v2_dot(dir, a.points[0]); - for (u32 i = 1; i < a.count; ++i) { - struct v2 p = a.points[i]; + f32 furthest_dot = v2_dot(dir, xform_mul_v2(xf, points[0])); + for (u32 i = 1; i < count; ++i) { + struct v2 p = xform_mul_v2(xf, points[i]); f32 dot = v2_dot(dir, p); if (dot > furthest_dot) { furthest = i; @@ -40,21 +59,698 @@ INTERNAL u32 poly_support_point_index(struct v2_array a, struct v2 dir) return furthest; } -INTERNAL struct v2 menkowski_point(struct v2_array poly0, struct v2_array shape1, struct v2 dir) +INTERNAL struct v2 menkowski_point(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1, struct v2 dir) { - return v2_sub(poly_support_point(poly0, dir), poly_support_point(shape1, v2_neg(dir))); + return v2_sub(collider_support_point(shape0, xf0, dir), collider_support_point(shape1, xf1, v2_neg(dir))); } -b32 collider_collision_boolean(struct v2_array shape0, struct v2_array shape1) +struct collider_collision_points_result collider_collision_points(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1) +{ + struct temp_arena scratch = scratch_begin_no_conflict(); /* TODO: Only begin scratch for EPA */ + struct collider_collision_points_result res = ZI; + + struct v2 *points0 = shape0->points; + struct v2 *points1 = shape1->points; + u32 count0 = shape0->count; + u32 count1 = shape1->count; + //f32 radius0 = shape0->radius; + //f32 radius1 = shape1->radius; + + /* TODO: Parameterize */ + const f32 tolerance = 0.0025f; + //const f32 min_unique_pt_dist_sq = 0.001f * 0.001f; + const f32 min_unique_pt_dist_sq = 0.001f * 0.001f; + + b32 colliding = false; + b32 simplex_is_closest_edge = false; + + struct collider_simplex s = ZI; + struct v2 *proto = NULL; + u32 proto_count = 0; + + struct v2 normal = ZI; + struct collider_collision_point points[2] = ZI; + u32 num_points = 0; + + struct v2 dir = ZI; + struct v2 m = ZI; + +#if COLLIDER_DEBUG + u32 dbg_step = 0; +#endif + + /* ========================== * + * GJK + * + * Determine encapsulating simplex if colliding, or closest edge / point to + * origin on simplex (for check if shape distances are within tolerance) + * ========================== */ + { + /* First point is support point in shape's general directions to eachother */ + s.a = menkowski_point(shape0, shape1, xf0, xf1, v2_sub(xf1.og, xf0.og)); + s.len = 1; + + struct v2 removed_a = ZI; + struct v2 removed_b = ZI; + u32 num_removed = 0; + b32 done = false; + while (!done) { + if (s.len == 1) { + /* Second point is support point towards origin */ + dir = v2_neg(s.a); + + DBGSTEP; + m = menkowski_point(shape0, shape1, xf0, xf1, dir); + /* Check that new point is far enough away from existing point */ + if (v2_len_sq(v2_sub(m, s.a)) < min_unique_pt_dist_sq) { + done = true; + break; + } + s.b = s.a; + s.a = m; + s.len = 2; + + /* Third point is support point in direction of line normal towards origin */ + dir = v2_perp_towards_dir(v2_sub(s.b, s.a), v2_neg(s.a)); + } + + { + DBGSTEP; + m = menkowski_point(shape0, shape1, xf0, xf1, dir); + /* Check that new point is far enough away from existing points */ + if (v2_len_sq(v2_sub(m, s.a)) < min_unique_pt_dist_sq || + v2_len_sq(v2_sub(m, s.b)) < min_unique_pt_dist_sq || + ((num_removed >= 1) && ( + (v2_len_sq(v2_sub(m, removed_a)) < min_unique_pt_dist_sq) || + (num_removed >= 2 && v2_len_sq(v2_sub(m, removed_b)) < min_unique_pt_dist_sq) + )) || + math_fabs(v2_wedge(v2_sub(s.b, s.a), v2_sub(m, s.a))) < min_unique_pt_dist_sq + ) { + simplex_is_closest_edge = true; + done = true; + break; + } + s.c = s.b; + s.b = s.a; + s.a = m; + s.len = 3; + } + + /* Determine voronoi region of the simplex in which the origin lies */ + i32 voronoi_mask = 0; + struct v2 vab = v2_sub(s.b, s.a); + struct v2 vac = v2_sub(s.c, s.a); + struct v2 vbc = v2_sub(s.c, s.b); + 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)) > 0) << 0; /* Regions ab, a, and b*/ + voronoi_mask |= (v2_dot(rac_dir, v2_neg(s.a)) > 0) << 1; /* Regions ac, a, and c */ + voronoi_mask |= (v2_dot(rbc_dir, v2_neg(s.b)) > 0) << 2; /* Regions bc, b, and c */ + /* Remove point or edge and determine next direction based on voronoi region */ + switch (voronoi_mask) { + case 0: + { /* No region, must be in simplex */ + colliding = true; + done = true; + } break; + case 1: + { /* Region ab, remove c */ + num_removed = 1; + removed_a = s.c; + s.len = 2; + dir = rab_dir; /* Next third point is in direction of region ab */ + } break; + case 2: + { /* Region ac, remove b */ + num_removed = 1; + removed_a = s.b; + s.len = 2; + s.b = s.c; + dir = rac_dir; /* Next third point is in direction of region ac */ + } break; + case 4: + { /* Region bc, remove a */ + num_removed = 1; + removed_a = s.a; + s.len = 2; + s.a = s.b; + s.b = s.c; + dir = rbc_dir; /* Next third point is in direction of region bc */ + } break; + case 3: + { /* Region a, remove bc */ + num_removed = 2; + removed_a = s.b; + removed_b = s.c; + s.len = 1; + } break; + case 5: + { /* Region b, remove ac */ + num_removed = 2; + removed_a = s.a; + removed_b = s.c; + s.len = 1; + s.a = s.b; + } break; + case 6: + { /* Region c, remove ab */ + num_removed = 2; + removed_a = s.a; + removed_b = s.b; + s.len = 1; + s.a = s.c; + } break; + default: + { + /* Unknown region (should be impossible) */ + ASSERT(false); + res.path = -1; + done = true; + } break; + } + } + } + + if (colliding) { + /* ========================== * + * Epa (to find collision normal from inside shape) + * ========================== */ + + proto = arena_dry_push(scratch.arena, struct v2); + proto_count = 0; + { + ASSERT(s.len == 3); + struct v2 *tmp = arena_push_array(scratch.arena, struct v2, 3); + tmp[0] = s.a; + tmp[1] = s.b; + tmp[2] = s.c; + proto_count = 3; + } + + while (colliding) { + 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]; + 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_len_sq(vse))); + 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; + } + } + + /* TODO: Remove this (debugging) */ + s.a = proto[pen_ps_index]; + s.b = proto[pen_pe_index]; + s.len = 2; + + /* Find new point in dir */ + DBGSTEP; + { + /* Next point is in direction of line normal pointing outwards from simplex */ + /* TODO: If winding order is guaranteed then this can become v2_perp_left/right? */ + struct v2 a = proto[pen_ps_index]; + struct v2 b = proto[pen_pe_index]; + struct v2 n = proto[(pen_pe_index < proto_count - 1) ? (pen_pe_index + 1) : 0]; /* Next point along prototype after edge */ + struct v2 vab = v2_sub(b, a); + struct v2 vna = v2_sub(a, n); + dir = v2_perp_towards_dir(vab, vna); + } + m = menkowski_point(shape0, shape1, xf0, xf1, dir); + + /* Check unique */ + /* TODO: Better */ + { + b32 unique = true; + for (u32 i = 0; i < proto_count; ++i) { + struct v2 edge_start = proto[i]; + struct v2 edge_end = i < proto_count - 1 ? proto[i + 1] : proto[0]; + struct v2 vsm = v2_sub(m, edge_start); + if (v2_len_sq(vsm) < min_unique_pt_dist_sq || + math_fabs(v2_wedge(v2_sub(edge_end, edge_start), vsm)) < min_unique_pt_dist_sq) { + unique = false; + break; + } + } + if (!unique) { + res.path = 1; + normal = v2_norm(dir); + break; + } + } + + /* Insert point into prototype */ + /* FIXME: Preserve winding order */ + arena_push(scratch.arena, struct collider_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; + } + } else if (simplex_is_closest_edge && s.len > 1) { + /* Shapes are not overlapping (origin is outside of simplex). Project + * origin to determine if distance is within tolerance. */ + ASSERT(s.len == 2); + struct v2 vab = v2_sub(s.b, s.a); + struct v2 vao = v2_neg(s.a); + f32 ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1); + struct v2 p = v2_add(s.a, v2_mul(vab, ratio)); + if (v2_len_sq(p) <= (tolerance * tolerance)) { + res.path = 2; + normal = v2_norm(dir); + colliding = true; + } + } + + if (colliding) { + /* ========================== * + * Clip to determine final points + * ========================== */ + + /* Max vertices must be < 16 to fit in 4 bit ids */ + CT_ASSERT(ARRAY_COUNT(shape0->points) <= 16); + + DBGSTEP; +#if 0 + { + const f32 wedge_epsilon = 0.001f; + + /* shape0 a -> b winding = clockwise */ + u32 id_a0; + u32 id_b0; + struct v2 a0; + struct v2 b0; + struct v2 vab0; + + /* shape1 a -> b winding = counterclockwise */ + u32 id_a1; + u32 id_b1; + struct v2 a1; + struct v2 b1; + struct v2 vab1; + { + u32 p_i = collider_support_point_index(shape0, xf0, normal); + u32 a_i = (p_i > 0) ? (p_i - 1) : (count0 - 1); + u32 b_i = ((p_i + 1) < count0) ? (p_i + 1) : 0; + + struct v2 p = xform_mul_v2(xf0, points0[p_i]); + struct v2 a = xform_mul_v2(xf0, points0[a_i]); + struct v2 b = xform_mul_v2(xf0, points0[b_i]); + + struct v2 vap = v2_sub(p, a); + struct v2 vpb = v2_sub(b, p); + + /* FIXME: Make winding order independent */ + if (v2_wedge(vap, normal) < (v2_wedge(vpb, normal) + wedge_epsilon)) { + id_a0 = a_i; + id_b0 = p_i; + a0 = a; + b0 = p; + vab0 = vap; + } else { + id_a0 = p_i; + id_b0 = b_i; + a0 = p; + b0 = b; + vab0 = vpb; + } + } + { + struct v2 neg_normal = v2_neg(normal); + + u32 p_i = collider_support_point_index(shape1, xf1, neg_normal); + u32 a_i = ((p_i + 1) < count1) ? (p_i + 1) : 0; + u32 b_i = (p_i > 0) ? (p_i - 1) : (count1 - 1); + + struct v2 p = xform_mul_v2(xf1, points1[p_i]); + struct v2 a = xform_mul_v2(xf1, points1[a_i]); + struct v2 b = xform_mul_v2(xf1, points1[b_i]); + + struct v2 vap = v2_sub(p, a); + struct v2 vpb = v2_sub(b, p); + + /* FIXME: Make winding order independent */ + if (v2_wedge(vap, normal) < (v2_wedge(vpb, normal) + wedge_epsilon)) { + id_a1 = a_i; + id_b1 = p_i; + a1 = a; + b1 = p; + vab1 = vap; + } else { + id_a1 = p_i; + id_b1 = b_i; + a1 = p; + b1 = b; + vab1 = vpb; + } + } + + f32 a0t = 0; + f32 a1t = 0; + f32 b0t = 0; + f32 b1t = 0; + + struct v2 vba0 = v2_neg(vab0); + struct v2 vba1 = v2_neg(vab1); + { + { + struct v2 va0a1 = v2_sub(a1, a0); + struct v2 va1a0 = v2_neg(va0a1); + { + f32 w = v2_wedge(vab0, normal); + if (w != 0) { + w = 1 / w; + a0t = v2_wedge(va0a1, normal) * w; + } + } + { + f32 w = v2_wedge(vab1, normal); + if (w != 0) { + w = 1 / w; + a1t = v2_wedge(va1a0, normal) * w; + } + } + } + { + struct v2 vb0b1 = v2_sub(b1, b0); + struct v2 vb1b0 = v2_neg(vb0b1); + { + f32 w = v2_wedge(vba0, normal); + if (w != 0) { + w = 1 / w; + b0t = v2_wedge(vb0b1, normal) * w; + } + } + { + f32 w = v2_wedge(vba1, normal); + if (w != 0) { + w = 1 / w; + b1t = v2_wedge(vb1b0, normal) * w; + } + } + } + } + + a0t = clamp_f32(a0t, 0, 1); + a1t = clamp_f32(a1t, 0, 1); + b0t = clamp_f32(b0t, 0, 1); + b1t = clamp_f32(b1t, 0, 1); + + struct v2 a0_clipped = v2_add(a0, v2_mul(vab0, a0t)); + struct v2 a1_clipped = v2_add(a1, v2_mul(vab1, a1t)); + struct v2 b0_clipped = v2_add(b0, v2_mul(vba0, b0t)); + struct v2 b1_clipped = v2_add(b1, v2_mul(vba1, b1t)); + + struct v2 va0a1_clipped = v2_sub(a1_clipped, a0_clipped); + struct v2 vb0b1_clipped = v2_sub(b1_clipped, b0_clipped); + + f32 a_sep = v2_dot(va0a1_clipped, normal); + f32 b_sep = v2_dot(vb0b1_clipped, normal); + if (a_sep < tolerance) { + struct collider_collision_point *point = &points[num_points++]; + point->id = id_a0 | (id_a1 << 4); + point->separation = a_sep; + point->point = v2_add(a0_clipped, v2_mul(va0a1_clipped, 0.5f)); + } + if (b_sep < tolerance) { + struct collider_collision_point *point = &points[num_points++]; + point->id = id_b0 | (id_b1 << 4); + point->separation = b_sep; + point->point = v2_add(b0_clipped, v2_mul(vb0b1_clipped, 0.5f)); + } + } +#else + { + const f32 wedge_epsilon = 0.001f; + + /* shape0 a -> b winding = clockwise */ + u32 id_a0; + u32 id_b0; + struct v2 a0; + struct v2 b0; + struct v2 vab0; + + /* shape1 a -> b winding = counterclockwise */ + u32 id_a1; + u32 id_b1; + struct v2 a1; + struct v2 b1; + struct v2 vab1; + { + u32 p_i = collider_support_point_index(shape0, xf0, normal); + u32 a_i = (p_i > 0) ? (p_i - 1) : (count0 - 1); + u32 b_i = ((p_i + 1) < count0) ? (p_i + 1) : 0; + + struct v2 p = xform_mul_v2(xf0, points0[p_i]); + struct v2 a = xform_mul_v2(xf0, points0[a_i]); + struct v2 b = xform_mul_v2(xf0, points0[b_i]); + + struct v2 vap = v2_sub(p, a); + struct v2 vpb = v2_sub(b, p); + + /* FIXME: Make winding order independent */ + if (v2_wedge(vap, normal) < (v2_wedge(vpb, normal) + wedge_epsilon)) { + id_a0 = a_i; + id_b0 = p_i; + a0 = a; + b0 = p; + vab0 = vap; + } else { + id_a0 = p_i; + id_b0 = b_i; + a0 = p; + b0 = b; + vab0 = vpb; + } + } + { + struct v2 neg_normal = v2_neg(normal); + + u32 p_i = collider_support_point_index(shape1, xf1, neg_normal); + u32 a_i = ((p_i + 1) < count1) ? (p_i + 1) : 0; + u32 b_i = (p_i > 0) ? (p_i - 1) : (count1 - 1); + + struct v2 p = xform_mul_v2(xf1, points1[p_i]); + struct v2 a = xform_mul_v2(xf1, points1[a_i]); + struct v2 b = xform_mul_v2(xf1, points1[b_i]); + + struct v2 vap = v2_sub(p, a); + struct v2 vpb = v2_sub(b, p); + + /* FIXME: Make winding order independent */ + if (v2_wedge(vap, normal) < (v2_wedge(vpb, normal) + wedge_epsilon)) { + id_a1 = a_i; + id_b1 = p_i; + a1 = a; + b1 = p; + vab1 = vap; + } else { + id_a1 = p_i; + id_b1 = b_i; + a1 = p; + b1 = b; + vab1 = vpb; + } + } + + f32 a0t = 0; + f32 a1t = 0; + f32 b0t = 0; + f32 b1t = 0; + + struct v2 vba0 = v2_neg(vab0); + struct v2 vba1 = v2_neg(vab1); + { + { + struct v2 va0a1 = v2_sub(a1, a0); + struct v2 va1a0 = v2_neg(va0a1); + { + f32 w = v2_wedge(vab0, normal); + if (w != 0) { + w = 1 / w; + a0t = v2_wedge(va0a1, normal) * w; + } + } + { + f32 w = v2_wedge(vab1, normal); + if (w != 0) { + w = 1 / w; + a1t = v2_wedge(va1a0, normal) * w; + } + } + } + { + struct v2 vb0b1 = v2_sub(b1, b0); + struct v2 vb1b0 = v2_neg(vb0b1); + { + f32 w = v2_wedge(vba0, normal); + if (w != 0) { + w = 1 / w; + b0t = v2_wedge(vb0b1, normal) * w; + } + } + { + f32 w = v2_wedge(vba1, normal); + if (w != 0) { + w = 1 / w; + b1t = v2_wedge(vb1b0, normal) * w; + } + } + } + } + + a0t = clamp_f32(a0t, 0, 1); + a1t = clamp_f32(a1t, 0, 1); + b0t = clamp_f32(b0t, 0, 1); + b1t = clamp_f32(b1t, 0, 1); + + struct v2 a0_clipped = v2_add(a0, v2_mul(vab0, a0t)); + struct v2 a1_clipped = v2_add(a1, v2_mul(vab1, a1t)); + struct v2 b0_clipped = v2_add(b0, v2_mul(vba0, b0t)); + struct v2 b1_clipped = v2_add(b1, v2_mul(vba1, b1t)); + + struct v2 va0a1_clipped = v2_sub(a1_clipped, a0_clipped); + struct v2 vb0b1_clipped = v2_sub(b1_clipped, b0_clipped); + + f32 a_sep = v2_dot(va0a1_clipped, normal); + f32 b_sep = v2_dot(vb0b1_clipped, normal); + if (a_sep < tolerance) { + struct collider_collision_point *point = &points[num_points++]; + point->id = id_a0 | (id_a1 << 4); + point->separation = a_sep; + point->point = v2_add(a0_clipped, v2_mul(va0a1_clipped, 0.5f)); + } + if (b_sep < tolerance) { + struct collider_collision_point *point = &points[num_points++]; + point->id = id_b0 | (id_b1 << 4); + point->separation = b_sep; + point->point = v2_add(b0_clipped, v2_mul(vb0b1_clipped, 0.5f)); + } + + res.a0 = a0_clipped; + res.a1 = a1_clipped; + res.b0 = b0_clipped; + res.b1 = b1_clipped; + } +#endif + } + + 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]; + } + res.prototype.len = proto_count; + } else { + if (s.len >= 1) { + res.prototype.points[0] = s.a; + if (s.len >= 2) { + res.prototype.points[1] = s.b; + if (s.len >= 3) { + res.prototype.points[2] = s.c; + } + } + } + res.prototype.len = s.len; + } + res.normal = normal; + res.points[0] = points[0]; + res.points[1] = points[1]; + res.num_points = num_points; + res.simplex = s; + scratch_end(scratch); + return res; +} + +/* ========================== * + * Debug functions + * TODO: Remove these + * ========================== */ + + /* TODO: Remove this (debugging) */ +struct v2_array menkowski(struct arena *arena, struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1) +{ + 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(shape0, shape1, xf0, xf1, dir); + 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 collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1) +{ + /* FIXME: Account for radius */ + struct v2_array res = { .points = arena_dry_push(arena, struct v2) }; + struct v2 *points0 = shape0->points; + struct v2 *points1 = shape1->points; + u32 count0 = shape0->count; + u32 count1 = shape1->count; + for (u64 i = 0; i < count0; ++i) { + struct v2 p0 = xform_mul_v2(xf0, points0[i]); + for (u64 j = 0; j < count1; ++j) { + struct v2 p1 = xform_mul_v2(xf1, points1[j]); + *arena_push(arena, struct v2) = v2_sub(p0, p1); + ++res.count; + } + } + return res; +} + + + + + + + + + + + + + + +/* ========================== * + * Boolean GJK (unused) + * ========================== */ + +#if 0 +b32 collider_collision_boolean(struct collider_shape *shape0, struct collider_shape *shape1) { struct { struct v2 a, b, c; } s = ZI; /* FIXME: Infinite loop when shapes exactly overlap same space? */ - struct v2 dir, p; /* First point is support point in shape's general directions to eachother */ - dir = v2_sub(shape1.points[0], shape0.points[0]); + dir = v2_sub(starting_point(shape1), starting_point(shape0)); if (v2_is_zero(dir)) dir = V2(1, 0); s.a = menkowski_point(shape0, shape1, dir); @@ -100,8 +796,68 @@ b32 collider_collision_boolean(struct v2_array shape0, struct v2_array shape1) return false; } +#endif -struct collider_collision_points_result collider_collision_points(struct v2_array shape0, struct v2_array shape1) + + + + + +/* ========================== * + * Polygon-only GJK (unused) + * ========================== */ + +#if 0 +INTERNAL struct v2 collider_support_point(struct collider_shape *a, struct v2 dir) +{ + struct v2 furthest = V2(0, 0); + switch (a->kind) { + case COLLIDER_SHAPE_KIND_POLYGON: + { + /* TODO: Could probably binary search for largest dot since shape is convex */ + struct v2 *points = a->polygon.points; + u32 count = a->polygon.count; + furthest = points[0]; + f32 furthest_dot = v2_dot(dir, furthest); + for (u32 i = 1; i < count; ++i) { + struct v2 p = points[i]; + f32 dot = v2_dot(dir, p); + if (dot > furthest_dot) { + furthest = p; + furthest_dot = dot; + } + } + } break; + + case COLLIDER_SHAPE_KIND_CIRCLE: + { + dir = v2_norm(dir); + furthest = v2_add(a->circle.pos, v2_mul(dir, a->circle.radius)); + } break; + } + return furthest; +} + +INTERNAL u32 poly_support_point_index(struct collider_shape *a, struct v2 dir) +{ + /* TODO: Could probably binary search for largest dot since shape is convex */ + ASSERT(a->kind == COLLIDER_SHAPE_KIND_POLYGON); /* Method for polygons only */ + struct v2 *points = a->polygon.points; + u32 count = a->polygon.count; + u32 furthest = 0; + f32 furthest_dot = v2_dot(dir, points[0]); + for (u32 i = 1; i < count; ++i) { + struct v2 p = points[i]; + f32 dot = v2_dot(dir, p); + if (dot > furthest_dot) { + furthest = i; + furthest_dot = dot; + } + } + return furthest; +} + +struct collider_collision_points_result collider_collision_points(struct collider_shape *shape0, struct collider_shape *shape1) { struct temp_arena scratch = scratch_begin_no_conflict(); /* TODO: Only begin scratch for EPA */ struct collider_collision_points_result res = ZI; @@ -136,7 +892,7 @@ struct collider_collision_points_result collider_collision_points(struct v2_arra * ========================== */ { /* First point is support point in shape's general directions to eachother */ - s.a = menkowski_point(shape0, shape1, v2_sub(shape1.points[0], shape0.points[0])); + s.a = menkowski_point(shape0, shape1, v2_sub(starting_point(shape1), starting_point(shape0))); s.len = 1; struct v2 removed_a = ZI; @@ -372,12 +1128,11 @@ struct collider_collision_points_result collider_collision_points(struct v2_arra if (colliding) { /* ========================== * - * Clipping + * Clip to determine final points * ========================== */ - /* FIXME: Limit max vertices in shape structure to at least < 16 for id generation to be correct */ - ASSERT(shape0.count <= 16); - ASSERT(shape1.count <= 16); + /* Max vertices must be < 16 to fit in 4 bit ids */ + CT_ASSERT(ARRAY_COUNT(shape0->polygon.points) <= 16); DBGSTEP; { @@ -397,13 +1152,16 @@ struct collider_collision_points_result collider_collision_points(struct v2_arra struct v2 b1; struct v2 vab1; { - u32 p_i = poly_support_point_index(shape0, normal); - u32 a_i = (p_i > 0) ? (p_i - 1) : (shape0.count - 1); - u32 b_i = ((p_i + 1) < shape0.count) ? (p_i + 1) : 0; + struct v2 *shape0_points = shape0->polygon.points; + u32 shape0_count = shape0->polygon.count; - struct v2 p = shape0.points[p_i]; - struct v2 a = shape0.points[a_i]; - struct v2 b = shape0.points[b_i]; + u32 p_i = poly_support_point_index(shape0, normal); + u32 a_i = (p_i > 0) ? (p_i - 1) : (shape0_count - 1); + u32 b_i = ((p_i + 1) < shape0_count) ? (p_i + 1) : 0; + + struct v2 p = shape0_points[p_i]; + struct v2 a = shape0_points[a_i]; + struct v2 b = shape0_points[b_i]; struct v2 vap = v2_sub(p, a); struct v2 vpb = v2_sub(b, p); @@ -426,13 +1184,16 @@ struct collider_collision_points_result collider_collision_points(struct v2_arra { struct v2 neg_normal = v2_neg(normal); - u32 p_i = poly_support_point_index(shape1, neg_normal); - u32 a_i = ((p_i + 1) < shape1.count) ? (p_i + 1) : 0; - u32 b_i = (p_i > 0) ? (p_i - 1) : (shape1.count - 1); + struct v2 *shape1_points = shape1->polygon.points; + u32 shape1_count = shape1->polygon.count; - struct v2 p = shape1.points[p_i]; - struct v2 a = shape1.points[a_i]; - struct v2 b = shape1.points[b_i]; + u32 p_i = poly_support_point_index(shape1, neg_normal); + u32 a_i = ((p_i + 1) < shape1_count) ? (p_i + 1) : 0; + u32 b_i = (p_i > 0) ? (p_i - 1) : (shape1_count - 1); + + struct v2 p = shape1_points[p_i]; + struct v2 a = shape1_points[a_i]; + struct v2 b = shape1_points[b_i]; struct v2 vap = v2_sub(p, a); struct v2 vpb = v2_sub(b, p); @@ -563,14 +1324,14 @@ abort: * ========================== */ /* TODO: Remove this (debugging) */ -struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1) +struct v2_array menkowski(struct arena *arena, struct collider_shape *shape0, struct collider_shape *shape1) { 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(poly0, poly1, dir); + struct v2 p = menkowski_point(shape0, shape1, dir); if (res.count == 0 || !v2_eq(p, res.points[res.count - 1])) { *arena_push(arena, struct v2) = p; ++res.count; @@ -580,17 +1341,492 @@ struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_ } /* TODO: Remove this (debugging) */ -struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_array poly1) +struct v2_array cloud(struct arena *arena, struct collider_shape *shape0, struct collider_shape *shape1) { 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; + if (shape0->kind == COLLIDER_SHAPE_KIND_POLYGON && shape1->kind == COLLIDER_SHAPE_KIND_POLYGON) { + struct v2 *points0 = shape0->polygon.points; + struct v2 *points1 = shape1->polygon.points; + u32 count0 = shape0->polygon.count; + u32 count1 = shape1->polygon.count; + for (u64 i = 0; i < count0; ++i) { + struct v2 p0 = points0[i]; + for (u64 j = 0; j < count1; ++j) { + struct v2 p1 = points1[j]; + *arena_push(arena, struct v2) = v2_sub(p0, p1); + ++res.count; + } + } + } + return res; +} + + + + + + + + + +struct collider_collision_points_result collider_collision_points(struct collider_shape *shape0, struct collider_shape *shape1) +{ + struct temp_arena scratch = scratch_begin_no_conflict(); /* TODO: Only begin scratch for EPA */ + struct collider_collision_points_result res = ZI; + + /* TODO: Parameterize */ + const f32 tolerance = 0.0025f; + const f32 min_unique_pt_dist_sq = 0.001f * 0.001f; + + b32 colliding = false; + b32 simplex_is_closest_edge = false; + + struct collider_simplex s = ZI; + struct v2 *proto = NULL; + u32 proto_count = 0; + + struct v2 normal = ZI; + struct collider_collision_point points[2] = ZI; + u32 num_points = 0; + + struct v2 dir = ZI; + struct v2 m = ZI; + +#if COLLIDER_DEBUG + u32 dbg_step = 0; +#endif + + /* ========================== * + * GJK + * + * Determine encapsulating simplex if colliding, or closest edge / point to + * origin on simplex (for check if shape distances are within tolerance) + * ========================== */ + { + /* First point is support point in shape's general directions to eachother */ + s.a = menkowski_point(shape0, shape1, v2_sub(starting_point(shape1), starting_point(shape0))); + s.len = 1; + + struct v2 removed_a = ZI; + struct v2 removed_b = ZI; + u32 num_removed = 0; + b32 done = false; + while (!done) { + if (s.len == 1) { + /* Second point is support point towards origin */ + dir = v2_neg(s.a); + + DBGSTEP; + m = menkowski_point(shape0, shape1, dir); + /* Check that new point is far enough away from existing point */ + if (v2_len_sq(v2_sub(m, s.a)) < min_unique_pt_dist_sq) { + done = true; + break; + } + s.b = s.a; + s.a = m; + s.len = 2; + + /* Third point is support point in direction of line normal towards origin */ + dir = v2_perp_towards_dir(v2_sub(s.b, s.a), v2_neg(s.a)); + } + + { + DBGSTEP; + m = menkowski_point(shape0, shape1, dir); + /* Check that new point is far enough away from existing points */ + if (v2_len_sq(v2_sub(m, s.a)) < min_unique_pt_dist_sq || + v2_len_sq(v2_sub(m, s.b)) < min_unique_pt_dist_sq || + ((num_removed >= 1) && ( + (v2_len_sq(v2_sub(m, removed_a)) < min_unique_pt_dist_sq) || + (num_removed >= 2 && v2_len_sq(v2_sub(m, removed_b)) < min_unique_pt_dist_sq) + )) || + math_fabs(v2_wedge(v2_sub(s.b, s.a), v2_sub(m, s.a))) < min_unique_pt_dist_sq + ) { + simplex_is_closest_edge = true; + done = true; + break; + } + s.c = s.b; + s.b = s.a; + s.a = m; + s.len = 3; + } + + /* Determine voronoi region of the simplex in which the origin lies */ + i32 voronoi_mask = 0; + struct v2 vab = v2_sub(s.b, s.a); + struct v2 vac = v2_sub(s.c, s.a); + struct v2 vbc = v2_sub(s.c, s.b); + 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)) > 0) << 0; /* Regions ab, a, and b*/ + voronoi_mask |= (v2_dot(rac_dir, v2_neg(s.a)) > 0) << 1; /* Regions ac, a, and c */ + voronoi_mask |= (v2_dot(rbc_dir, v2_neg(s.b)) > 0) << 2; /* Regions bc, b, and c */ + /* Remove point or edge and determine next direction based on voronoi region */ + switch (voronoi_mask) { + case 0: + { /* No region, must be in simplex */ + colliding = true; + done = true; + } break; + case 1: + { /* Region ab, remove c */ + num_removed = 1; + removed_a = s.c; + s.len = 2; + dir = rab_dir; /* Next third point is in direction of region ab */ + } break; + case 2: + { /* Region ac, remove b */ + num_removed = 1; + removed_a = s.b; + s.len = 2; + s.b = s.c; + dir = rac_dir; /* Next third point is in direction of region ac */ + } break; + case 4: + { /* Region bc, remove a */ + num_removed = 1; + removed_a = s.a; + s.len = 2; + s.a = s.b; + s.b = s.c; + dir = rbc_dir; /* Next third point is in direction of region bc */ + } break; + case 3: + { /* Region a, remove bc */ + num_removed = 2; + removed_a = s.b; + removed_b = s.c; + s.len = 1; + } break; + case 5: + { /* Region b, remove ac */ + num_removed = 2; + removed_a = s.a; + removed_b = s.c; + s.len = 1; + s.a = s.b; + } break; + case 6: + { /* Region c, remove ab */ + num_removed = 2; + removed_a = s.a; + removed_b = s.b; + s.len = 1; + s.a = s.c; + } break; + default: + { + /* Unknown region (should be impossible) */ + ASSERT(false); + res.path = -1; + done = true; + } break; + } } } + if (colliding) { + /* ========================== * + * Epa (to find collision normal from inside shape) + * ========================== */ + + proto = arena_dry_push(scratch.arena, struct v2); + proto_count = 0; + { + ASSERT(s.len == 3); + struct v2 *tmp = arena_push_array(scratch.arena, struct v2, 3); + tmp[0] = s.a; + tmp[1] = s.b; + tmp[2] = s.c; + proto_count = 3; + } + + while (colliding) { + 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]; + 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_len_sq(vse))); + 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; + } + } + + /* TODO: Remove this (debugging) */ + s.a = proto[pen_ps_index]; + s.b = proto[pen_pe_index]; + s.len = 2; + + /* Find new point in dir */ + DBGSTEP; + { + /* Next point is in direction of line normal pointing outwards from simplex */ + /* TODO: If winding order is guaranteed then this can become v2_perp_left/right? */ + struct v2 a = proto[pen_ps_index]; + struct v2 b = proto[pen_pe_index]; + struct v2 n = proto[(pen_pe_index < proto_count - 1) ? (pen_pe_index + 1) : 0]; /* Next point along prototype after edge */ + struct v2 vab = v2_sub(b, a); + struct v2 vna = v2_sub(a, n); + dir = v2_perp_towards_dir(vab, vna); + } + m = menkowski_point(shape0, shape1, dir); + + /* Check unique */ + /* TODO: Better */ + { + b32 unique = true; + for (u32 i = 0; i < proto_count; ++i) { + struct v2 edge_start = proto[i]; + struct v2 edge_end = i < proto_count - 1 ? proto[i + 1] : proto[0]; + struct v2 vsm = v2_sub(m, edge_start); + if (v2_len_sq(vsm) < min_unique_pt_dist_sq || + math_fabs(v2_wedge(v2_sub(edge_end, edge_start), vsm)) < min_unique_pt_dist_sq) { + unique = false; + break; + } + } + if (!unique) { + res.path = 1; + normal = v2_norm(dir); + break; + } + } + + /* Insert point into prototype */ + /* FIXME: Preserve winding order */ + arena_push(scratch.arena, struct collider_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; + } + } else if (simplex_is_closest_edge && s.len > 1) { + /* Shapes are not overlapping (origin is outside of simplex). Project + * origin to determine if distance is within tolerance. */ + ASSERT(s.len == 2); + struct v2 vab = v2_sub(s.b, s.a); + struct v2 vao = v2_neg(s.a); + f32 ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1); + struct v2 p = v2_add(s.a, v2_mul(vab, ratio)); + if (v2_len_sq(p) <= (tolerance * tolerance)) { + res.path = 2; + normal = v2_norm(dir); + colliding = true; + } + } + + if (colliding) { + /* ========================== * + * Clip to determine final points + * ========================== */ + + /* Max vertices must be < 16 to fit in 4 bit ids */ + CT_ASSERT(ARRAY_COUNT(shape0->polygon.points) <= 16); + + DBGSTEP; + { + const f32 wedge_epsilon = 0.001f; + + /* shape0 a -> b winding = clockwise */ + u32 id_a0; + u32 id_b0; + struct v2 a0; + struct v2 b0; + struct v2 vab0; + + /* shape1 a -> b winding = counterclockwise */ + u32 id_a1; + u32 id_b1; + struct v2 a1; + struct v2 b1; + struct v2 vab1; + { + struct v2 *shape0_points = shape0->polygon.points; + u32 shape0_count = shape0->polygon.count; + + u32 p_i = poly_support_point_index(shape0, normal); + u32 a_i = (p_i > 0) ? (p_i - 1) : (shape0_count - 1); + u32 b_i = ((p_i + 1) < shape0_count) ? (p_i + 1) : 0; + + struct v2 p = shape0_points[p_i]; + struct v2 a = shape0_points[a_i]; + struct v2 b = shape0_points[b_i]; + + struct v2 vap = v2_sub(p, a); + struct v2 vpb = v2_sub(b, p); + + /* FIXME: Make winding order independent */ + if (v2_wedge(vap, normal) < (v2_wedge(vpb, normal) + wedge_epsilon)) { + id_a0 = a_i; + id_b0 = p_i; + a0 = a; + b0 = p; + vab0 = vap; + } else { + id_a0 = p_i; + id_b0 = b_i; + a0 = p; + b0 = b; + vab0 = vpb; + } + } + { + struct v2 neg_normal = v2_neg(normal); + + struct v2 *shape1_points = shape1->polygon.points; + u32 shape1_count = shape1->polygon.count; + + u32 p_i = poly_support_point_index(shape1, neg_normal); + u32 a_i = ((p_i + 1) < shape1_count) ? (p_i + 1) : 0; + u32 b_i = (p_i > 0) ? (p_i - 1) : (shape1_count - 1); + + struct v2 p = shape1_points[p_i]; + struct v2 a = shape1_points[a_i]; + struct v2 b = shape1_points[b_i]; + + struct v2 vap = v2_sub(p, a); + struct v2 vpb = v2_sub(b, p); + + /* FIXME: Make winding order independent */ + if (v2_wedge(vap, normal) < (v2_wedge(vpb, normal) + wedge_epsilon)) { + id_a1 = a_i; + id_b1 = p_i; + a1 = a; + b1 = p; + vab1 = vap; + } else { + id_a1 = p_i; + id_b1 = b_i; + a1 = p; + b1 = b; + vab1 = vpb; + } + } + + f32 a0t = 0; + f32 a1t = 0; + f32 b0t = 0; + f32 b1t = 0; + + struct v2 vba0 = v2_neg(vab0); + struct v2 vba1 = v2_neg(vab1); + { + { + struct v2 va0a1 = v2_sub(a1, a0); + struct v2 va1a0 = v2_neg(va0a1); + { + f32 w = v2_wedge(vab0, normal); + if (w != 0) { + w = 1 / w; + a0t = v2_wedge(va0a1, normal) * w; + } + } + { + f32 w = v2_wedge(vab1, normal); + if (w != 0) { + w = 1 / w; + a1t = v2_wedge(va1a0, normal) * w; + } + } + } + { + struct v2 vb0b1 = v2_sub(b1, b0); + struct v2 vb1b0 = v2_neg(vb0b1); + { + f32 w = v2_wedge(vba0, normal); + if (w != 0) { + w = 1 / w; + b0t = v2_wedge(vb0b1, normal) * w; + } + } + { + f32 w = v2_wedge(vba1, normal); + if (w != 0) { + w = 1 / w; + b1t = v2_wedge(vb1b0, normal) * w; + } + } + } + } + + a0t = clamp_f32(a0t, 0, 1); + a1t = clamp_f32(a1t, 0, 1); + b0t = clamp_f32(b0t, 0, 1); + b1t = clamp_f32(b1t, 0, 1); + + struct v2 a0_clipped = v2_add(a0, v2_mul(vab0, a0t)); + struct v2 a1_clipped = v2_add(a1, v2_mul(vab1, a1t)); + struct v2 b0_clipped = v2_add(b0, v2_mul(vba0, b0t)); + struct v2 b1_clipped = v2_add(b1, v2_mul(vba1, b1t)); + + struct v2 va0a1_clipped = v2_sub(a1_clipped, a0_clipped); + struct v2 vb0b1_clipped = v2_sub(b1_clipped, b0_clipped); + + f32 a_sep = v2_dot(va0a1_clipped, normal); + f32 b_sep = v2_dot(vb0b1_clipped, normal); + if (a_sep < tolerance) { + struct collider_collision_point *point = &points[num_points++]; + point->id = id_a0 | (id_a1 << 4); + point->separation = a_sep; + point->point = v2_add(a0_clipped, v2_mul(va0a1_clipped, 0.5f)); + } + if (b_sep < tolerance) { + struct collider_collision_point *point = &points[num_points++]; + point->id = id_b0 | (id_b1 << 4); + point->separation = b_sep; + point->point = v2_add(b0_clipped, v2_mul(vb0b1_clipped, 0.5f)); + } + } + } + + 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]; + } + res.prototype.len = proto_count; + } else { + if (s.len >= 1) { + res.prototype.points[0] = s.a; + if (s.len >= 2) { + res.prototype.points[1] = s.b; + if (s.len >= 3) { + res.prototype.points[2] = s.c; + } + } + } + res.prototype.len = s.len; + } + res.normal = normal; + res.points[0] = points[0]; + res.points[1] = points[1]; + res.num_points = num_points; + res.simplex = s; + scratch_end(scratch); return res; } +#endif diff --git a/src/collider.h b/src/collider.h index 5c2fea98..2dbd7680 100644 --- a/src/collider.h +++ b/src/collider.h @@ -1,26 +1,28 @@ #ifndef COLLIDER_H #define COLLIDER_H -#define COLLIDER_DEBUG RTC - #if COLLIDER_DEBUG extern u32 collider_debug_steps; #endif -struct collider_menkowski_point { - struct v2 p0; /* Support point of first shape in dir */ - struct v2 p1; /* Support point of second shape in -dir */ - struct v2 p; /* Menkowski difference point */ -}; +struct v2 collider_support_point(struct collider_shape *a, struct xform xf, struct v2 dir); +#if 0 /* Returns simple true or false indicating shape collision */ -b32 collider_collision_boolean(struct v2_array shape0, struct v2_array shape1); +b32 collider_collision_boolean(struct collider_shape *shape0, struct collider_shape *shape1); +#endif struct collider_simplex { u32 len; struct v2 a, b, c; }; +struct collider_menkowski_point { + struct v2 p0; /* Support point of first shape in dir */ + struct v2 p1; /* Support point of second shape in -dir */ + struct v2 p; /* Menkowski difference point */ +}; + struct collider_collision_point { struct v2 point; f32 separation; @@ -39,11 +41,12 @@ struct collider_collision_points_result { i32 path; struct collider_simplex simplex; struct collider_prototype prototype; + struct v2 a0, b0, a1, b1; /* Clipping faces */ }; -struct collider_collision_points_result collider_collision_points(struct v2_array shape0, struct v2_array shape1); +struct collider_collision_points_result collider_collision_points(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1); -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); +struct v2_array menkowski(struct arena *arena, struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1); +struct v2_array cloud(struct arena *arena, struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1); #endif diff --git a/src/common.h b/src/common.h index 653f5274..9fecbdf9 100644 --- a/src/common.h +++ b/src/common.h @@ -615,6 +615,16 @@ struct trs { f32 r; }; +/* ========================== * + * Collider types + * ========================== */ + +struct collider_shape { + struct v2 points[8]; + u32 count; + f32 radius; +}; + /* ========================== * * Common utilities * ========================== */ diff --git a/src/config.h b/src/config.h index f44215fe..6ae947a8 100644 --- a/src/config.h +++ b/src/config.h @@ -45,6 +45,8 @@ #define USER_INTERP_OFFSET_TICK_RATIO 1.1 #define USER_INTERP_ENABLED 0 +#define COLLIDER_DEBUG RTC + /* ========================== * * Settings * ========================== */ diff --git a/src/draw.c b/src/draw.c index 4cbef17b..7d221c52 100644 --- a/src/draw.c +++ b/src/draw.c @@ -4,6 +4,7 @@ #include "font.h" #include "scratch.h" #include "sprite.h" +#include "collider.h" GLOBAL struct { struct renderer_handle solid_white; @@ -269,6 +270,25 @@ void draw_solid_arrow_ray(struct renderer_canvas *canvas, struct v2 pos, struct draw_solid_arrow_line(canvas, pos, end, thickness, arrowhead_height, color); } +void draw_solid_collider_line(struct renderer_canvas *canvas, struct xform draw_xf, struct collider_shape shape, struct xform shape_xf, f32 thickness, u32 color, u32 detail) +{ + struct temp_arena scratch = scratch_begin_no_conflict(); + + struct v2 *points = arena_push_array(scratch.arena, struct v2, detail); + for (u32 i = 0; i < detail; ++i) { + f32 angle = ((f32)i / (f32)detail) * (2 * PI); + struct v2 dir = V2(math_cos(angle), math_sin(angle)); + struct v2 p = collider_support_point(&shape, shape_xf, dir); + p = xform_mul_v2(draw_xf, p); + points[i] = p; + } + + struct v2_array poly = { .points = points, .count = detail }; + draw_solid_poly_line(canvas, poly, true, thickness, color); + + scratch_end(scratch); +} + /* ========================== * * Text * ========================== */ diff --git a/src/draw.h b/src/draw.h index 2ed5896c..8880b15c 100644 --- a/src/draw.h +++ b/src/draw.h @@ -38,6 +38,7 @@ void draw_solid_quad_line(struct renderer_canvas *canvas, struct quad quad, f32 void draw_solid_rect_line(struct renderer_canvas *canvas, struct rect rect, f32 thickness, u32 color); void draw_solid_arrow_line(struct renderer_canvas *canvas, struct v2 start, struct v2 end, f32 thickness, f32 arrowhead_height, u32 color); void draw_solid_arrow_ray(struct renderer_canvas *canvas, struct v2 pos, struct v2 rel, f32 thickness, f32 arrowhead_height, u32 color); +void draw_solid_collider_line(struct renderer_canvas *canvas, struct xform draw_xf, struct collider_shape shape, struct xform shape_xf, f32 thickness, u32 color, u32 detail); void draw_text(struct renderer_canvas *canvas, struct font *font, struct v2 pos, struct string str); void draw_text_ex(struct renderer_canvas *canvas, struct font *font, struct v2 pos, f32 scale, struct string str); diff --git a/src/entity.h b/src/entity.h index 14e49bc3..9ab2ac57 100644 --- a/src/entity.h +++ b/src/entity.h @@ -115,17 +115,12 @@ struct entity { /* TODO: Remove this (testing) */ - struct entity_handle colliding_with; - struct collider_simplex simplex; - struct collider_prototype prototype; - struct v2 pendir; - b32 solved; - i32 path; + b32 colliding; + struct collider_collision_points_result res; b32 test_torque_applied; - - + struct collider_shape local_collider; diff --git a/src/game.c b/src/game.c index f75648bd..8bb9b0e3 100644 --- a/src/game.c +++ b/src/game.c @@ -131,9 +131,9 @@ INTERNAL void spawn_test_entities(f32 offset) struct entity *player_ent; { //struct v2 pos = V2(0.25, -10); - struct v2 pos = V2(0.25, -7); + //struct v2 pos = V2(0.25, -7); //struct v2 pos = V2(0.25, -5.27); - //struct v2 pos = V2(0.25, -2); + struct v2 pos = V2(0.25, -2); //struct v2 pos = V2(1.1230469346046448864129274625156, -1); /* Touching right side of box */ //struct v2 pos = V2(1.1230469346046448864129274625156 - 0.0001, -1); /* Touching right side of box */ //struct v2 pos = V2(0.374142020941, -0.246118023992); /* Touching glitch spot */ @@ -165,8 +165,8 @@ INTERNAL void spawn_test_entities(f32 offset) entity_enable_prop(e, ENTITY_PROP_PLAYER_CONTROLLED); //e->control_force = 4500; - //e->control_force = 1200; - e->control_force = 250; + e->control_force = 1200; + //e->control_force = 250; e->control_torque = 10; e->control.focus = V2(0, -1); @@ -205,13 +205,13 @@ INTERNAL void spawn_test_entities(f32 offset) } /* Box */ +#if 1 if (!G.first_spawn) { - - //struct v2 pos = V2(0.5, -1); - struct v2 pos = V2(0.5, 20); + struct v2 pos = V2(0.5, -1); + //struct v2 pos = V2(0.5, 20); //struct v2 pos = V2(1, -1); - //struct v2 size = V2(1, 1); - struct v2 size = V2(500, 50); + struct v2 size = V2(1, 1); + //struct v2 size = V2(500, 50); //f32 rot = PI / 4; f32 rot = 0; struct entity *e = entity_alloc(root); @@ -233,6 +233,7 @@ INTERNAL void spawn_test_entities(f32 offset) entity_set_xform(e, XFORM_TRS(.t = pos, .s = size, .r = rot)); } +#endif /* Camera */ if (!G.first_spawn) { @@ -279,27 +280,14 @@ INTERNAL void create_contact_manifolds(void) /* FIXME: I think it's technically possible for manifold entities to swap between iterations */ struct entity_store *store = G.tick.entity_store; - struct sprite_scope *sprite_frame_scope = G.sprite_frame_scope; struct entity *root = G.root; for (u64 e0_index = 0; e0_index < store->reserved; ++e0_index) { struct entity *e0 = &store->entities[e0_index]; if (!entity_is_valid_and_active(e0)) continue; if (!entity_has_prop(e0, ENTITY_PROP_PHYSICAL)) continue; - /* Calculate entity 0 shape */ struct xform e0_xf = entity_get_xform(e0); - struct quad e0_quad; - struct v2_array e0_poly; - { - struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, e0->sprite); - struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("shape"), e0->animation_frame); - e0_quad = xform_mul_quad(e0->sprite_local_xform, quad_from_rect(slice.rect)); - e0_quad = xform_mul_quad(e0_xf, e0_quad); - e0_poly = (struct v2_array) { - .count = ARRAY_COUNT(e0_quad.e), - .points = e0_quad.e - }; - } + struct collider_shape e0_collider = e0->local_collider; for (u64 e1_index = 0; e1_index < store->reserved; ++e1_index) { struct entity *e1 = &store->entities[e1_index]; @@ -351,34 +339,17 @@ INTERNAL void create_contact_manifolds(void) /* Calculate entity 1 shape */ struct xform e1_xf = entity_get_xform(e1); - struct quad e1_quad; - struct v2_array e1_poly; - { - struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, e1->sprite); - struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("shape"), e1->animation_frame); - e1_quad = xform_mul_quad(e1->sprite_local_xform, quad_from_rect(slice.rect)); - e1_quad = xform_mul_quad(e1_xf, e1_quad); - e1_poly = (struct v2_array) { - .count = ARRAY_COUNT(e1_quad.e), - .points = e1_quad.e - }; - } + struct collider_shape e1_collider = e1->local_collider; - struct collider_collision_points_result res = collider_collision_points(e0_poly, e1_poly); + struct collider_collision_points_result res = collider_collision_points(&e0_collider, &e1_collider, e0_xf, e1_xf); /* Parts of algorithm are hard-coded to support 2 contact points */ CT_ASSERT(ARRAY_COUNT(manifold->contacts) == 2); CT_ASSERT(ARRAY_COUNT(res.points) == 2); - /* TODO: Remove this (debugging) */ - if (manifold) { - manifold->prototype = res.prototype; - manifold->simplex = res.simplex; - manifold->solved = res.solved; - manifold->path = res.path; - } - if (res.num_points > 0) { + /* TODO: Move this down */ + if (res.num_points > 0 || COLLIDER_DEBUG) { if (!manifold) { manifold = entity_alloc(root); manifold->manifold_e0 = e0->handle; @@ -394,19 +365,21 @@ INTERNAL void create_contact_manifolds(void) *entry = manifold->handle; fixed_dict_set(&dict_arena, &dict, manifold_key, entry); } - - /* TODO: Remove this (debugging) */ - { - manifold->prototype = res.prototype; - manifold->simplex = res.simplex; - manifold->solved = res.solved; - } } + manifold->manifold_normal = res.normal; + + /* TODO: Remove this (debugging) */ + { + e0->colliding = true; + e1->colliding = true; + manifold->res = res; + } + } + + if (res.num_points > 0) { struct v2 normal = res.normal; struct v2 tangent = v2_perp(normal); - manifold->manifold_normal = normal; - /* Delete old contacts that are no longer present */ for (u32 i = 0; i < manifold->num_contacts; ++i) { struct contact *old = &manifold->contacts[i]; @@ -500,16 +473,14 @@ INTERNAL void create_contact_manifolds(void) } else if (manifold) { +#if COLLIDER_DEBUG + if (e0->valid) e0->colliding = false; + if (e1->valid) e1->colliding = false; + manifold->num_contacts = 0; +#else /* No longer colliding, delete manifold */ -#if 0 manifold->num_contacts = 0; entity_enable_prop(manifold, ENTITY_PROP_RELEASE); -#else - if (res.solved) { - manifold->prototype.len = 0; - manifold->simplex.len = 0; - } - manifold->num_contacts = 0; #endif } } @@ -600,7 +571,7 @@ INTERNAL struct soft_result make_soft(f32 hertz, f32 zeta, f32 h) INTERNAL void solve_collisions(f32 dt, b32 apply_bias) { -#if 1 +#if 0 struct entity_store *store = G.tick.entity_store; for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { struct entity *manifold = &store->entities[entity_index]; @@ -878,7 +849,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) case GAME_CMD_KIND_SPAWN_TEST: { logf_info("Spawning (test)"); -#if 1 +#if 0 for (u32 i = 0; i < 50; ++i) { spawn_test_entities(-(f32)i); } @@ -924,7 +895,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) } /* ========================== * - * Update animations from sprite + * Update entity from sprite * ========================== */ for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { @@ -934,8 +905,8 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) /* Update animation */ + struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, ent->sprite); { - struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, ent->sprite); struct sprite_sheet_span span = sprite_sheet_get_span(sheet, ent->sprite_span_name); f64 time_in_frame = ent->animation_time_in_frame + G.tick.dt; @@ -961,18 +932,26 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) /* Update sprite local xform */ - struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, ent->sprite); - struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("pivot"), ent->animation_frame); - struct v2 sprite_size = v2_div(sheet->frame_size, (f32)PIXELS_PER_UNIT); + { + struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("pivot"), ent->animation_frame); + struct v2 sprite_size = v2_div(sheet->frame_size, (f32)PIXELS_PER_UNIT); - struct v2 dir = v2_mul_v2(sprite_size, slice.dir); - f32 rot = v2_angle(dir) + PI / 2; + struct v2 dir = v2_mul_v2(sprite_size, slice.dir); + f32 rot = v2_angle(dir) + PI / 2; - struct xform xf = XFORM_IDENT; - xf = xform_rotated(xf, -rot); - xf = xform_scaled(xf, sprite_size); - xf = xform_translated(xf, v2_neg(slice.center)); - ent->sprite_local_xform = xf; + struct xform xf = XFORM_IDENT; + xf = xform_rotated(xf, -rot); + xf = xform_scaled(xf, sprite_size); + xf = xform_translated(xf, v2_neg(slice.center)); + ent->sprite_local_xform = xf; + } + + /* Update collider */ + { + struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("shape"), ent->animation_frame); + ent->local_collider = collider_from_quad(xform_mul_quad(ent->sprite_local_xform, quad_from_rect(slice.rect))); + ent->local_collider.radius = 0.1; + } } /* ========================== * @@ -1208,7 +1187,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) * Create forces from control focus (aim) * ========================== */ -#if 0 +#if 1 for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { struct entity *ent = &store->entities[entity_index]; if (!entity_is_valid_and_active(ent)) continue; @@ -1361,7 +1340,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) * Create ground friction force (gravity) * ========================== */ -#if 0 +#if 1 /* TODO: Do this globally rather than creating entities for constant forces */ for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { struct entity *ent = &store->entities[entity_index]; @@ -1423,31 +1402,28 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) * Physics * ========================== */ +#if 1 { integrate_velocities_from_forces(dt); create_contact_manifolds(); - (UNUSED)create_contact_manifolds; - (UNUSED)solve_collisions; - (UNUSED)integrate_positions_from_velocities; - (UNUSED)warm_start_contacts; - f32 substep_dt = dt / GAME_PHYSICS_SUBSTEPS; for (u32 i = 0; i < GAME_PHYSICS_SUBSTEPS; ++i) { -#if 1 #if GAME_PHYSICS_ENABLE_WARM_STARTING warm_start_contacts(); #endif solve_collisions(substep_dt, true); integrate_positions_from_velocities(substep_dt); solve_collisions(substep_dt, false); /* Relaxation */ -#else - //solve_collisions(substep_dt, true); - solve_collisions(substep_dt, false); - integrate_positions_from_velocities(substep_dt); -#endif } } +#else + (UNUSED)create_contact_manifolds; + (UNUSED)solve_collisions; + (UNUSED)integrate_velocities_from_forces; + (UNUSED)integrate_positions_from_velocities; + (UNUSED)warm_start_contacts; +#endif /* ========================== * * Initialize bullet kinematics from sources @@ -1465,7 +1441,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) struct v2 pos = xform_mul_v2(src_xf, ent->bullet_src_pos); struct v2 impulse = xform_basis_mul_v2(src_xf, ent->bullet_src_dir); - impulse = v2_mul(v2_norm(impulse), ent->bullet_impulse); + impulse = v2_with_len(impulse, ent->bullet_impulse); struct xform xf = XFORM_TRS(.t = pos, .r = v2_angle(impulse) + PI / 2); entity_set_xform(ent, xf); diff --git a/src/math.h b/src/math.h index fe47162b..9802cef8 100644 --- a/src/math.h +++ b/src/math.h @@ -671,12 +671,11 @@ INLINE struct v2 v2_perp_towards_dir(struct v2 v, struct v2 dir) return v2_perp_mul(v, (wedge >= 0) - (wedge < 0)); } -INLINE struct v2 v2_norm(struct v2 a) +INLINE struct v2 v2_with_len(struct v2 a, f32 len) { f32 l_sq = a.x * a.x + a.y * a.y; if (l_sq != 0) { - /* TODO: Benchmark vs math_rqsrt(l) */ - f32 denom = 1.f / math_sqrt(l_sq); + f32 denom = len / math_sqrt(l_sq); a.x *= denom; a.y *= denom; } @@ -694,6 +693,11 @@ INLINE struct v2 v2_clamp_len(struct v2 a, f32 max) return a; } +INLINE struct v2 v2_norm(struct v2 a) +{ + return v2_with_len(a, 1.f); +} + INLINE struct v2 v2_round(struct v2 a) { return V2(math_round(a.x), math_round(a.y)); @@ -923,8 +927,8 @@ INLINE struct xform xform_rotated_to(struct xform xf, f32 r) INLINE struct xform xform_scaled_to(struct xform xf, struct v2 s) { - xf.bx = v2_mul(v2_norm(xf.bx), s.x); - xf.by = v2_mul(v2_norm(xf.by), s.y); + xf.bx = v2_with_len(xf.bx, s.x); + xf.by = v2_with_len(xf.by, s.y); return xf; } @@ -1095,7 +1099,6 @@ INLINE struct quad quad_from_rect(struct rect rect) (struct v2) { rect.x + rect.width, rect.y }, /* Top right */ (struct v2) { rect.x + rect.width, rect.y + rect.height }, /* Bottom right */ (struct v2) { rect.x, rect.y + rect.height }, /* Bottom left */ - }; } @@ -1151,6 +1154,22 @@ INLINE struct quad quad_floor(struct quad quad) }; } +/* ========================== * + * Collider + * ========================== */ + +INLINE struct collider_shape collider_from_quad(struct quad quad) +{ + struct collider_shape res; + res.points[0] = quad.p1; + res.points[1] = quad.p2; + res.points[2] = quad.p3; + res.points[3] = quad.p4; + res.count = 4; + res.radius = 0; + return res; +} + /* ========================== * * Convex polygon * ========================== */ diff --git a/src/user.c b/src/user.c index accfce65..f2269855 100644 --- a/src/user.c +++ b/src/user.c @@ -889,8 +889,6 @@ INTERNAL void user_update(void) if (G.debug_draw && !skip_debug_draw) { struct temp_arena temp = arena_temp_begin(scratch.arena); - b32 colliding = false; - #if 0 struct font *disp_font = font_load_async(STR("res/fonts/fixedsys.ttf"), 12.0f); if (disp_font) { @@ -949,6 +947,7 @@ INTERNAL void user_update(void) } #endif +#if 0 /* Draw slices */ if (!sprite_tag_is_nil(ent->sprite)) { struct sprite_sheet *sheet = sprite_sheet_from_tag_async(sprite_frame_scope, sprite); @@ -983,65 +982,46 @@ INTERNAL void user_update(void) if (slice.has_ray) { struct v2 ray = xform_basis_mul_v2(sprite_xform, slice.dir); ray = xform_basis_mul_v2(G.world_view, ray); - ray = v2_mul(v2_norm(ray), 25); + ray = v2_with_len(ray, 25); draw_solid_arrow_ray(G.viewport_canvas, center, ray, 2, 10, ray_color); } } } } +#endif + + /* Draw collider */ + if (entity_has_prop(ent, ENTITY_PROP_PHYSICAL)) { + struct collider_shape collider = ent->local_collider; + u32 color = ent->colliding ? RGBA_32_F(1, 1, 1, 1) : RGBA_32_F(1, 1, 1, 0.25); + f32 thickness = 2; + u32 detail = 32; + //draw_solid_collider_line(G.viewport_canvas, collider, xform_mul(G.world_view, xf), thickness, color, detail); + draw_solid_collider_line(G.viewport_canvas, G.world_view, collider, xf, thickness, color, detail); + } /* Draw collision */ if (entity_has_prop(ent, ENTITY_PROP_MANIFOLD)) { struct entity *e0 = entity_from_handle(store, ent->manifold_e0); struct entity *e1 = entity_from_handle(store, ent->manifold_e1); (UNUSED)e1; - (UNUSED)colliding; struct xform e0_xf = entity_get_xform(e0); struct xform e1_xf = entity_get_xform(e1); - /* Create shapes */ - struct quad e0_quad; - struct v2_array e0_poly; - struct quad e1_quad; - struct v2_array e1_poly; - { - { - struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, e0->sprite); - struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("shape"), e0->animation_frame); - e0_quad = xform_mul_quad(e0->sprite_local_xform, quad_from_rect(slice.rect)); - e0_quad = xform_mul_quad(e0_xf, e0_quad); - e0_poly = (struct v2_array) { - .count = ARRAY_COUNT(e0_quad.e), - .points = e0_quad.e - }; - } - { - struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, e1->sprite); - struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("shape"), e1->animation_frame); - e1_quad = xform_mul_quad(e1->sprite_local_xform, quad_from_rect(slice.rect)); - e1_quad = xform_mul_quad(e1_xf, e1_quad); - e1_poly = (struct v2_array) { - .count = ARRAY_COUNT(e1_quad.e), - .points = e1_quad.e - }; - } - } - (UNUSED)e0_quad; - (UNUSED)e0_poly; - (UNUSED)e1_quad; - (UNUSED)e1_poly; + struct collider_shape e0_collider = e0->local_collider; + struct collider_shape e1_collider = e1->local_collider; -#if 0 +#if 1 /* Draw menkowski */ { - u32 color = ent->solved ? RGBA_32_F(0, 0, 0.25, 1) : RGBA_32_F(0, 0.25, 0.25, 1); + u32 color = ent->res.solved ? RGBA_32_F(0, 0, 0.25, 1) : RGBA_32_F(0, 0.25, 0.25, 1); f32 thickness = 2; (UNUSED)thickness; - struct v2_array m = menkowski(temp.arena, e0_poly, e1_poly); - //struct v2_array m = menkowski(temp.arena, e0_poly, e1_poly, v2_sub(ent->xf1.og, ent->xf0.og)); + struct v2_array m = menkowski(temp.arena, &e0_collider, &e1_collider, e0_xf, e1_xf); + //struct v2_array m = menkowski(temp.arena, e0_collider, e1_collider, v2_sub(ent->xf1.og, ent->xf0.og)); for (u64 i = 0; i < m.count; ++i) m.points[i] = xform_mul_v2(G.world_view, m.points[i]); draw_solid_poly_line(G.viewport_canvas, m, true, thickness, color); @@ -1053,7 +1033,7 @@ INTERNAL void user_update(void) u32 color = RGBA_32_F(1, 1, 1, 1); f32 radius = 2; - struct v2_array m = cloud(temp.arena, e0_poly, e1_poly); + struct v2_array m = cloud(temp.arena, &e0_collider, &e1_collider, e0_xf, e1_xf); for (u64 i = 0; i < m.count; ++i) { struct v2 p = xform_mul_v2(G.world_view, m.points[i]);; @@ -1080,8 +1060,8 @@ INTERNAL void user_update(void) u32 color = RGBA_32_F(1, 1, 1, 0.25); struct v2_array m = { - .points = ent->prototype.points, - .count = ent->prototype.len + .points = ent->res.prototype.points, + .count = ent->res.prototype.len }; for (u64 i = 0; i < m.count; ++i) m.points[i] = xform_mul_v2(G.world_view, m.points[i]); draw_solid_poly_line(G.viewport_canvas, m, true, thickness, color); @@ -1090,12 +1070,12 @@ INTERNAL void user_update(void) /* Draw simplex */ { f32 thickness = 2; - u32 line_color = colliding ? COLOR_WHITE: COLOR_YELLOW; + u32 line_color = COLOR_YELLOW; u32 color_first = RGBA_32_F(1, 0, 0, 0.75); u32 color_second = RGBA_32_F(0, 1, 0, 0.75); u32 color_third = RGBA_32_F(0, 0, 1, 0.75); - struct collider_simplex simplex = ent->simplex; + struct collider_simplex simplex = ent->res.simplex; struct v2 simplex_points[] = { simplex.a, simplex.b, simplex.c }; for (u64 i = 0; i < ARRAY_COUNT(simplex_points); ++i) simplex_points[i] = xform_mul_v2(G.world_view, simplex_points[i]); struct v2_array simplex_array = { .count = simplex.len, .points = simplex_points }; @@ -1145,7 +1125,8 @@ INTERNAL void user_update(void) struct v2 end = xform_mul_v2(G.world_view, v2_add(point, v2_mul(v2_norm(ent->manifold_normal), len))); draw_solid_arrow_line(G.viewport_canvas, start, end, arrow_thickness, arrow_height, color); } - /* Draw id */ +#if 0 + /* Draw info text */ { struct font *disp_font = font_load_async(STR("res/fonts/fixedsys.ttf"), 12.0f); if (disp_font) { @@ -1161,7 +1142,7 @@ INTERNAL void user_update(void) "separation: %F" ); struct string text = string_format(temp.arena, fmt, - FMT_SINT(ent->path), + FMT_SINT(ent->res.path), FMT_UINT(e0->handle.idx), FMT_UINT(e1->handle.idx), FMT_HEX(contact.id), @@ -1173,6 +1154,23 @@ INTERNAL void user_update(void) draw_text(G.viewport_canvas, disp_font, v2_add(v2_round(xform_mul_v2(G.world_view, point)), V2(0, offset_px)), text); } } +#endif + } + } + + /* Draw clipping */ + { + f32 thickness = 2; + u32 color = RGBA_32_F(1, 0, 1, 0.75); + { + struct v2 start = xform_mul_v2(G.world_view, ent->res.a0); + struct v2 end = xform_mul_v2(G.world_view, ent->res.b0); + draw_solid_line(G.viewport_canvas, start, end, thickness, color); + } + { + struct v2 start = xform_mul_v2(G.world_view, ent->res.a1); + struct v2 end = xform_mul_v2(G.world_view, ent->res.b1); + draw_solid_line(G.viewport_canvas, start, end, thickness, color); } } #endif