fix sphere-cube collisions, with tests

libraries/shared/src/ShapeCollider.cpp

@@ -15,7 +15,6 @@
 
 #include "GeometryUtil.h"
 #include "ShapeCollider.h"
-#include "StreamUtils.h"    // adebug
 
 // NOTE:
 //
@@ -594,6 +593,95 @@ bool listList(const ListShape* listA, const ListShape* listB, CollisionList& col
 // helper function
 bool sphereAACube(const glm::vec3& sphereCenter, float sphereRadius, const glm::vec3& cubeCenter, 
         float cubeSide, CollisionList& collisions) {
+    // sphere is A
+    // cube is B
+    // BA = B - A = from center of A to center of B 
+    float halfCubeSide = 0.5f * cubeSide;
+    glm::vec3 BA = cubeCenter - sphereCenter;
+    float distance = glm::length(BA);
+    if (distance > EPSILON) {
+        float maxBA = glm::max(glm::max(glm::abs(BA.x), glm::abs(BA.y)), glm::abs(BA.z));
+        if (maxBA > halfCubeSide + sphereRadius) {
+            // sphere misses cube entirely
+            return false;
+        } 
+        CollisionInfo* collision = collisions.getNewCollision();
+        if (!collision) {
+            return false;
+        }
+        if (maxBA > halfCubeSide) {
+            // sphere hits cube but its center is outside cube
+            
+            // contactPoint is on surface of sphere
+            glm::vec3 cubeContact = glm::abs(BA);
+            glm::vec3 direction = cubeContact - glm::vec3(halfCubeSide);
+
+            if (direction.x < 0.0f) {
+                direction.x = 0.0f;
+            }
+            if (direction.y < 0.0f) {
+                direction.y = 0.0f;
+            }
+            if (direction.z < 0.0f) {
+                direction.z = 0.0f;
+            }
+
+            glm::vec3 signs = glm::sign(BA);
+            direction.x *= signs.x;
+            direction.y *= signs.y;
+            direction.z *= signs.z;
+            direction = glm::normalize(direction);
+            collision->_contactPoint = sphereCenter + sphereRadius * direction;
+
+            // penetration points from contact point on cube to that on sphere
+            if (cubeContact.x > halfCubeSide) {
+                cubeContact.x = halfCubeSide;
+            }
+            cubeContact.x *= -signs.x;
+            if (cubeContact.y > halfCubeSide) {
+                cubeContact.y = halfCubeSide;
+            }
+            cubeContact.y *= -signs.y;
+            if (cubeContact.z > halfCubeSide) {
+                cubeContact.z = halfCubeSide;
+            }
+            cubeContact.z *= -signs.z;
+            //collision->_penetration = collision->_contactPoint - cubeCenter + cubeContact;
+            collision->_penetration = collision->_contactPoint - (cubeCenter + cubeContact);
+        } else {
+            // sphere center is inside cube
+            // --> push out nearest face
+            glm::vec3 direction;
+            BA /= distance;
+            glm::modf(BA, direction);
+            direction = glm::normalize(direction); 
+
+            // penetration is the projection of surfaceAB on direction
+            collision->_penetration = (halfCubeSide + sphereRadius - distance * glm::dot(BA, direction)) * direction;
+            // contactPoint is on surface of A
+            collision->_contactPoint = sphereCenter + sphereRadius * direction;
+        }
+        return true;
+    } else if (sphereRadius + halfCubeSide > distance) {
+        // NOTE: for cocentric approximation we collide sphere and cube as two spheres which means 
+        // this algorithm will probably be wrong when both sphere and cube are very small (both ~EPSILON)
+        CollisionInfo* collision = collisions.getNewCollision();
+        if (collision) {
+            // the penetration and contactPoint are undefined, so we pick a penetration direction (-yAxis)
+            collision->_penetration = (sphereRadius + halfCubeSide) * glm::vec3(0.0f, -1.0f, 0.0f);
+            // contactPoint is on surface of A
+            collision->_contactPoint = sphereCenter + collision->_penetration;
+            return true;
+        }
+    }
+    return false;
+}
+
+// helper function
+/* KEEP THIS CODE -- this is how to collide the cube with stark face normals (no rounding).
+* We might want to use this code later for sealing boundaries between adjacent voxels.
+bool sphereAACube_StarkAngles(const glm::vec3& sphereCenter, float sphereRadius, const glm::vec3& cubeCenter, 
+        float cubeSide, CollisionList& collisions) {
     glm::vec3 BA = cubeCenter - sphereCenter;
     float distance = glm::length(BA);
     if (distance > EPSILON) {
@@ -608,45 +696,11 @@ bool sphereAACube(const glm::vec3& sphereCenter, float sphereRadius, const glm::
         if (glm::dot(surfaceAB, BA) > 0.f) {
             CollisionInfo* collision = collisions.getNewCollision();
             if (collision) {
-                /* KEEP THIS CODE -- this is how to collide the cube with stark face normals (no rounding).
-                 * We might want to use this code later for sealing boundaries between adjacent voxels.
                 // penetration is parallel to box side direction
                 BA /= maxBA;
                 glm::vec3 direction;
                 glm::modf(BA, direction);
                 direction = glm::normalize(direction); 
-                */
-
-                // For rounded normals at edges and corners:
-                // At this point imagine that sphereCenter touches a "normalized" cube with rounded edges.
-                // This cube has a sidelength of 2 and its smoothing radius is sphereRadius/maxBA.
-                // We're going to try to compute the "negative normal" (and hence direction of penetration) 
-                // of this surface.
-
-                float radius = sphereRadius / (distance * maxBA);   // normalized radius
-                float shortLength = maxBA - radius;
-                glm::vec3 direction = BA;
-                if (shortLength > 0.0f) {
-                    direction = glm::abs(BA) - glm::vec3(shortLength);
-                    // Set any negative components to zero, and adopt the sign of the original BA component.  
-                    // Unfortunately there isn't an easy way to make this fast.
-                    if (direction.x < 0.0f) {
-                        direction.x = 0.f;
-                    } else if (BA.x < 0.f) {
-                        direction.x = -direction.x;
-                    }
-                    if (direction.y < 0.0f) {
-                        direction.y = 0.f;
-                    } else if (BA.y < 0.f) {
-                        direction.y = -direction.y;
-                    }
-                    if (direction.z < 0.0f) {
-                        direction.z = 0.f;
-                    } else if (BA.z < 0.f) {
-                        direction.z = -direction.z;
-                    }
-                }
-                direction = glm::normalize(direction); 
 
                 // penetration is the projection of surfaceAB on direction
                 collision->_penetration = glm::dot(surfaceAB, direction) * direction;
@@ -669,6 +723,7 @@ bool sphereAACube(const glm::vec3& sphereCenter, float sphereRadius, const glm::
     }
     return false;
 }
+*/
 
 bool sphereAACube(const SphereShape* sphereA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions) {
     return sphereAACube(sphereA->getPosition(), sphereA->getRadius(), cubeCenter, cubeSide, collisions);

tests/physics/src/ShapeColliderTests.cpp

@@ -681,58 +681,164 @@ void ShapeColliderTests::capsuleTouchesCapsule() {
     }
 }
 
-void ShapeColliderTests::sphereTouchesAACube() {
+void ShapeColliderTests::sphereTouchesAACubeFaces() {
     CollisionList collisions(16);
     
     glm::vec3 cubeCenter(1.23f, 4.56f, 7.89f);
+    float cubeSide = 2.34f;
+
+    float sphereRadius = 1.13f;
+    glm::vec3 sphereCenter(0.0f);
+    SphereShape sphere(sphereRadius, sphereCenter);
+
+    QVector<glm::vec3> axes;
+    axes.push_back(xAxis);
+    axes.push_back(-xAxis);
+    axes.push_back(yAxis);
+    axes.push_back(-yAxis);
+    axes.push_back(zAxis);
+    axes.push_back(-zAxis);
+
+    for (int i = 0; i < axes.size(); ++i) {
+        glm::vec3 axis = axes[i];
+        // outside
+        {
+            collisions.clear();
+            float overlap = 0.25f;
+            float sphereOffset = 0.5f * cubeSide + sphereRadius - overlap;
+            sphereCenter = cubeCenter + sphereOffset * axis;
+            sphere.setPosition(sphereCenter);
+    
+            if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){
+                std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube.  axis = " << axis << std::endl;
+            }
+            CollisionInfo* collision = collisions[0];
+            if (!collision) {
+                std::cout << __FILE__ << ":" << __LINE__ << " ERROR: no CollisionInfo. axis = " << axis << std::endl;
+            }
+    
+            glm::vec3 expectedPenetration  = - overlap * axis;
+            if (glm::distance(expectedPenetration, collision->_penetration) > EPSILON) {
+                std::cout << __FILE__ << ":" << __LINE__ << " ERROR: penetration = " << collision->_penetration 
+                    << "  expected " << expectedPenetration 
+                    << "  axis = " << axis
+                    << std::endl;
+            }
+    
+            glm::vec3 expectedContact = sphereCenter - sphereRadius * axis;
+            if (glm::distance(expectedContact, collision->_contactPoint) > EPSILON) {
+                std::cout << __FILE__ << ":" << __LINE__ << " ERROR: contactaPoint = " << collision->_contactPoint 
+                    << "  expected " << expectedContact 
+                    << "  axis = " << axis
+                    << std::endl;
+            }
+        }
+
+        // inside
+        {
+            collisions.clear();
+            float overlap = 1.25f * sphereRadius;
+            float sphereOffset = 0.5f * cubeSide + sphereRadius - overlap;
+            sphereCenter = cubeCenter + sphereOffset * axis;
+            sphere.setPosition(sphereCenter);
+    
+            if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){
+                std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube." 
+                    << "  axis = " << axis
+                    << std::endl;
+            }
+            CollisionInfo* collision = collisions[0];
+            if (!collision) {
+                std::cout << __FILE__ << ":" << __LINE__ << " ERROR: no CollisionInfo on y-axis."
+                    << "  axis = " << axis 
+                    << std::endl;
+            }
+    
+            glm::vec3 expectedPenetration  = - overlap * axis;
+            if (glm::distance(expectedPenetration, collision->_penetration) > EPSILON) {
+                std::cout << __FILE__ << ":" << __LINE__ << " ERROR: penetration = " << collision->_penetration 
+                    << "  expected " << expectedPenetration
+                    << "  axis = " << axis 
+                    << std::endl;
+            }
+    
+            glm::vec3 expectedContact = sphereCenter - sphereRadius * axis;
+            if (glm::distance(expectedContact, collision->_contactPoint) > EPSILON) {
+                std::cout << __FILE__ << ":" << __LINE__ << " ERROR: contactaPoint = " << collision->_contactPoint 
+                    << "  expected " << expectedContact
+                    << "  axis = " << axis 
+                    << std::endl;
+            }
+        }
+    }
+}
+
+void ShapeColliderTests::sphereTouchesAACubeEdges() {
+    CollisionList collisions(20);
+    
+    glm::vec3 cubeCenter(0.0f, 0.0f, 0.0f);
     float cubeSide = 2.0f;
 
     float sphereRadius = 1.0f;
     glm::vec3 sphereCenter(0.0f);
     SphereShape sphere(sphereRadius, sphereCenter);
 
-    float sphereOffset = (0.5f * cubeSide + sphereRadius - 0.25f);
+    QVector<glm::vec3> axes;
+    // edges
+    axes.push_back(glm::vec3(0.0f, 1.0f, 1.0f));
+    axes.push_back(glm::vec3(0.0f, 1.0f, -1.0f));
+    axes.push_back(glm::vec3(0.0f, -1.0f, 1.0f));
+    axes.push_back(glm::vec3(0.0f, -1.0f, -1.0f));
+    axes.push_back(glm::vec3(1.0f, 1.0f, 0.0f));
+    axes.push_back(glm::vec3(1.0f, -1.0f, 0.0f));
+    axes.push_back(glm::vec3(-1.0f, 1.0f, 0.0f));
+    axes.push_back(glm::vec3(-1.0f, -1.0f, 0.0f));
+    axes.push_back(glm::vec3(1.0f, 0.0f, 1.0f));
+    axes.push_back(glm::vec3(1.0f, 0.0f, -1.0f));
+    axes.push_back(glm::vec3(-1.0f, 0.0f, 1.0f));
+    axes.push_back(glm::vec3(-1.0f, 0.0f, -1.0f));
+    // and corners
+    axes.push_back(glm::vec3(1.0f, 1.0f, 1.0f));
+    axes.push_back(glm::vec3(1.0f, 1.0f, -1.0f));
+    axes.push_back(glm::vec3(1.0f, -1.0f, 1.0f));
+    axes.push_back(glm::vec3(1.0f, -1.0f, -1.0f));
+    axes.push_back(glm::vec3(-1.0f, 1.0f, 1.0f));
+    axes.push_back(glm::vec3(-1.0f, 1.0f, -1.0f));
+    axes.push_back(glm::vec3(-1.0f, -1.0f, 1.0f));
+    axes.push_back(glm::vec3(-1.0f, -1.0f, -1.0f));
 
-    // top
+    for (int i =0; i < axes.size(); ++i) {
-    sphereCenter = cubeCenter + sphereOffset * yAxis;
+        glm::vec3 axis = axes[i];
-    sphere.setPosition(sphereCenter);
+        float lengthAxis = glm::length(axis);
-    if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){
+        axis /= lengthAxis;
-        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube" << std::endl;
+        float overlap = 0.25f;
-    }
     
-    // bottom
+        sphereCenter = cubeCenter + (lengthAxis * 0.5f * cubeSide + sphereRadius - overlap) * axis;
-    sphereCenter = cubeCenter - sphereOffset * yAxis;
+        sphere.setPosition(sphereCenter);
-    sphere.setPosition(sphereCenter);
+    
-    if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){
+        if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){
-        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube" << std::endl;
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube.  axis = " << axis << std::endl;
-    }
+        }
-
+        CollisionInfo* collision = collisions[i];
-    // left
+        if (!collision) {
-    sphereCenter = cubeCenter + sphereOffset * xAxis;
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: no CollisionInfo. axis = " << axis << std::endl;
-    sphere.setPosition(sphereCenter);
+        }
-    if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){
+    
-        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube" << std::endl;
+        glm::vec3 expectedPenetration  = - overlap * axis;
-    }
+        if (glm::distance(expectedPenetration, collision->_penetration) > EPSILON) {
-
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: penetration = " << collision->_penetration 
-    // right
+                << "  expected " << expectedPenetration 
-    sphereCenter = cubeCenter - sphereOffset * xAxis;
+                << "  axis = " << axis
-    sphere.setPosition(sphereCenter);
+                << std::endl;
-    if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){
+        }
-        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube" << std::endl;
+    
-    }
+        glm::vec3 expectedContact = sphereCenter - sphereRadius * axis;
-
+        if (glm::distance(expectedContact, collision->_contactPoint) > EPSILON) {
-    // forward
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: contactaPoint = " << collision->_contactPoint 
-    sphereCenter = cubeCenter + sphereOffset * zAxis;
+                << "  expected " << expectedContact 
-    sphere.setPosition(sphereCenter);
+                << "  axis = " << axis
-    if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){
+                << std::endl;
-        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube" << std::endl;
+        }
-    }
-
-    // back
-    sphereCenter = cubeCenter - sphereOffset * zAxis;
-    sphere.setPosition(sphereCenter);
-    if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){
-        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube" << std::endl;
     }
 }
 
@@ -802,6 +908,7 @@ void ShapeColliderTests::runAllTests() {
     capsuleMissesCapsule();
     capsuleTouchesCapsule();
 
-    sphereTouchesAACube();
+    sphereTouchesAACubeFaces();
+    sphereTouchesAACubeEdges();
     sphereMissesAACube();
 }

tests/physics/src/ShapeColliderTests.h

@@ -23,7 +23,8 @@ namespace ShapeColliderTests {
     void capsuleMissesCapsule();
     void capsuleTouchesCapsule();
 
-    void sphereTouchesAACube();
+    void sphereTouchesAACubeFaces();
+    void sphereTouchesAACubeEdges();
     void sphereMissesAACube();
 
     void runAllTests();