Anyway I noticed there is no btCapsuleCapsuleCollisionAlgorithm and maybe there's a good reason of which I'm unaware in my ignorance, but over the years I've learned things from this project and I thought I'd try to give back by posting an attempt of my own.
Things to note: I bound all of my shapes in an oriented-bounding-box and interpret the values of the box to describe, in this case, the capsule. I didn't include the support functions (e.g. IntersectRayCapsule), because those are well known and published. The CalcIntercept function is a bit overkill here -- it really just represents a 2D-circles-over-time TOI, but I just used the 3D-spheres-over-time version for my convenience (i.e. I was being lazy). I don't calculate a normal or point of contact, but this is trivial.
There are comments inline, which should give any reader a good start, but I'm certainly willing to expand, if this generates interest/questions. Also because I have literally just written this, there are possible bugs and/or gaps in my logic. Presumably once you possess a robust MovingCapsuleSegmentIntersect you can use it to implement, for example, MovingCapsuleTriangleIntersect.
Code: Select all
struct OBB
{
Transform transform;
Vector3 extents;
};
struct ContinuousCollisionData
{
OBB Last;
Vector3 Motion;
};
static bool MovingCapsuleSegmentIntersect(
ContinuousCollisionData const& capsule,
Vector3 const& d,
Vector3 const& a,
Vector3 const& b,
decimal segRadius,
decimal& TOIout
)
{
// Bring segment and relative motion into capsule's space.
auto const worldToLocal{ capsule.Last.transform.getInverse() };
auto const p{ worldToLocal * a };
auto const q{ worldToLocal * b };
auto const r{ worldToLocal.getOrientation() * d };
auto const R{ capsule.Last.extents.x + segRadius };
auto const upA{ capsule.Last.extents.y - capsule.Last.extents.x };
auto const s{ q - p };
auto const ss{ s.x * s.x + s.z * s.z };
// Reduce the problem to a 2D moving circle colliding with a stationary 2D segment.
// Find closest 2D point on segment to circle.
auto t{ -(s.x * p.x + s.z * p.z) };
if (t != 0) // Prevent potential divide-by-zero.
{
t /= ss;
}
Vector3 pq;
if (t <= 0)
{
pq = p;
}
else if (t >= 1)
{
pq = q;
}
else
{
pq = p + t * s;
}
auto const RR{ R * R };
if (pq.x * pq.x + pq.z * pq.z <= RR)
{
// 2D circle and segment already overlap; consider the third dimension.
// If the closest point in 2D is outside of the
// capped cylinder in 3D, bring the appropriate end
// cap of the capsule into play.
if (pq.y < -upA)
{
return IntersectRayCapsule(
Ray{ Vector3{ 0, -upA, 0 }, Vector3{ 0, -upA, 0 } + r },
p,
q,
R,
TOIout
);
}
else if (pq.y > upA)
{
return IntersectRayCapsule(
Ray{ Vector3{ 0, upA, 0 }, Vector3{ 0, upA, 0 } + r },
p,
q,
R,
TOIout
);
}
TOIout = 0;
return true;
}
// Circle and segment are disjoint.
// Determine closest point to circle's center on the line of the segment.
auto cx{ p.x + t * s.x };
auto cz{ p.z + t * s.z };
auto const cc{ cx * cx + cz * cz };
if (cc <= RR)
{
// The problem reduces to determining the time of intersection for
// the closest segment point (the tip of the spear) and the circle.
if (!CalcIntercept(
Vector3{ pq.x, 0, pq.z },
Vector3{ r.x, 0, r.z },
R,
0,
t
))
{
return false;
}
}
else
{
auto rxs{ r.x * s.z - r.z * s.x };
auto constexpr EPSILON{ 1e-06f };
if (decimal::abs(rxs) < EPSILON)
{
// Circle motion is parallel to segment.
return false;
}
// Implicitly determine intersection point of circle's motion
// with stationary segment (r * t).
//t = (p.x * s.z - p.z * s.x) / rxs;
t = (p.x * s.z - p.z * s.x);
// Using similar triangles, refine t to account for
// circle's radius.
//t = t * (1 - R / decimal::sqrt(cc));
t = (t - t * R / decimal::sqrt(cc)) / rxs;
// We now have a time t where the circle
// contacts the line of the segment. If
// the resulting point of contact is not
// on the segment, then test the relevant cap.
pq = r * t;
auto tt{ s.x * (pq.x - p.x) + s.z * (pq.z - p.z) };
if (tt < 0)
{
if (!CalcIntercept(
Vector3{ p.x, 0, p.z },
Vector3{ r.x, 0, r.z },
R,
0,
t
))
{
return false;
}
}
else if (tt > ss)
{
if (!CalcIntercept(
Vector3{ q.x, 0, q.z },
Vector3{ r.x, 0, r.z },
R,
0,
t
))
{
return false;
}
}
}
if (t >= 0 && t <= 1)
{
// The circle touches the segment at time t. Calculate 3D point
// of impact on the infinite 3D cylinder of the circle.
// Move the circle.
pq = r * t;
auto const tt{ s.x * (pq.x - p.x) + s.z * (pq.z - p.z) };
auto const cy{ p.y + tt * s.y / ss - pq.y };
// If the intersection point is outside of the
// capped cylinder, bring the appropriate end
// cap of the capsule into play. Otherwise time
// t is correct.
if (cy < -upA)
{
return IntersectRayCapsule(
Ray{ Vector3{ 0, -upA, 0 }, Vector3{ 0, -upA, 0 } + r },
p,
q,
R,
TOIout
);
}
else if (cy > upA)
{
return IntersectRayCapsule(
Ray{ Vector3{ 0, upA, 0 }, Vector3{ 0, upA, 0 } + r },
p,
q,
R,
TOIout
);
}
TOIout = t;
return true;
}
return false;
}
bool MovingCapsuleCapsuleIntersect(
ContinuousCollisionData const& capsuleA,
ContinuousCollisionData const& capsuleB,
decimal& TOIout
)
{
auto upB{ capsuleB.Last.transform.getOrientation().upVector() * (capsuleB.Last.extents.y - capsuleB.Last.extents.x) };
return MovingCapsuleSegmentIntersect(
capsuleA,
capsuleA.Motion - capsuleB.Motion,
capsuleB.Last.transform.getPosition() - upB,
capsuleB.Last.transform.getPosition() + upB,
capsuleB.Last.extents.x,
TOIout
);
}