997 lines
33 KiB
C
997 lines
33 KiB
C
////////////////////////////////
|
|
//~ 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_SupportPointFromDirInternal(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_SupportPointFromDirInternal(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) {
|
|
/* 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));
|
|
}
|
|
|
|
{
|
|
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 = PushDry(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_SupportPointFromDirInternal(shape0, xf0, normal, b0.i);
|
|
} else {
|
|
collapse0 = 1;
|
|
b0 = a0;
|
|
}
|
|
}
|
|
if (a1.i == b1.i) {
|
|
if (shape1->count > 1) {
|
|
b1 = CLD_SupportPointFromDirInternal(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 = PushDry(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 || !EqVec2(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 = PushDry(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
|