fix capsule-vs-capule collision test

libraries/shared/src/ShapeCollider.cpp

@@ -343,6 +343,72 @@ bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, Col
     return false;
 }
 
+/// \param lineP point on line
+/// \param lineDir normalized direction of line
+/// \param cylinderP point on cylinder axis
+/// \param cylinderDir normalized direction of cylinder axis
+/// \param cylinderRadius radius of cylinder
+/// \param hitLow[out] distance from point on line to first intersection with cylinder
+/// \param hitHigh[out] distance from point on line to second intersection with cylinder
+/// \return true if line hits cylinder
+bool lineCylinder(const glm::vec3& lineP, const glm::vec3& lineDir, 
+        const glm::vec3& cylinderP, const glm::vec3& cylinderDir, float cylinderRadius,
+        float& hitLow, float& hitHigh) {
+
+    // first handle parallel case
+    float uDotV = glm::dot(lineDir, cylinderDir);
+    if (fabsf(1.0f - fabsf(uDotV)) < EPSILON) {
+        // line and cylinder are parallel
+        if (glm::distance2(lineP, cylinderP) <= cylinderRadius * cylinderRadius) {
+            // line is inside cylinder, which we consider a hit
+            hitLow = 0.0f;
+            hitHigh = 0.0f;
+            return true;
+        }
+        return false;
+    }
+    
+    // Given a line with point 'p' and normalized direction 'u' and 
+    // a cylinder with axial point 's', radius 'r', and normalized direction 'v'
+    // the intersection of the two is on the line at distance 't' from 'p'.
+    //
+    // Determining the values of t reduces to solving a quadratic equation: At^2 + Bt + C = 0 
+    //
+    // where:
+    //
+    // P = p-s
+    // w = u-(u.v)v
+    // Q = P-(P.v)v
+    //       
+    // A = w^2
+    // B = 2(w.Q)
+    // C = Q^2 - r^2
+
+    glm::vec3 P = lineP - cylinderP;
+    glm::vec3 w = lineDir - uDotV * cylinderDir;
+    glm::vec3 Q = P - glm::dot(P, cylinderDir) * cylinderDir;
+
+    // we save a few multiplies by storing 2*A rather than just A
+    float A2 = 2.0f * glm::dot(w, w);
+    float B = 2.0f * glm::dot(w, Q);
+
+    // since C is only ever used once (in the determinant) we compute it inline
+    float determinant = B * B - 2.0f * A2 * (glm::dot(Q, Q) - cylinderRadius * cylinderRadius);
+    if (determinant < 0.0f) {
+        return false;
+    }
+    hitLow  = (-B - sqrtf(determinant)) / A2;
+    hitHigh = -(hitLow + 2.0f * B / A2);
+
+    if (hitLow > hitHigh) {
+        // re-arrange so hitLow is always the smaller value
+        float temp = hitHigh;
+        hitHigh = hitLow;
+        hitLow = temp;
+    }
+    return true;
+}
+
 bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, CollisionList& collisions) {
     glm::vec3 axisA;
     capsuleA->computeNormalizedAxis(axisA);
@@ -358,23 +424,43 @@ bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB,
     float denominator = 1.0f - aDotB * aDotB;
     float totalRadius = capsuleA->getRadius() + capsuleB->getRadius();
     if (denominator > EPSILON) {
-        // distances to points of closest approach
-        float distanceA = glm::dot((centerB - centerA), (axisA - (aDotB) * axisB)) / denominator;
-        float distanceB = glm::dot((centerA - centerB), (axisB - (aDotB) * axisA)) / denominator;
-        
-        // clamp the distances to the ends of the capsule line segments
-        float absDistanceA = fabs(distanceA);
-        if (absDistanceA > capsuleA->getHalfHeight()) {
-            float signA = distanceA < 0.0f ? -1.0f : 1.0f;
-            distanceA = signA * capsuleA->getHalfHeight();
-        }
-        float absDistanceB = fabs(distanceB);
-        if (absDistanceB > capsuleB->getHalfHeight()) {
-            float signB = distanceB < 0.0f ? -1.0f : 1.0f;
-            distanceB = signB * capsuleB->getHalfHeight();
+        // perform line-cylinder intesection test between axis of cylinderA and cylinderB with exanded radius
+        float hitLow = 0.0f;
+        float hitHigh = 0.0f;
+        if (!lineCylinder(centerA, axisA, centerB, axisB, totalRadius, hitLow, hitHigh)) {
+            return false;
         }
 
-        // collide like spheres at closest approaches (do most of the math relative to B)
+        float halfHeightA = capsuleA->getHalfHeight();
+        if (hitLow > halfHeightA || hitHigh < -halfHeightA) {
+            // the intersections are off the ends of capsuleA
+            return false;
+        }
+
+        // compute nearest approach on axisA of axisB
+        float distanceA = glm::dot((centerB - centerA), (axisA - (aDotB) * axisB)) / denominator;
+        // clamp to intersection zone
+        if (distanceA > hitLow) {
+            if (distanceA > hitHigh) {
+                distanceA = hitHigh;
+            }
+        } else {
+            distanceA = hitLow;
+        }
+        // clamp to capsule segment
+        distanceA = glm::clamp(distanceA, -halfHeightA, halfHeightA);
+
+        // find the closest point on capsuleB to sphere on capsuleA
+        float distanceB = glm::dot(centerA + distanceA * axisA - centerB, axisB);
+        float halfHeightB = capsuleB->getHalfHeight();
+        if (fabsf(distanceB) > halfHeightB) {
+            // we must clamp distanceB...
+            distanceB = glm::clamp(distanceB, -halfHeightB, halfHeightB);
+            // ...and therefore must recompute distanceA
+            distanceA = glm::clamp(glm::dot(centerB + distanceB * axisB - centerA, axisA), -halfHeightA, halfHeightA);
+        }
+
+        // collide like two spheres (do most of the math relative to B)
         glm::vec3 BA = (centerB + distanceB * axisB) - (centerA + distanceA * axisA);
         float distanceSquared = glm::dot(BA, BA);
         if (distanceSquared < totalRadius * totalRadius) {