fixes for capsuleVsAACube() with unit tests

libraries/shared/src/ShapeCollider.cpp

@@ -624,8 +624,6 @@ glm::vec3 cubeNormals[3] = { glm::vec3(1.0f, 0.0f, 0.0f), glm::vec3(0.0f, 1.0f,
 // hence the first pair is (1, 2) because the OTHER faces for xFace are (yFace, zFace) = (1, 2)
 int wallIndices[] = { 1, 2, 0, 2, 0, 1 };
 
-const float INV_SQRT_TWO = 1.0f / SQUARE_ROOT_OF_2;
-
 bool capsuleVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) {
     const CapsuleShape* capsuleA = static_cast<const CapsuleShape*>(shapeA);
     const AACubeShape* cubeB = static_cast<const AACubeShape*>(shapeB);
@@ -635,12 +633,13 @@ bool capsuleVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& co
     capsuleA->computeNormalizedAxis(capsuleAxis);
     float halfHeight = capsuleA->getHalfHeight();
     glm::vec3 cubeCenter = cubeB->getTranslation();
-    glm::vec3 BA = cubeCenter - capsuleA->getTranslation();
+    glm::vec3 capsuleCenter = capsuleA->getTranslation();
+    glm::vec3 BA = cubeCenter - capsuleCenter;
     float axialOffset = glm::dot(capsuleAxis, BA);
     if (fabsf(axialOffset) > halfHeight) {
         axialOffset = (axialOffset < 0.0f) ? -halfHeight : halfHeight;
     }
-    glm::vec3 nearestApproach = axialOffset * capsuleAxis;
+    glm::vec3 nearestApproach = capsuleCenter + axialOffset * capsuleAxis;
 
     // transform nearestApproach into cube-relative frame
     nearestApproach -= cubeCenter;
@@ -661,6 +660,13 @@ bool capsuleVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& co
         faceNormal = glm::vec3(0.0f, 0.0f, signs.z);
     }
 
+    if (fabs(glm::dot(faceNormal, capsuleAxis)) < EPSILON) {
+        // TODO: Andrew to implement this special case 
+        // where capsule is perpendicular to the face that it hits
+        // (Make this its own function?  or implement it here?)
+        return false;
+    }
+
     // Revisualize the capsule as a startPoint and an axis that points toward the cube face.
     // We raytrace forward into the four planes that neigbhor the face to find the furthest 
     // point along the capsule's axis that might hit face.
@@ -680,10 +686,10 @@ bool capsuleVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& co
     float capsuleRadius = capsuleA->getRadius();
 
     // Loop over the directions that are NOT parallel to face (there are two of them).
-    // For each direction we'll raytrace along capsuleAxis to find point impact 
-    // on the furthest face and clamp it to remain on the capsule's line segment
-    float distances[2] = { 0, capsuleLength};
-    int numCompleteMisses = 0;
+    // For each direction we'll raytrace along capsuleAxis to find where the axis passes
+    // through the furthest face and then we'll clamp to remain on the capsule's line segment
+    float shortestDistance = capsuleLength;
+
     for (int i = 0; i < 2; ++i) {
         int wallIndex = wallIndices[2 * faceIndex + i];
         // each direction has two walls: positive and negative
@@ -691,44 +697,48 @@ bool capsuleVsAACube(const Shape* shapeA, const Shape* shapeB, CollisionList& co
             glm::vec3 wallNormal = wallSign * cubeNormals[wallIndex];
             float axisDotWall = glm::dot(capsuleAxis, wallNormal);
             if (axisDotWall > EPSILON) {
-                float distance = (halfCubeSide + glm::dot(capsuleStart, wallNormal)) / axisDotWall;
-                distances[i] = glm::clamp(distance, 0.0f, capsuleLength);
-                if (distance == distances[i]) {
-                    // the wall truncated the capsule which means there is a possibility that the capusule 
-                    // actually collides against the edge of the face, so we check for that case now
-                    glm::vec3 impact = capsuleStart + distance * capsuleAxis;
-                    float depth = glm::dot(impact, faceNormal) - halfCubeSide;
-                    if (depth > 0.0f) {
-                        if (depth < capsuleRadius) {
-                            // need to recast against the diagonal plane: wall rotated away from the face
-                            glm::vec3 diagonalNormal = INV_SQRT_TWO * (wallNormal - faceNormal);
-                            distances[i] = glm::min(glm::dot(capsuleStart, diagonalNormal), 0.0f);
-                        } else {
-                            // capsule misses this wall by more than capsuleRadius
-                            ++numCompleteMisses;
-                        }
+                // formula for distance to intersection between line (P,p) and plane (V,n) is: (V-P)*n / p*n
+                float newDistance = (halfCubeSide - glm::dot(capsuleStart, wallNormal)) / axisDotWall;
+                if (newDistance < 0.0f) {
+                    // The wall is behind the capsule, but there is still a possibility that it collides 
+                    // against the edge so we recast against the diagonal plane beteween the two faces.
+                    // NOTE: it is impossible for the startPoint to be in front of the diagonal plane,
+                    // therefore we know we'll get a valid distance.
+                    glm::vec3 thirdNormal = glm::cross(faceNormal, wallNormal);
+                    glm::vec3 diagonalNormal = glm::normalize(glm::cross(glm::cross(capsuleAxis, thirdNormal), thirdNormal));
+                    newDistance = glm::dot(halfCubeSide * (faceNormal + wallNormal) - capsuleStart, diagonalNormal) / 
+                        glm::dot(capsuleAxis, diagonalNormal);
+                } else if (newDistance < capsuleLength) {
+                    // The wall truncates the capsule axis, but we must check the case where the capsule
+                    // collides with an edge/corner rather than the face.  The good news is that this gives us
+                    // an opportunity to check for an early exit case.
+                    float heightOfImpact = glm::dot(capsuleStart + newDistance * capsuleAxis, faceNormal);
+                    if (heightOfImpact > halfCubeSide + SQUARE_ROOT_OF_2 * capsuleRadius) {
+                        // it is impossible for the capsule to hit the face
+                        return false;
+                    } else {
+                        // recast against the diagonal plane between the two faces
+                        glm::vec3 thirdNormal = glm::cross(faceNormal, wallNormal);
+                        glm::vec3 diagonalNormal = glm::normalize(glm::cross(glm::cross(capsuleAxis, thirdNormal), thirdNormal));
+                        newDistance = glm::dot(halfCubeSide * (faceNormal + wallNormal) - capsuleStart, diagonalNormal) / 
+                            glm::dot(capsuleAxis, diagonalNormal);
                     }
                 }
-                // there can't be more than one hit for any direction so we break
+                if (newDistance < shortestDistance) {
+                    shortestDistance = newDistance;
+                }
+                // there can only be one hit per direction
                 break;
             }
         }
     }
-    if (numCompleteMisses == 2) {
-        return false;
-    }
-
+    
     // chose the point that produces the deepest penetration against face
-    glm::vec3 point0 = capsuleStart + distances[0] * capsuleAxis;
-    glm::vec3 point1 = capsuleStart + distances[1] * capsuleAxis;
-    if (glm::dot(point0, faceNormal) > glm::dot(point1, faceNormal)) {
-        point0 = point1;
-    }
-    // move back into real frame
-    point0 += cubeCenter;
+    // and translate back into real frame
+    glm::vec3 sphereCenter = cubeCenter + capsuleStart + shortestDistance * capsuleAxis;
 
     // collide like a sphere at point0 with capsuleRadius
-    CollisionInfo* collision = sphereVsAACubeHelper(point0, capsuleRadius,
+    CollisionInfo* collision = sphereVsAACubeHelper(sphereCenter, capsuleRadius,
             cubeCenter, 2.0f * halfCubeSide, collisions);
     if (collision) {
         // we hit! so store back pointers to the shapes

tests/physics/src/ShapeColliderTests.cpp

@@ -16,6 +16,7 @@
 #include <glm/glm.hpp>
 #include <glm/gtx/quaternion.hpp>
 
+#include <AACubeShape.h>
 #include <CapsuleShape.h>
 #include <CollisionInfo.h>
 #include <PlaneShape.h>
@@ -1022,6 +1023,557 @@ void ShapeColliderTests::sphereTouchesAACubeCorners() {
     }
 }
 
+void ShapeColliderTests::capsuleMissesAACube() {
+    CollisionList collisions(16);
+    
+    float capsuleRadius = 1.0f;
+
+    glm::vec3 cubeCenter(1.23f, 4.56f, 7.89f);
+    float cubeSide = 2.0f;
+    AACubeShape cube(cubeSide, cubeCenter);
+
+    glm::vec3 faceNormals[] = {xAxis, yAxis, zAxis};
+    int numDirections = 3;
+
+    float offset = 2.0f * EPSILON;
+
+    // faces
+    for (int i = 0; i < numDirections; ++i) {
+        for (float sign = -1.0f; sign < 2.0f; sign += 2.0f) {
+            glm::vec3 faceNormal = sign * faceNormals[i];
+
+            glm::vec3 secondNormal = faceNormals[(i + 1) % numDirections];
+            glm::vec3 thirdNormal = faceNormals[(i + 2) % numDirections];
+
+            // pick a random point somewhere above the face
+            glm::vec3 startPoint = cubeCenter + (cubeSide + capsuleRadius) * faceNormal + 
+                (cubeSide * (randFloat() - 0.5f)) * secondNormal +
+                (cubeSide * (randFloat() - 0.5f)) * thirdNormal;
+
+            // pick a second random point slightly more than one radius above the face
+            glm::vec3 endPoint = cubeCenter + (0.5f * cubeSide + capsuleRadius + offset) * faceNormal +
+                (cubeSide * (randFloat() - 0.5f)) * secondNormal +
+                (cubeSide * (randFloat() - 0.5f)) * thirdNormal;
+
+            // randomly swap the points so capsule axis may point toward or away from face
+            if (randFloat() > 0.5f) {
+                glm::vec3 temp = startPoint;
+                startPoint = endPoint;
+                endPoint = temp;
+            }
+
+            // create a capsule between the points
+            CapsuleShape capsule(capsuleRadius, startPoint, endPoint);
+
+            // collide capsule with cube
+            bool hit = ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions);
+            if (hit) {
+                std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule should NOT collide with cube face."
+                    << "  faceNormal = " << faceNormal << std::endl;
+            }
+        }
+    }
+
+    // edges
+    // loop over each face...
+    for (int i = 0; i < numDirections; ++i) {
+        for (float faceSign = -1.0f; faceSign < 2.0f; faceSign += 2.0f) {
+            glm::vec3 faceNormal = faceSign * faceNormals[i];
+
+            // loop over each neighboring face...
+            for (int j = (i + 1) % numDirections; j != i; j = (j + 1) % numDirections) {
+                // Compute the index to the third direction, which points perpendicular to both the face
+                // and the neighbor face.
+                int k = (j + 1) % numDirections;
+                if (k == i) {
+                    k = (i + 1) % numDirections;
+                }
+                glm::vec3 thirdNormal = faceNormals[k];
+
+                collisions.clear();
+                for (float neighborSign = -1.0f; neighborSign < 2.0f; neighborSign += 2.0f) {
+                    glm::vec3 neighborNormal = neighborSign * faceNormals[j];
+
+                    // combine the face and neighbor normals to get the edge normal
+                    glm::vec3 edgeNormal = glm::normalize(faceNormal + neighborNormal);
+
+                    // pick a random point somewhere above the edge
+                    glm::vec3 startPoint = cubeCenter + (SQUARE_ROOT_OF_2 * cubeSide + capsuleRadius) * edgeNormal + 
+                        (cubeSide * (randFloat() - 0.5f)) * thirdNormal;
+        
+                    // pick a second random point slightly more than one radius above the edge
+                    glm::vec3 endPoint = cubeCenter + (SQUARE_ROOT_OF_2 * 0.5f * cubeSide + capsuleRadius + offset) * edgeNormal +
+                        (cubeSide * (randFloat() - 0.5f)) * thirdNormal;
+        
+                    // randomly swap the points so capsule axis may point toward or away from edge
+                    if (randFloat() > 0.5f) {
+                        glm::vec3 temp = startPoint;
+                        startPoint = endPoint;
+                        endPoint = temp;
+                    }
+        
+                    // create a capsule between the points
+                    CapsuleShape capsule(capsuleRadius, startPoint, endPoint);
+        
+                    // collide capsule with cube
+                    bool hit = ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions);
+                    if (hit) {
+                        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule should NOT collide with cube face."
+                            << "  edgeNormal = " << edgeNormal << std::endl;
+                    }
+                }
+            }
+        }
+    }
+
+    // corners
+    for (float firstSign = -1.0f; firstSign < 2.0f; firstSign += 2.0f) {
+        glm::vec3 firstNormal = firstSign * faceNormals[0];
+        for (float secondSign = -1.0f; secondSign < 2.0f; secondSign += 2.0f) {
+            glm::vec3 secondNormal = secondSign * faceNormals[1];
+            for (float thirdSign = -1.0f; thirdSign < 2.0f; thirdSign += 2.0f) {
+                collisions.clear();
+                glm::vec3 thirdNormal = thirdSign * faceNormals[2];
+
+                // the cornerNormal is the normalized sum of the three faces
+                glm::vec3 cornerNormal = glm::normalize(firstNormal + secondNormal + thirdNormal);
+
+                // pick a random point somewhere above the corner
+                glm::vec3 startPoint = cubeCenter + (SQUARE_ROOT_OF_3 * cubeSide + capsuleRadius) * cornerNormal + 
+                    (0.25f * cubeSide * (randFloat() - 0.5f)) * firstNormal +
+                    (0.25f * cubeSide * (randFloat() - 0.5f)) * secondNormal +
+                    (0.25f * cubeSide * (randFloat() - 0.5f)) * thirdNormal;
+    
+                // pick a second random point slightly more than one radius above the corner
+                glm::vec3 endPoint = cubeCenter + (SQUARE_ROOT_OF_3 * 0.5f * cubeSide + capsuleRadius + offset) * cornerNormal;
+    
+                // randomly swap the points so capsule axis may point toward or away from corner
+                if (randFloat() > 0.5f) {
+                    glm::vec3 temp = startPoint;
+                    startPoint = endPoint;
+                    endPoint = temp;
+                }
+    
+                // create a capsule between the points
+                CapsuleShape capsule(capsuleRadius, startPoint, endPoint);
+    
+                // collide capsule with cube
+                bool hit = ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions);
+                if (hit) {
+                    std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule should NOT collide with cube face."
+                        << "  cornerNormal = " << cornerNormal << std::endl;
+                }
+            }
+        }
+    }
+}
+
+void ShapeColliderTests::capsuleTouchesAACube() {
+    CollisionList collisions(16);
+    
+    float capsuleRadius = 1.0f;
+
+    //glm::vec3 cubeCenter(0.0f);
+    glm::vec3 cubeCenter(1.23f, 4.56f, 7.89f);
+    float cubeSide = 2.0f;
+    AACubeShape cube(cubeSide, cubeCenter);
+
+    glm::vec3 faceNormals[] = {xAxis, yAxis, zAxis};
+    int numDirections = 3;
+
+    float overlap = 0.25f * capsuleRadius;
+    float allowableError = 10.0f * EPSILON;
+
+    // capsule caps against cube faces
+    for (int i = 0; i < numDirections; ++i) {
+        for (float sign = -1.0f; sign < 2.0f; sign += 2.0f) {
+            glm::vec3 faceNormal = sign * faceNormals[i];
+
+            glm::vec3 secondNormal = faceNormals[(i + 1) % numDirections];
+            glm::vec3 thirdNormal = faceNormals[(i + 2) % numDirections];
+
+            // pick a random point somewhere above the face
+            glm::vec3 startPoint = cubeCenter + (cubeSide + capsuleRadius) * faceNormal + 
+                (cubeSide * (randFloat() - 0.5f)) * secondNormal +
+                (cubeSide * (randFloat() - 0.5f)) * thirdNormal;
+
+            // pick a second random point slightly less than one radius above the face
+            // (but reduce width of range by 2*overlap to prevent the penetration from 
+            // registering against other faces)
+            glm::vec3 endPoint = cubeCenter + (0.5f * cubeSide + capsuleRadius - overlap) * faceNormal +
+                ((cubeSide - 2.0f * overlap) * (randFloat() - 0.5f)) * secondNormal +
+                ((cubeSide - 2.0f * overlap) * (randFloat() - 0.5f)) * thirdNormal;
+            glm::vec3 collidingPoint = endPoint;
+
+            // randomly swap the points so capsule axis may point toward or away from face
+            if (randFloat() > 0.5f) {
+                glm::vec3 temp = startPoint;
+                startPoint = endPoint;
+                endPoint = temp;
+            }
+
+            // create a capsule between the points
+            CapsuleShape capsule(capsuleRadius, startPoint, endPoint);
+
+            // collide capsule with cube
+            bool hit = ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions);
+            if (!hit) {
+                std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule should collide with cube"
+                    << "  faceNormal = " << faceNormal << std::endl;
+                break;
+            }
+
+            CollisionInfo* collision = collisions.getLastCollision();
+            if (!collision) {
+                std::cout << __FILE__ << ":" << __LINE__
+                    << " ERROR: null collision with faceNormal = " << faceNormal << std::endl;
+                return;
+            }
+        
+            // penetration points from capsule into cube 
+            glm::vec3 expectedPenetration = - overlap * faceNormal;
+            float inaccuracy = glm::length(collision->_penetration - expectedPenetration);
+            if (fabs(inaccuracy) > allowableError) {
+                std::cout << __FILE__ << ":" << __LINE__
+                    << " ERROR: bad penetration: expected = " << expectedPenetration
+                    << " actual = " << collision->_penetration 
+                    << " faceNormal = " << faceNormal
+                    << std::endl;
+            }
+        
+            // contactPoint is on surface of capsule
+            glm::vec3 expectedContactPoint = collidingPoint - capsuleRadius * faceNormal;
+            inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
+            if (fabs(inaccuracy) > allowableError) {
+                std::cout << __FILE__ << ":" << __LINE__
+                    << " ERROR: bad contactPoint: expected = " << expectedContactPoint
+                    << " actual = " << collision->_contactPoint 
+                    << " faceNormal = " << faceNormal
+                    << std::endl;
+            }
+        }
+    }
+
+    // capsule caps against cube edges
+    // loop over each face...
+    for (int i = 0; i < numDirections; ++i) {
+        for (float faceSign = -1.0f; faceSign < 2.0f; faceSign += 2.0f) {
+            glm::vec3 faceNormal = faceSign * faceNormals[i];
+
+            // loop over each neighboring face...
+            for (int j = (i + 1) % numDirections; j != i; j = (j + 1) % numDirections) {
+                // Compute the index to the third direction, which points perpendicular to both the face
+                // and the neighbor face.
+                int k = (j + 1) % numDirections;
+                if (k == i) {
+                    k = (i + 1) % numDirections;
+                }
+                glm::vec3 thirdNormal = faceNormals[k];
+
+                collisions.clear();
+                for (float neighborSign = -1.0f; neighborSign < 2.0f; neighborSign += 2.0f) {
+                    glm::vec3 neighborNormal = neighborSign * faceNormals[j];
+
+                    // combine the face and neighbor normals to get the edge normal
+                    glm::vec3 edgeNormal = glm::normalize(faceNormal + neighborNormal);
+
+                    // pick a random point somewhere above the edge
+                    glm::vec3 startPoint = cubeCenter + (SQUARE_ROOT_OF_2 * cubeSide + capsuleRadius) * edgeNormal + 
+                        (cubeSide * (randFloat() - 0.5f)) * thirdNormal;
+        
+                    // pick a second random point slightly less than one radius above the edge
+                    glm::vec3 endPoint = cubeCenter + (SQUARE_ROOT_OF_2 * 0.5f * cubeSide + capsuleRadius - overlap) * edgeNormal +
+                        (cubeSide * (randFloat() - 0.5f)) * thirdNormal;
+                    glm::vec3 collidingPoint = endPoint;
+        
+                    // randomly swap the points so capsule axis may point toward or away from edge
+                    if (randFloat() > 0.5f) {
+                        glm::vec3 temp = startPoint;
+                        startPoint = endPoint;
+                        endPoint = temp;
+                    }
+        
+                    // create a capsule between the points
+                    CapsuleShape capsule(capsuleRadius, startPoint, endPoint);
+        
+                    // collide capsule with cube
+                    bool hit = ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions);
+                    if (!hit) {
+                        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule should collide with cube"
+                            << "  edgeNormal = " << edgeNormal << std::endl;
+                    }
+
+                    CollisionInfo* collision = collisions.getLastCollision();
+                    if (!collision) {
+                        std::cout << __FILE__ << ":" << __LINE__
+                            << " ERROR: null collision with edgeNormal = " << edgeNormal << std::endl;
+                        return;
+                    }
+                
+                    // penetration points from capsule into cube 
+                    glm::vec3 expectedPenetration = - overlap * edgeNormal;
+                    float inaccuracy = glm::length(collision->_penetration - expectedPenetration);
+                    if (fabs(inaccuracy) > allowableError) {
+                        std::cout << __FILE__ << ":" << __LINE__
+                            << " ERROR: bad penetration: expected = " << expectedPenetration
+                            << " actual = " << collision->_penetration 
+                            << " edgeNormal = " << edgeNormal
+                            << std::endl;
+                    }
+                
+                    // contactPoint is on surface of capsule
+                    glm::vec3 expectedContactPoint = collidingPoint - capsuleRadius * edgeNormal;
+                    inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
+                    if (fabs(inaccuracy) > allowableError) {
+                        std::cout << __FILE__ << ":" << __LINE__
+                            << " ERROR: bad contactPoint: expected = " << expectedContactPoint
+                            << " actual = " << collision->_contactPoint 
+                            << " edgeNormal = " << edgeNormal
+                            << std::endl;
+                    }
+                }
+            }
+        }
+    }
+
+    // capsule caps against cube corners
+    for (float firstSign = -1.0f; firstSign < 2.0f; firstSign += 2.0f) {
+        glm::vec3 firstNormal = firstSign * faceNormals[0];
+        for (float secondSign = -1.0f; secondSign < 2.0f; secondSign += 2.0f) {
+            glm::vec3 secondNormal = secondSign * faceNormals[1];
+            for (float thirdSign = -1.0f; thirdSign < 2.0f; thirdSign += 2.0f) {
+                collisions.clear();
+                glm::vec3 thirdNormal = thirdSign * faceNormals[2];
+
+                // the cornerNormal is the normalized sum of the three faces
+                glm::vec3 cornerNormal = glm::normalize(firstNormal + secondNormal + thirdNormal);
+
+                // pick a random point somewhere above the corner
+                glm::vec3 startPoint = cubeCenter + (SQUARE_ROOT_OF_3 * cubeSide + capsuleRadius) * cornerNormal + 
+                    (0.25f * cubeSide * (randFloat() - 0.5f)) * firstNormal +
+                    (0.25f * cubeSide * (randFloat() - 0.5f)) * secondNormal +
+                    (0.25f * cubeSide * (randFloat() - 0.5f)) * thirdNormal;
+    
+                // pick a second random point slightly less than one radius above the corner
+                glm::vec3 endPoint = cubeCenter + (SQUARE_ROOT_OF_3 * 0.5f * cubeSide + capsuleRadius - overlap) * cornerNormal;
+                glm::vec3 collidingPoint = endPoint;
+    
+                // randomly swap the points so capsule axis may point toward or away from corner
+                if (randFloat() > 0.5f) {
+                    glm::vec3 temp = startPoint;
+                    startPoint = endPoint;
+                    endPoint = temp;
+                }
+    
+                // create a capsule between the points
+                CapsuleShape capsule(capsuleRadius, startPoint, endPoint);
+    
+                // collide capsule with cube
+                bool hit = ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions);
+                if (!hit) {
+                    std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule should collide with cube"
+                        << "  cornerNormal = " << cornerNormal << std::endl;
+                }
+
+                CollisionInfo* collision = collisions.getLastCollision();
+                if (!collision) {
+                    std::cout << __FILE__ << ":" << __LINE__
+                        << " ERROR: null collision with cornerNormal = " << cornerNormal << std::endl;
+                    return;
+                }
+            
+                // penetration points from capsule into cube 
+                glm::vec3 expectedPenetration = - overlap * cornerNormal;
+                float inaccuracy = glm::length(collision->_penetration - expectedPenetration);
+                if (fabs(inaccuracy) > allowableError) {
+                    std::cout << __FILE__ << ":" << __LINE__
+                        << " ERROR: bad penetration: expected = " << expectedPenetration
+                        << " actual = " << collision->_penetration 
+                        << " cornerNormal = " << cornerNormal
+                        << std::endl;
+                }
+            
+                // contactPoint is on surface of capsule
+                glm::vec3 expectedContactPoint = collidingPoint - capsuleRadius * cornerNormal;
+                inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
+                if (fabs(inaccuracy) > allowableError) {
+                    std::cout << __FILE__ << ":" << __LINE__
+                        << " ERROR: bad contactPoint: expected = " << expectedContactPoint
+                        << " actual = " << collision->_contactPoint 
+                        << " cornerNormal = " << cornerNormal
+                        << std::endl;
+                }
+            }
+        }
+    }
+
+    // capsule sides against cube edges
+    // loop over each face...
+    float capsuleLength = 2.0f;
+    for (int i = 0; i < numDirections; ++i) {
+        for (float faceSign = -1.0f; faceSign < 2.0f; faceSign += 2.0f) {
+            glm::vec3 faceNormal = faceSign * faceNormals[i];
+
+            // loop over each neighboring face...
+            for (int j = (i + 1) % numDirections; j != i; j = (j + 1) % numDirections) {
+                // Compute the index to the third direction, which points perpendicular to both the face
+                // and the neighbor face.
+                int k = (j + 1) % numDirections;
+                if (k == i) {
+                    k = (i + 1) % numDirections;
+                }
+                glm::vec3 thirdNormal = faceNormals[k];
+
+                collisions.clear();
+                for (float neighborSign = -1.0f; neighborSign < 2.0f; neighborSign += 2.0f) {
+                    glm::vec3 neighborNormal = neighborSign * faceNormals[j];
+
+                    // combine the face and neighbor normals to get the edge normal
+                    glm::vec3 edgeNormal = glm::normalize(faceNormal + neighborNormal);
+
+                    // pick a random point somewhere along the edge
+                    glm::vec3 edgePoint = cubeCenter + (SQUARE_ROOT_OF_2 * 0.5f * cubeSide) * edgeNormal + 
+                        ((cubeSide - 2.0f * overlap) * (randFloat() - 0.5f)) * thirdNormal;
+
+                    // pick a random normal that is deflected slightly from edgeNormal
+                    glm::vec3 deflectedNormal = glm::normalize(edgeNormal +
+                            (0.1f * (randFloat() - 0.5f)) * faceNormal +
+                            (0.1f * (randFloat() - 0.5f)) * neighborNormal);
+
+                    // compute the axis direction, which will be perp to deflectedNormal and thirdNormal
+                    glm::vec3 axisDirection = glm::normalize(glm::cross(deflectedNormal, thirdNormal));
+
+                    // compute a point for the capsule's axis along deflection normal away from edgePoint
+                    glm::vec3 axisPoint = edgePoint + (capsuleRadius - overlap) * deflectedNormal;
+
+                    // now we can compute the capsule endpoints
+                    glm::vec3 endPoint = axisPoint + (0.5f * capsuleLength * randFloat()) * axisDirection;
+                    glm::vec3 startPoint = axisPoint - (0.5f * capsuleLength * randFloat()) * axisDirection;
+        
+                    // create a capsule between the points
+                    CapsuleShape capsule(capsuleRadius, startPoint, endPoint);
+        
+                    // collide capsule with cube
+                    bool hit = ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions);
+                    if (!hit) {
+                        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule should collide with cube"
+                            << "  edgeNormal = " << edgeNormal << std::endl;
+                    }
+
+                    CollisionInfo* collision = collisions.getLastCollision();
+                    if (!collision) {
+                        std::cout << __FILE__ << ":" << __LINE__
+                            << " ERROR: null collision with edgeNormal = " << edgeNormal << std::endl;
+                        return;
+                    }
+                
+                    // penetration points from capsule into cube 
+                    glm::vec3 expectedPenetration = - overlap * deflectedNormal;
+                    float inaccuracy = glm::length(collision->_penetration - expectedPenetration);
+                    if (fabs(inaccuracy) > allowableError / capsuleLength) {
+                        std::cout << __FILE__ << ":" << __LINE__
+                            << " ERROR: bad penetration: expected = " << expectedPenetration
+                            << " actual = " << collision->_penetration 
+                            << " edgeNormal = " << edgeNormal
+                            << std::endl;
+                    }
+                
+                    // contactPoint is on surface of capsule
+                    glm::vec3 expectedContactPoint = axisPoint - capsuleRadius * deflectedNormal;
+                    inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
+                    if (fabs(inaccuracy) > allowableError / capsuleLength) {
+                        std::cout << __FILE__ << ":" << __LINE__
+                            << " ERROR: bad contactPoint: expected = " << expectedContactPoint
+                            << " actual = " << collision->_contactPoint 
+                            << " edgeNormal = " << edgeNormal
+                            << std::endl;
+                    }
+                }
+            }
+        }
+    }
+
+    // capsule sides against cube corners
+    for (float firstSign = -1.0f; firstSign < 2.0f; firstSign += 2.0f) {
+        glm::vec3 firstNormal = firstSign * faceNormals[0];
+        for (float secondSign = -1.0f; secondSign < 2.0f; secondSign += 2.0f) {
+            glm::vec3 secondNormal = secondSign * faceNormals[1];
+            for (float thirdSign = -1.0f; thirdSign < 2.0f; thirdSign += 2.0f) {
+                collisions.clear();
+                glm::vec3 thirdNormal = thirdSign * faceNormals[2];
+
+                // the cornerNormal is the normalized sum of the three faces
+                glm::vec3 cornerNormal = glm::normalize(firstNormal + secondNormal + thirdNormal);
+
+                // compute a penetration normal that is somewhat randomized about cornerNormal
+                glm::vec3 penetrationNormal = - glm::normalize(cornerNormal +
+                    (0.05f * cubeSide * (randFloat() - 0.5f)) * firstNormal +
+                    (0.05f * cubeSide * (randFloat() - 0.5f)) * secondNormal +
+                    (0.05f * cubeSide * (randFloat() - 0.5f)) * thirdNormal);
+
+                // pick a random point somewhere above the corner
+                glm::vec3 corner = cubeCenter + (0.5f * cubeSide) * (firstNormal + secondNormal + thirdNormal);
+                glm::vec3 startPoint = corner + (3.0f * cubeSide) * cornerNormal + 
+                    (0.25f * cubeSide * (randFloat() - 0.5f)) * firstNormal +
+                    (0.25f * cubeSide * (randFloat() - 0.5f)) * secondNormal +
+                    (0.25f * cubeSide * (randFloat() - 0.5f)) * thirdNormal;
+
+                // pick a second random point slightly less than one radius above the corner
+                // with some sight perp motion
+                glm::vec3 endPoint = corner - (capsuleRadius - overlap) * penetrationNormal;
+                glm::vec3 collidingPoint = endPoint;
+
+                // randomly swap the points so capsule axis may point toward or away from corner
+                if (randFloat() > 0.5f) {
+                    glm::vec3 temp = startPoint;
+                    startPoint = endPoint;
+                    endPoint = temp;
+                }
+    
+                // create a capsule between the points
+                CapsuleShape capsule(capsuleRadius, startPoint, endPoint);
+    
+                // collide capsule with cube
+                bool hit = ShapeCollider::capsuleVsAACube(&capsule, &cube, collisions);
+                if (!hit) {
+                    std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule should collide with cube"
+                        << "  cornerNormal = " << cornerNormal << std::endl;
+                }
+
+                CollisionInfo* collision = collisions.getLastCollision();
+                if (!collision) {
+                    std::cout << __FILE__ << ":" << __LINE__
+                        << " ERROR: null collision with cornerNormal = " << cornerNormal << std::endl;
+                    return;
+                }
+            
+                // penetration points from capsule into cube 
+                glm::vec3 expectedPenetration = overlap * penetrationNormal;
+                float inaccuracy = glm::length(collision->_penetration - expectedPenetration);
+                if (fabs(inaccuracy) > allowableError) {
+                    std::cout << __FILE__ << ":" << __LINE__
+                        << " ERROR: bad penetration: expected = " << expectedPenetration
+                        << " actual = " << collision->_penetration 
+                        << " cornerNormal = " << cornerNormal
+                        << std::endl;
+                }
+            
+                // contactPoint is on surface of capsule
+                glm::vec3 expectedContactPoint = collidingPoint + capsuleRadius * penetrationNormal;
+                inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
+                if (fabs(inaccuracy) > allowableError) {
+                    std::cout << __FILE__ << ":" << __LINE__
+                        << " ERROR: bad contactPoint: expected = " << expectedContactPoint
+                        << " actual = " << collision->_contactPoint 
+                        << " cornerNormal = " << cornerNormal
+                        << std::endl;
+                }
+            }
+        }
+    }
+}
+
+
 void ShapeColliderTests::rayHitsSphere() {
     float startDistance = 3.0f;
     glm::vec3 rayStart(-startDistance, 0.0f, 0.0f);
@@ -1489,6 +2041,9 @@ void ShapeColliderTests::runAllTests() {
     sphereTouchesAACubeEdges();
     sphereTouchesAACubeCorners();
 
+    capsuleMissesAACube();
+    capsuleTouchesAACube();
+
     rayHitsSphere();
     rayBarelyHitsSphere();
     rayBarelyMissesSphere();

tests/physics/src/ShapeColliderTests.h

@@ -28,6 +28,9 @@ namespace ShapeColliderTests {
     void sphereTouchesAACubeCorners();
     void sphereMissesAACube();
 
+    void capsuleMissesAACube();
+    void capsuleTouchesAACube();
+
     void rayHitsSphere();
     void rayBarelyHitsSphere();
     void rayBarelyMissesSphere();