//////////////////////////////////////////////////////////// //~ Debug #if COLLIDER_DEBUG void CLD_DebugBreakable(void) { #if RtcIsEnabled DEBUGBREAKABLE; #endif } #endif //////////////////////////////////////////////////////////// //~ Shape CLD_Shape CLD_ShapeFromQuad(Quad quad) { CLD_Shape result; result.points[0] = quad.p0; result.points[1] = quad.p1; result.points[2] = quad.p2; result.points[3] = quad.p3; result.count = 4; result.radius = 0; return result; } //////////////////////////////////////////////////////////// //~ Menkowski support point CLD_SupportPoint CLD_SupportPointFromDirEx(CLD_Shape *shape, Xform xf, Vec2 dir, i32 ignore) { Vec2 *points = shape->points; u32 count = shape->count; f32 radius = shape->radius; dir = RotateVec2(dir, -RotationFromXform(xf)); dir = MulVec2Vec2(dir, ScaleFromXform(xf)); if (count == 1) { /* Skip 'ignore' on single point colliders */ ignore = -1; } Vec2 furthest = ZI; u32 furthest_index = 0; f32 furthest_dot = -F32Infinity; for (u32 i = 0; i < count; ++i) { if ((i32)i == ignore) { continue; } Vec2 p = points[i]; f32 dot = DotVec2(dir, p); if (dot > furthest_dot) { furthest = p; furthest_dot = dot; furthest_index = i; } } if (radius > 0.0) { dir = Vec2WithLen(dir, radius); furthest = AddVec2(furthest, dir); } furthest = MulXformV2(xf, furthest); CLD_SupportPoint result; result.p = furthest; result.i = furthest_index; return result; } CLD_SupportPoint CLD_SupportPointFromDir(CLD_Shape *shape, Xform xf, Vec2 dir) { return CLD_SupportPointFromDirEx(shape, xf, dir, -1); } CLD_MenkowskiPoint CLD_MenkowskiPointFromDir(CLD_Shape *shape0, CLD_Shape *shape1, Xform xf0, Xform xf1, Vec2 dir) { CLD_MenkowskiPoint result; result.s0 = CLD_SupportPointFromDir(shape0, xf0, dir); result.s1 = CLD_SupportPointFromDir(shape1, xf1, NegVec2(dir)); result.p = SubVec2(result.s0.p, result.s1.p); return result; } //////////////////////////////////////////////////////////// //~ Aabb Aabb CLD_AabbFromShape(CLD_Shape *shape, Xform xf) { Aabb result; result.p0.x = CLD_SupportPointFromDir(shape, xf, VEC2(-1, 0)).p.x - CLD_CollisionTolerance; result.p0.y = CLD_SupportPointFromDir(shape, xf, VEC2(0, -1)).p.y - CLD_CollisionTolerance; result.p1.x = CLD_SupportPointFromDir(shape, xf, VEC2(1, 0)).p.x + CLD_CollisionTolerance; result.p1.y = CLD_SupportPointFromDir(shape, xf, VEC2(0, 1)).p.y + CLD_CollisionTolerance; return result; } Aabb CLD_CombineAabb(Aabb b0, Aabb b1) { Aabb result; result.p0.x = MinF32(MinF32(b0.p0.x, b0.p1.x), MinF32(b1.p0.x, b1.p1.x)); result.p0.y = MinF32(MinF32(b0.p0.y, b0.p1.y), MinF32(b1.p0.y, b1.p1.y)); result.p1.x = MaxF32(MaxF32(b0.p0.x, b0.p1.x), MaxF32(b1.p0.x, b1.p1.x)); result.p1.y = MaxF32(MaxF32(b0.p0.y, b0.p1.y), MaxF32(b1.p0.y, b1.p1.y)); return result; } b32 CLD_TestAabb(Aabb box0, Aabb box1) { f32 b0_x0 = box0.p0.x; f32 b0_x1 = box0.p1.x; f32 b1_x0 = box1.p0.x; f32 b1_x1 = box1.p1.x; f32 b0_y0 = box0.p0.y; f32 b0_y1 = box0.p1.y; f32 b1_y0 = box1.p0.y; f32 b1_y1 = box1.p1.y; return ((b0_x0 >= b1_x0 && b0_x0 <= b1_x1) || (b0_x1 >= b1_x0 && b0_x1 <= b1_x1) || (b1_x0 >= b0_x0 && b1_x0 <= b0_x1) || (b1_x1 >= b0_x0 && b1_x1 <= b0_x1)) && ((b0_y0 >= b1_y0 && b0_y0 <= b1_y1) || (b0_y1 >= b1_y0 && b0_y1 <= b1_y1) || (b1_y0 >= b0_y0 && b1_y0 <= b0_y1) || (b1_y1 >= b0_y0 && b1_y1 <= b0_y1)); } //////////////////////////////////////////////////////////// //~ Gjk /* * Determine simplex in menkowksi difference that encapsulates origin if shapes * overlap, or closest edge / point to origin on CLD_Menkowski difference if they * do not. */ #if COLLIDER_DEBUG CLD_GjkData CLD_GjkDataFromShapes(CLD_Shape *shape0, CLD_Shape *shape1, Xform xf0, Xform xf1, f32 min_unique_pt_dist_sq, u32 dbg_step) #else CLD_GjkData CLD_GjkDataFromShapes(CLD_Shape *shape0, CLD_Shape *shape1, Xform xf0, Xform xf1, f32 min_unique_pt_dist_sq) #endif { b32 overlapping = 0; CLD_MenkowskiSimplex s = ZI; Vec2 dir = ZI; CLD_MenkowskiPoint m = ZI; /* First point is support point in shape's general directions to eachother */ dir = SubVec2(xf1.og, xf0.og); if (IsVec2Zero(dir)) dir = VEC2(1, 0); s.a = CLD_MenkowskiPointFromDir(shape0, shape1, xf0, xf1, dir); s.len = 1; Vec2 removed_a = ZI; Vec2 removed_b = ZI; u32 num_removed = 0; for (;;) { if (s.len == 1) { //- Find second point in simplex /* Second point is support point towards origin */ dir = NegVec2(s.a.p); CLD_DBGSTEP; m = CLD_MenkowskiPointFromDir(shape0, shape1, xf0, xf1, dir); /* Check that new point is far enough away from existing point */ if (Vec2LenSq(SubVec2(m.p, s.a.p)) < min_unique_pt_dist_sq) { overlapping = 0; break; } s.b = s.a; s.a = m; s.len = 2; /* Third point is support point in direction of line normal towards origin */ dir = PerpVec2TowardsDir(SubVec2(s.b.p, s.a.p), NegVec2(s.a.p)); } { //- Find third point in simplex CLD_DBGSTEP; m = CLD_MenkowskiPointFromDir(shape0, shape1, xf0, xf1, dir); /* Check that new point is far enough away from existing points */ if (Vec2LenSq(SubVec2(m.p, s.a.p)) < min_unique_pt_dist_sq || Vec2LenSq(SubVec2(m.p, s.b.p)) < min_unique_pt_dist_sq || ( (num_removed >= 1) && ( (Vec2LenSq(SubVec2(m.p, removed_a)) < min_unique_pt_dist_sq) || (num_removed >= 2 && Vec2LenSq(SubVec2(m.p, removed_b)) < min_unique_pt_dist_sq)) ) || AbsF32(WedgeVec2(SubVec2(s.b.p, s.a.p), SubVec2(m.p, s.a.p))) < min_unique_pt_dist_sq) { overlapping = 0; break; } s.c = s.b; s.b = s.a; s.a = m; s.len = 3; if ((AbsF32(WedgeVec2(SubVec2(s.b.p, s.a.p), NegVec2(s.a.p))) <= min_unique_pt_dist_sq) || (AbsF32(WedgeVec2(SubVec2(s.c.p, s.b.p), NegVec2(s.b.p))) <= min_unique_pt_dist_sq) || (AbsF32(WedgeVec2(SubVec2(s.c.p, s.a.p), NegVec2(s.a.p))) <= min_unique_pt_dist_sq)) { /* Simplex lies on origin */ overlapping = 1; break; } } //- Determine region of the simplex in which the origin lies CLD_DBGSTEP; Vec2 vab = SubVec2(s.b.p, s.a.p); Vec2 vac = SubVec2(s.c.p, s.a.p); Vec2 vbc = SubVec2(s.c.p, s.b.p); Vec2 rab_dir = PerpVec2TowardsDir(vab, NegVec2(vac)); Vec2 rac_dir = PerpVec2TowardsDir(vac, NegVec2(vab)); Vec2 rbc_dir = PerpVec2TowardsDir(vbc, vab); f32 rab_dot = DotVec2(rab_dir, NegVec2(s.a.p)); f32 rac_dot = DotVec2(rac_dir, NegVec2(s.a.p)); f32 rbc_dot = DotVec2(rbc_dir, NegVec2(s.b.p)); f32 vab_dot = DotVec2(vab, NegVec2(s.a.p)) / Vec2LenSq(vab); f32 vac_dot = DotVec2(vac, NegVec2(s.a.p)) / Vec2LenSq(vac); f32 vbc_dot = DotVec2(vbc, NegVec2(s.b.p)) / Vec2LenSq(vbc); if (rab_dot >= 0 && vab_dot >= 0 && vab_dot <= 1) { //- Region ab, remove c num_removed = 1; removed_a = s.c.p; s.len = 2; dir = rab_dir; /* Next third point is in direction of region ab */ } else if (rac_dot >= 0 && vac_dot >= 0 && vac_dot <= 1) { //- Region ac, remove b num_removed = 1; removed_a = s.b.p; s.len = 2; s.b = s.c; dir = rac_dir; /* Next third point is in direction of region ac */ } else if (rbc_dot >= 0 && vbc_dot >= 0 && vbc_dot <= 1) { //- Region bc, remove a num_removed = 1; removed_a = s.a.p; s.len = 2; s.a = s.b; s.b = s.c; dir = rbc_dir; /* Next third point is in direction of region bc */ } else if (vab_dot <= 0 && vac_dot <= 0) { //- Region a, remove bc num_removed = 2; removed_a = s.b.p; removed_b = s.c.p; s.len = 1; } else if (vab_dot >= 1 && vbc_dot <= 0) { //- Region b, remove ac num_removed = 2; removed_a = s.a.p; removed_b = s.c.p; s.len = 1; s.a = s.b; } else if (vac_dot >= 1 && vbc_dot >= 1) { //- Region c, remove ab num_removed = 2; removed_a = s.a.p; removed_b = s.b.p; s.len = 1; s.a = s.c; } else { /* No region, must be in simplex */ overlapping = 1; break; } } #if COLLIDER_DEBUG abort : #endif CLD_GjkData result = { .simplex = s, .overlapping = overlapping, .final_dir = dir, #if COLLIDER_DEBUG .dbg_step = dbg_step #endif }; return result; } //////////////////////////////////////////////////////////// //~ Epa /* Expands upon result of GJK calculation to determine collision normal & * closest edge when shapes are overlapping */ #if COLLIDER_DEBUG CLD_EpaData CLD_EpaDataFromShapes(CLD_Shape *shape0, CLD_Shape *shape1, Xform xf0, Xform xf1, CLD_GjkData gjk_result, f32 min_unique_pt_dist_sq, u32 max_iterations, u32 dbg_step) #else CLD_EpaData CLD_EpaDataFromShapes(CLD_Shape *shape0, CLD_Shape *shape1, Xform xf0, Xform xf1, CLD_GjkData gjk_result, f32 min_unique_pt_dist_sq, u32 max_iterations) #endif { TempArena scratch = BeginScratchNoConflict(); CLD_MenkowskiFeature closest_feature = ZI; Vec2 normal = ZI; CLD_MenkowskiPoint *proto = 0; u32 proto_count = 0; if (gjk_result.overlapping) { CLD_MenkowskiSimplex s = gjk_result.simplex; proto = ArenaNext(scratch.arena, CLD_MenkowskiPoint); { Assert(s.len == 3); CLD_MenkowskiPoint *tmp = PushStructsNoZero(scratch.arena, CLD_MenkowskiPoint, 3); tmp[0] = s.a; tmp[1] = s.b; tmp[2] = s.c; proto_count = 3; } i32 winding = WindingFromVec2(SubVec2(s.c.p, s.a.p), SubVec2(s.b.p, s.a.p)); u32 epa_iterations = 0; for (;;) { ++epa_iterations; /* Find dir from origin to closest edge */ /* FIXME: Winding order of ps & pe index */ f32 closest_len_sq = F32Infinity; CLD_MenkowskiPoint closest_a = ZI; CLD_MenkowskiPoint closest_b = ZI; u32 closest_b_index = 0; for (u32 i = 0; i < proto_count; ++i) { u32 a_index = i; u32 b_index = (i < proto_count - 1) ? (i + 1) : 0; CLD_MenkowskiPoint a = proto[a_index]; CLD_MenkowskiPoint b = proto[b_index]; Vec2 vab = SubVec2(b.p, a.p); Vec2 vao = NegVec2(a.p); f32 proj_ratio = ClampF32(DotVec2(vao, vab) / Vec2LenSq(vab), 0, 1); Vec2 proj = AddVec2(a.p, MulVec2(vab, proj_ratio)); f32 proj_len_sq = Vec2LenSq(proj); if (proj_len_sq < closest_len_sq - min_unique_pt_dist_sq) { closest_a = a; closest_b = b; closest_b_index = b_index; closest_len_sq = proj_len_sq; } } Vec2 vab = SubVec2(closest_b.p, closest_a.p); /* Find new point in dir */ Vec2 dir = MulVec2(PerpVec2(vab), winding); CLD_MenkowskiPoint m = CLD_MenkowskiPointFromDir(shape0, shape1, xf0, xf1, dir); #if COLLIDER_DEBUG { /* If debug step count is reached, we still want to inspect the normal at the step */ normal = NormVec2(dir); closest_feature.a = closest_a; closest_feature.b = closest_b; closest_feature.len = 2; } #endif /* Check validity of new point */ CLD_DBGSTEP; { b32 valid = 1; { /* NOTE: Changing this value affects how stable normals are for circular colliders */ //const f32 validity_epsilon = min_unique_pt_dist_sq; /* Arbitrary */ //const f32 validity_epsilon = 0.00000000001f; /* Arbitrary */ const f32 validity_epsilon = min_unique_pt_dist_sq; /* Arbitrary */ Vec2 vam = SubVec2(m.p, closest_a.p); Vec2 vbm = SubVec2(closest_b.p, closest_a.p); f32 dot = DotVec2(vab, vam) / Vec2LenSq(vab); if (dot >= -validity_epsilon && dot <= 1 - validity_epsilon && (WedgeVec2(vab, vam) * -winding) >= -validity_epsilon) { /* New point is not between edge */ valid = 0; } else if (Vec2LenSq(vam) < min_unique_pt_dist_sq || Vec2LenSq(vbm) < min_unique_pt_dist_sq) { /* New point is too close to existing */ valid = 0; } } if (!valid || epa_iterations >= max_iterations) { normal = NormVec2(dir); closest_feature.a = closest_a; closest_feature.b = closest_b; closest_feature.len = 2; break; } } /* Expand prototype */ PushStructNoZero(scratch.arena, CLD_MenkowskiPoint); ++proto_count; /* Shift points in prototype to make room */ for (u32 i = proto_count - 1; i > closest_b_index; --i) { u32 shift_from = (i > 0) ? i - 1 : proto_count - 1; u32 shift_to = i; proto[shift_to] = proto[shift_from]; } /* Insert new point into prototype */ proto[closest_b_index] = m; } } else { normal = NormVec2(gjk_result.final_dir); closest_feature.len = gjk_result.simplex.len; closest_feature.a = gjk_result.simplex.a; closest_feature.b = gjk_result.simplex.b; } #if COLLIDER_DEBUG abort : #endif CLD_EpaData result = { .normal = normal, .closest_feature = closest_feature }; #if COLLIDER_DEBUG result.dbg_step = dbg_step; u32 len = MinU32(proto_count, countof(result.prototype.points)); for (u32 i = 0; i < len; ++i) { result.prototype.points[i] = proto[i].p; } result.prototype.len = len; #endif EndScratch(scratch); return result; } //////////////////////////////////////////////////////////// //~ Clipping CLD_ClippedLine CLD_ClipLineToLine(Vec2 a0, Vec2 b0, Vec2 a1, Vec2 b1, Vec2 normal) { Vec2 vab0 = SubVec2(b0, a0); Vec2 vab1 = SubVec2(b1, a1); Vec2 va0a1 = SubVec2(a1, a0); Vec2 vb0b1 = SubVec2(b1, b0); f32 vab0_w = WedgeVec2(vab0, normal); f32 vab1_w = WedgeVec2(vab1, normal); f32 va0a1_w = WedgeVec2(va0a1, normal); f32 vb0b1_w = WedgeVec2(vb0b1, normal); /* FIXME: Handle 0 denominator */ f32 a0t; f32 b0t; { f32 w = 1 / vab0_w; a0t = ClampF32(va0a1_w * w, 0, 1); b0t = ClampF32(vb0b1_w * -w, 0, 1); } f32 a1t; f32 b1t; { f32 w = 1 / vab1_w; a1t = ClampF32(-va0a1_w * w, 0, 1); b1t = ClampF32(-vb0b1_w * -w, 0, 1); } CLD_ClippedLine result; result.a0_clipped = AddVec2(a0, MulVec2(vab0, a0t)); result.a1_clipped = AddVec2(a1, MulVec2(vab1, a1t)); result.b0_clipped = AddVec2(b0, MulVec2(vab0, -b0t)); result.b1_clipped = AddVec2(b1, MulVec2(vab1, -b1t)); return result; } Vec2 CLD_ClipPointToLine(Vec2 a, Vec2 b, Vec2 p, Vec2 normal) { Vec2 vab = SubVec2(b, a); Vec2 vap = SubVec2(p, a); f32 vab_w = WedgeVec2(vab, normal); f32 vap_w = WedgeVec2(vap, normal); f32 t; { f32 w = 1 / vab_w; t = ClampF32(vap_w * w, 0, 1); } Vec2 result = AddVec2(a, MulVec2(vab, t)); return result; } //////////////////////////////////////////////////////////// //~ Collision points CLD_CollisionData CLD_CollisionDataFromShapes(CLD_Shape *shape0, CLD_Shape *shape1, Xform xf0, Xform xf1) { CLD_CollisionData result = ZI; const f32 tolerance = CLD_CollisionTolerance; const f32 min_unique_pt_dist_sq = CLD_MinUniquePtDistSq; const u32 max_epa_iterations = CLD_MaxEpaIterations; CLD_CollisionPoint points[2] = ZI; u32 num_points = 0; b32 colliding = 0; Vec2 normal = ZI; #if COLLIDER_DEBUG u32 dbg_step = 0; #endif CLD_GjkData gjk_result = ZI; CLD_EpaData epa_result = ZI; /* Run GJK */ #if COLLIDER_DEBUG gjk_result = CLD_GjkDataFromShapes(shape0, shape1, xf0, xf1, min_unique_pt_dist_sq, dbg_step); dbg_step = gjk_result.dbg_step; #else gjk_result = CLD_GjkDataFromShapes(shape0, shape1, xf0, xf1, min_unique_pt_dist_sq); #endif CLD_DBGSTEP; /* Run EPA */ #if COLLIDER_DEBUG epa_result = CLD_EpaDataFromShapes(shape0, shape1, xf0, xf1, gjk_result, min_unique_pt_dist_sq, max_epa_iterations, dbg_step); dbg_step = epa_result.dbg_step; #else epa_result = CLD_EpaDataFromShapes(shape0, shape1, xf0, xf1, gjk_result, min_unique_pt_dist_sq, max_epa_iterations); #endif normal = epa_result.normal; CLD_DBGSTEP; /* Determine collision */ if (gjk_result.overlapping) { colliding = 1; } else { CLD_MenkowskiFeature f = epa_result.closest_feature; /* Shapes not overlapping, determine if distance between shapes within tolerance */ if (f.len == 1) { Vec2 p = NegVec2(f.a.p); if (Vec2LenSq(p) <= (tolerance * tolerance)) { colliding = 1; } } else { /* Project origin to determine if distance is within tolerance. */ Assert(f.len == 2); Vec2 vab = SubVec2(f.b.p, f.a.p); Vec2 vao = NegVec2(f.a.p); f32 ratio = ClampF32(DotVec2(vab, vao) / DotVec2(vab, vab), 0, 1); Vec2 p = AddVec2(f.a.p, MulVec2(vab, ratio)); if (Vec2LenSq(p) <= (tolerance * tolerance)) { colliding = 1; } } } /* Clip to determine final points */ if (colliding) { /* Max vertices must be < 16 to fit in 4 bit ids */ StaticAssert(countof(shape0->points) <= 16); CLD_MenkowskiFeature f = epa_result.closest_feature; { b32 collapse0 = 0; b32 collapse1 = 0; CLD_SupportPoint a0 = f.a.s0; CLD_SupportPoint a1 = f.a.s1; CLD_SupportPoint b0 = f.b.s0; CLD_SupportPoint b1 = f.b.s1; /* FIXME: Manually account for shapes w/ 1 & 2 points */ if (f.len == 2) { if (a0.i == b0.i) { if (shape0->count > 1) { b0 = CLD_SupportPointFromDirEx(shape0, xf0, normal, b0.i); } else { collapse0 = 1; b0 = a0; } } if (a1.i == b1.i) { if (shape1->count > 1) { b1 = CLD_SupportPointFromDirEx(shape1, xf1, NegVec2(normal), b1.i); } else { collapse1 = 1; b1 = a1; } } } else { collapse0 = 1; collapse1 = 1; b0 = a0; b1 = a1; } Vec2 vab0 = SubVec2(b0.p, a0.p); Vec2 vab1 = SubVec2(b1.p, a1.p); Vec2 vab0_norm = NormVec2(vab0); Vec2 vab1_norm = NormVec2(vab1); /* Swap points based on normal direction for consistent clipping */ if (WedgeVec2(normal, vab0) < 0) { CLD_SupportPoint tmp = a0; a0 = b0; b0 = tmp; vab0 = NegVec2(vab0); } if (WedgeVec2(normal, vab1) < 0) { CLD_SupportPoint tmp = a1; a1 = b1; b1 = tmp; vab1 = NegVec2(vab1); } /* Collapse lines that are too far in the direction of the normal to be accurately clipped */ f32 collapse_epsilon = 0.05f; collapse0 = collapse0 || AbsF32(WedgeVec2(normal, vab0_norm)) < collapse_epsilon; collapse1 = collapse1 || AbsF32(WedgeVec2(normal, vab1_norm)) < collapse_epsilon; /* Collapse lines into deepest point */ if (collapse0) { if (DotVec2(normal, vab0) > 0) { a0 = b0; } else { /* TODO: Remove this (debugging) */ b0 = a0; } } if (collapse1) { if (DotVec2(normal, vab1) < 0) { a1 = b1; } else { /* TODO: Remove this (debugging) */ b1 = a1; } } f32 a_sep = F32Infinity; f32 b_sep = F32Infinity; Vec2 a_midpoint = ZI; Vec2 b_midpoint = ZI; b32 ignore_a = 1; b32 ignore_b = 1; if (!collapse0 && !collapse1) { /* Clip line to line */ CLD_ClippedLine clip_result = CLD_ClipLineToLine(a0.p, b0.p, a1.p, b1.p, normal); Vec2 a0_clipped = clip_result.a0_clipped; Vec2 a1_clipped = clip_result.a1_clipped; Vec2 b0_clipped = clip_result.b0_clipped; Vec2 b1_clipped = clip_result.b1_clipped; /* Calc midpoint between clipped a & b */ Vec2 va0a1_clipped = SubVec2(a1_clipped, a0_clipped); Vec2 vb0b1_clipped = SubVec2(b1_clipped, b0_clipped); a_sep = DotVec2(va0a1_clipped, normal); b_sep = DotVec2(vb0b1_clipped, normal); a_midpoint = AddVec2(a0_clipped, MulVec2(va0a1_clipped, 0.5f)); b_midpoint = AddVec2(b0_clipped, MulVec2(vb0b1_clipped, 0.5f)); ignore_a = 0; ignore_b = 0; Vec2 vfin = SubVec2(b_midpoint, a_midpoint); if (Vec2LenSq(vfin) < (0.005 * 0.005)) { if (a_sep > b_sep) { ignore_a = 1; } else { ignore_b = 1; } } result.a0_clipped = a0_clipped; result.a1_clipped = a1_clipped; result.b0_clipped = b0_clipped; result.b1_clipped = b1_clipped; } else { Vec2 p0 = a0.p; Vec2 p1 = a1.p; /* TODO: Choose ID based on closest clipped point */ if (collapse1 && !collapse0) { /* Project a1 onto vab0 */ p0 = CLD_ClipPointToLine(a0.p, b0.p, a1.p, normal); } if (collapse0 && !collapse1) { /* Project a0 onto vab1 */ p1 = CLD_ClipPointToLine(a1.p, b1.p, a0.p, normal); } /* Calc midpoint */ Vec2 vsep = SubVec2(p1, p0); a_midpoint = AddVec2(p0, MulVec2(vsep, 0.5f)); a_sep = DotVec2(normal, p1) - DotVec2(normal, p0); ignore_a = 0; result.a0_clipped = p0; result.a1_clipped = p1; result.b0_clipped = p0; result.b1_clipped = p1; } /* Insert points */ if (!ignore_a && a_sep < tolerance) { CLD_CollisionPoint *point = &points[num_points++]; point->id = a0.i | (a1.i << 4); point->separation = a_sep; point->point = a_midpoint; } if (!ignore_b && b_sep < tolerance) { CLD_CollisionPoint *point = &points[num_points++]; point->id = b0.i | (b1.i << 4); point->separation = b_sep; point->point = b_midpoint; } result.a0 = a0.p; result.a1 = a1.p; result.b0 = b0.p; result.b1 = b1.p; } } #if COLLIDER_DEBUG result.solved = 1; abort: result.simplex = gjk_result.simplex; result.prototype.len = epa_result.prototype.len; CopyBytes(result.prototype.points, epa_result.prototype.points, sizeof(result.prototype.points[0]) * result.prototype.len); #endif result.normal = normal; result.points[0] = points[0]; result.points[1] = points[1]; result.num_points = num_points; return result; } //////////////////////////////////////////////////////////// //~ Closest points /* TODO: De-duplicate code between CLD_ClosestPointDataFromShapes & CLD_CollisionDataFromShapes */ CLD_ClosestPointData CLD_ClosestPointDataFromShapes(CLD_Shape *shape0, CLD_Shape *shape1, Xform xf0, Xform xf1) { CLD_ClosestPointData result = ZI; const f32 tolerance = CLD_CollisionTolerance; const f32 min_unique_pt_dist_sq = CLD_MinUniquePtDistSq; const u32 max_epa_iterations = CLD_MaxEpaIterations; Vec2 p0 = ZI; Vec2 p1 = ZI; b32 colliding = 0; #if COLLIDER_DEBUG u32 dbg_step = 0; #endif CLD_GjkData gjk_result = ZI; CLD_EpaData epa_result = ZI; //- Run GJK #if COLLIDER_DEBUG gjk_result = CLD_GjkDataFromShapes(shape0, shape1, xf0, xf1, min_unique_pt_dist_sq, dbg_step); dbg_step = gjk_result.dbg_step; #else gjk_result = CLD_GjkDataFromShapes(shape0, shape1, xf0, xf1, min_unique_pt_dist_sq); #endif CLD_DBGSTEP; //- Run EPA #if COLLIDER_DEBUG epa_result = CLD_EpaDataFromShapes(shape0, shape1, xf0, xf1, gjk_result, min_unique_pt_dist_sq, max_epa_iterations, dbg_step); dbg_step = epa_result.dbg_step; #else epa_result = CLD_EpaDataFromShapes(shape0, shape1, xf0, xf1, gjk_result, min_unique_pt_dist_sq, max_epa_iterations); #endif CLD_DBGSTEP; //- Resolve points colliding = gjk_result.overlapping; CLD_MenkowskiFeature f = epa_result.closest_feature; if (f.len == 1) { p0 = f.a.s0.p; p1 = f.a.s1.p; colliding = gjk_result.overlapping || Vec2LenSq(NegVec2(f.a.p)) <= (tolerance * tolerance); } else { Assert(f.len == 2); /* FIXME: Winding order dependent? */ f32 ratio; { /* Determine ratio between edge a & b that projected origin lies */ Vec2 vab = SubVec2(f.b.p, f.a.p); Vec2 vao = NegVec2(f.a.p); ratio = ClampF32(DotVec2(vab, vao) / DotVec2(vab, vab), 0, 1); } /* Shape 0 */ p0 = SubVec2(f.b.s0.p, f.a.s0.p); p0 = MulVec2(p0, ratio); p0 = AddVec2(p0, f.a.s0.p); /* Shape 1 */ p1 = SubVec2(f.b.s1.p, f.a.s1.p); p1 = MulVec2(p1, ratio); p1 = AddVec2(p1, f.a.s1.p); colliding = gjk_result.overlapping || Vec2LenSq(SubVec2(p1, p0)) <= (tolerance * tolerance); } #if COLLIDER_DEBUG result.solved = 1; abort: result.simplex = gjk_result.simplex; result.prototype.len = epa_result.prototype.len; CopyBytes(result.prototype.points, epa_result.prototype.points, sizeof(result.prototype.points[0]) * result.prototype.len); result.simplex = gjk_result.simplex; #endif result.p0 = p0; result.p1 = p1; result.colliding = colliding; return result; } //////////////////////////////////////////////////////////// //~ Time of impact /* Takes 2 shapes and their xforms at t=0 and t=1. * Returns time of impact in range [0, 1]. */ f32 CLD_TimeOfImpact(CLD_Shape *c0, CLD_Shape *c1, Xform xf0_t0, Xform xf1_t0, Xform xf0_t1, Xform xf1_t1, f32 tolerance, u32 max_iterations) { f32 t0 = 0; f32 t1 = 1; f32 t0_sep = 0; f32 t1_sep = 0; f32 t = 0; f32 t_sep = F32Infinity; /* Find direction p0 -> p1 at t=0 */ Vec2 dir; Vec2 dir_neg; { CLD_ClosestPointData closest_points = CLD_ClosestPointDataFromShapes(c0, c1, xf0_t0, xf1_t0); if (closest_points.colliding) { /* Shapes are penetrating at t=0 */ return 0; } dir = SubVec2(closest_points.p1, closest_points.p0); t0_sep = Vec2Len(dir); dir = DivVec2(dir, t0_sep); /* Normalize */ dir_neg = NegVec2(dir); } { Vec2 p0 = CLD_SupportPointFromDir(c0, xf0_t1, dir).p; Vec2 p1 = CLD_SupportPointFromDir(c1, xf1_t1, dir_neg).p; t1_sep = DotVec2(dir, SubVec2(p1, p0)); if (t1_sep > 0) { /* Shapes are not penetrating at t=1 */ return 1; } } u32 iteration = 0; while (AbsF32(t_sep) > tolerance && iteration < max_iterations) { /* Use mix of bisection & 0 position method to find root * (as described in https://box2d.org/files/ErinCatto_ContinuousCollision_GDC2013.pdf) */ if (iteration & 1) { /* Bisect */ t = (t1 + t0) / 2.0; } else { /* False position (fastest for linear case) */ f32 m = (t1_sep - t0_sep) / (t1 - t0); t = (-t1_sep / m) + t1; } Xform xf0 = LerpXform(xf0_t0, xf0_t1, t); Xform xf1 = LerpXform(xf1_t0, xf1_t1, t); Vec2 p0 = CLD_SupportPointFromDir(c0, xf0, dir).p; Vec2 p1 = CLD_SupportPointFromDir(c1, xf1, dir_neg).p; t_sep = DotVec2(dir, SubVec2(p1, p0)); /* Update bracket */ if (t_sep > 0) { t0 = t; t0_sep = t_sep; } else { t1 = t; t1_sep = t_sep; } ++iteration; } return t; } //////////////////////////////////////////////////////////// //~ Point cloud debugging /* TODO: Remove this (debugging) */ Vec2Array CLD_Menkowski(Arena *arena, CLD_Shape *shape0, CLD_Shape *shape1, Xform xf0, Xform xf1, u32 detail) { Vec2Array result = { .points = ArenaNext(arena, Vec2) }; for (u64 i = 0; i < detail; ++i) { f32 angle = ((f32)i / detail) * (2 * Pi); Vec2 dir = Vec2FromAngle(angle); CLD_MenkowskiPoint m = CLD_MenkowskiPointFromDir(shape0, shape1, xf0, xf1, dir); if (result.count == 0 || !MatchVec2(m.p, result.points[result.count - 1])) { *PushStructNoZero(arena, Vec2) = m.p; ++result.count; } } return result; } /* TODO: Remove this (debugging) */ Vec2Array CLD_PointCloud(Arena *arena, CLD_Shape *shape0, CLD_Shape *shape1, Xform xf0, Xform xf1) { /* FIXME: Account for radius */ Vec2Array result = { .points = ArenaNext(arena, Vec2) }; Vec2 *points0 = shape0->points; Vec2 *points1 = shape1->points; u32 count0 = shape0->count; u32 count1 = shape1->count; for (u64 i = 0; i < count0; ++i) { Vec2 p0 = MulXformV2(xf0, points0[i]); for (u64 j = 0; j < count1; ++j) { Vec2 p1 = MulXformV2(xf1, points1[j]); *PushStructNoZero(arena, Vec2) = SubVec2(p0, p1); ++result.count; } } return result; } //////////////////////////////////////////////////////////// //~ Boolean GJK (unused) #if 0 b32 CLD_GjkBoolean(CLD_Shape *shape0, CLD_Shape *shape1) { struct { Vec2 a, b, c; } s = ZI; /* FIXME: Infinite loop when shapes exactly overlap same space? */ Vec2 dir, p; /* First point is support point in shape's general directions to eachother */ dir = SubVec2(starting_point(shape1), starting_point(shape0)); if (IsVec2Zero(dir)) dir = VEC2(1, 0); s.a = CLD_MenkowskiPointFromDir(shape0, shape1, dir); /* Second point is support point towards origin */ dir = NegVec2(s.a); p = CLD_MenkowskiPointFromDir(shape0, shape1, dir); if (DotVec2(dir, p) >= 0) { s.b = s.a; s.a = p; for (;;) { /* Third point is support point in direction of line normal towards origin */ dir = PerpVec2TowardsDir(SubVec2(s.b, s.a), NegVec2(s.a)); p = CLD_MenkowskiPointFromDir(shape0, shape1, dir); if (DotVec2(dir, p) < 0) { /* New point did not cross origin, collision impossible */ break; } s.c = s.b; s.b = s.a; s.a = p; Vec2 vab = SubVec2(s.b, s.a); Vec2 vac = SubVec2(s.c, s.a); Vec2 a_to_origin = NegVec2(s.a); dir = PerpVec2TowardsDir(vab, NegVec2(vac)); /* Normal of ab pointing away from c */ if (DotVec2(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 = PerpVec2TowardsDir(vac, NegVec2(vab)); /* Normal of ac pointing away from b */ if (DotVec2(dir, a_to_origin) >= 0) { /* Point is in region ac, remove b from simplex */ s.b = s.c; } else { /* Point is in simplex */ return 1; } } } } return 0; } #endif