Code:
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 );}
)