Add unit tests for ray intersections with shapes.

tests/physics/src/ShapeColliderTests.cpp

@@ -11,6 +11,7 @@
 
 //#include <stdio.h>
 #include <iostream>
+#include <math.h>
 
 #include <glm/glm.hpp>
 #include <glm/gtx/quaternion.hpp>
@@ -897,6 +898,405 @@ void ShapeColliderTests::sphereMissesAACube() {
     }
 }
 
+void ShapeColliderTests::rayHitsSphere() {
+    float startDistance = 3.0f;
+    glm::vec3 rayStart(-startDistance, 0.0f, 0.0f);
+    glm::vec3 rayDirection(1.0f, 0.0f, 0.0f);
+
+    float radius = 1.0f;
+    glm::vec3 center(0.0f);
+
+    SphereShape sphere(radius, center);
+
+    // very simple ray along xAxis
+    {
+        float distance = FLT_MAX;
+        if (!ShapeCollider::findRayIntersectionWithShape(&sphere, rayStart, rayDirection, distance)) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should intersect sphere" << std::endl;
+        }
+    
+        float expectedDistance = startDistance - radius;
+        float relativeError = fabsf(distance - expectedDistance) / startDistance;
+        if (relativeError > EPSILON) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray sphere intersection distance error = " << relativeError << std::endl;
+        }
+    }
+
+    // ray along a diagonal axis
+    {
+        rayStart = glm::vec3(startDistance, startDistance, 0.0f);
+        rayDirection = - glm::normalize(rayStart);
+    
+        float distance = FLT_MAX;
+        if (!ShapeCollider::findRayIntersectionWithShape(&sphere, rayStart, rayDirection, distance)) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should intersect sphere" << std::endl;
+        }
+    
+        float expectedDistance = SQUARE_ROOT_OF_2 * startDistance - radius;
+        float relativeError = fabsf(distance - expectedDistance) / startDistance;
+        if (relativeError > EPSILON) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray sphere intersection distance error = " << relativeError << std::endl;
+        }
+    }
+
+    // rotated and displaced ray and sphere
+    {
+        startDistance = 7.41f;
+        radius = 3.917f;
+
+        glm::vec3 axis = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f));
+        glm::quat rotation = glm::angleAxis(0.987654321f, axis);
+        glm::vec3 translation(35.7f, 2.46f, -1.97f);
+
+        glm::vec3 unrotatedRayDirection(-1.0f, 0.0f, 0.0f);
+        glm::vec3 untransformedRayStart(startDistance, 0.0f, 0.0f);
+
+        rayStart = rotation * (untransformedRayStart + translation);
+        rayDirection = rotation * unrotatedRayDirection;
+
+        sphere.setRadius(radius);
+        sphere.setPosition(rotation * translation);
+    
+        float distance = FLT_MAX;
+        if (!ShapeCollider::findRayIntersectionWithShape(&sphere, rayStart, rayDirection, distance)) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should intersect sphere" << std::endl;
+        }
+    
+        float expectedDistance = startDistance - radius;
+        float relativeError = fabsf(distance - expectedDistance) / startDistance;
+        if (relativeError > EPSILON) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray sphere intersection distance error = " << relativeError << std::endl;
+        }
+    }
+}
+
+void ShapeColliderTests::rayBarelyHitsSphere() {
+    float radius = 1.0f;
+    glm::vec3 center(0.0f);
+    float delta = 2.0f * EPSILON;
+
+    float startDistance = 3.0f;
+    glm::vec3 rayStart(-startDistance, radius - delta, 0.0f);
+    glm::vec3 rayDirection(1.0f, 0.0f, 0.0f);
+
+    SphereShape sphere(radius, center);
+
+    // very simple ray along xAxis
+    float distance = FLT_MAX;
+    if (!ShapeCollider::findRayIntersectionWithShape(&sphere, rayStart, rayDirection, distance)) {
+        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should just barely hit sphere" << std::endl;
+    }
+
+    // translate and rotate the whole system... 
+    glm::vec3 axis = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f));
+    glm::quat rotation = glm::angleAxis(0.987654321f, axis);
+    glm::vec3 translation(35.7f, 2.46f, -1.97f);
+
+    rayStart = rotation * (rayStart + translation);
+    rayDirection = rotation * rayDirection;
+    sphere.setPosition(rotation * translation);
+
+    // ...and test again
+    distance = FLT_MAX;
+    if (!ShapeCollider::findRayIntersectionWithShape(&sphere, rayStart, rayDirection, distance)) {
+        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should just barely hit sphere" << std::endl;
+    }
+}
+
+
+void ShapeColliderTests::rayBarelyMissesSphere() {
+    // same as the barely-hits case, but this time we move the ray away from sphere
+    float radius = 1.0f;
+    glm::vec3 center(0.0f);
+    float delta = 2.0f * EPSILON;
+
+    float startDistance = 3.0f;
+    glm::vec3 rayStart(-startDistance, radius + delta, 0.0f);
+    glm::vec3 rayDirection(1.0f, 0.0f, 0.0f);
+
+    SphereShape sphere(radius, center);
+
+    // very simple ray along xAxis
+    float distance = FLT_MAX;
+    if (ShapeCollider::findRayIntersectionWithShape(&sphere, rayStart, rayDirection, distance)) {
+        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should just barely miss sphere" << std::endl;
+    }
+    if (distance != FLT_MAX) {
+        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" << std::endl;
+    }
+
+    // translate and rotate the whole system... 
+    glm::vec3 axis = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f));
+    glm::quat rotation = glm::angleAxis(0.987654321f, axis);
+    glm::vec3 translation(35.7f, 2.46f, -1.97f);
+
+    rayStart = rotation * (rayStart + translation);
+    rayDirection = rotation * rayDirection;
+    sphere.setPosition(rotation * translation);
+
+    // ...and test again
+    distance = FLT_MAX;
+    if (ShapeCollider::findRayIntersectionWithShape(&sphere, rayStart, rayDirection, distance)) {
+        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should just barely miss sphere" << std::endl;
+    }
+    if (distance != FLT_MAX) {
+        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" << std::endl;
+    }
+}
+
+void ShapeColliderTests::rayHitsCapsule() {
+    float startDistance = 3.0f;
+    float radius = 1.0f;
+    float halfHeight = 2.0f;
+    glm::vec3 center(0.0f);
+    CapsuleShape capsule(radius, halfHeight);
+
+    { // simple test along xAxis 
+        // toward capsule center
+        glm::vec3 rayStart(startDistance, 0.0f, 0.0f);
+        glm::vec3 rayDirection(-1.0f, 0.0f, 0.0f);
+        float distance = FLT_MAX;
+        if (!ShapeCollider::findRayIntersectionWithShape(&capsule, rayStart, rayDirection, distance)) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit capsule" << std::endl;
+        }
+        float expectedDistance = startDistance - radius;
+        float relativeError = fabsf(distance - expectedDistance) / startDistance;
+        if (relativeError > EPSILON) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = " << relativeError << std::endl;
+        }
+
+        // toward top of cylindrical wall
+        rayStart.y = halfHeight;
+        distance = FLT_MAX;
+        if (!ShapeCollider::findRayIntersectionWithShape(&capsule, rayStart, rayDirection, distance)) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit capsule" << std::endl;
+        }
+        relativeError = fabsf(distance - expectedDistance) / startDistance;
+        if (relativeError > EPSILON) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = " << relativeError << std::endl;
+        }
+
+        // toward top cap
+        float delta = 2.0f * EPSILON;
+        rayStart.y = halfHeight + delta;
+        distance = FLT_MAX;
+        if (!ShapeCollider::findRayIntersectionWithShape(&capsule, rayStart, rayDirection, distance)) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit capsule" << std::endl;
+        }
+        relativeError = fabsf(distance - expectedDistance) / startDistance;
+        if (relativeError > EPSILON) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = " << relativeError << std::endl;
+        }
+
+        const float EDGE_CASE_SLOP_FACTOR = 20.0f;
+
+        // toward tip of top cap
+        rayStart.y = halfHeight + radius - delta;
+        distance = FLT_MAX;
+        if (!ShapeCollider::findRayIntersectionWithShape(&capsule, rayStart, rayDirection, distance)) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit capsule" << std::endl;
+        }
+        expectedDistance = startDistance - radius * sqrtf(2.0f * delta);    // using small angle approximation of cosine
+        relativeError = fabsf(distance - expectedDistance) / startDistance;
+        // for edge cases we allow a LOT of error
+        if (relativeError > EDGE_CASE_SLOP_FACTOR * EPSILON) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = " << relativeError << std::endl;
+        }
+
+        // toward tip of bottom cap
+        rayStart.y = - halfHeight - radius + delta;
+        distance = FLT_MAX;
+        if (!ShapeCollider::findRayIntersectionWithShape(&capsule, rayStart, rayDirection, distance)) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit capsule" << std::endl;
+        }
+        expectedDistance = startDistance - radius * sqrtf(2.0f * delta);    // using small angle approximation of cosine
+        relativeError = fabsf(distance - expectedDistance) / startDistance;
+        // for edge cases we allow a LOT of error
+        if (relativeError > EDGE_CASE_SLOP_FACTOR * EPSILON) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = " << relativeError << std::endl;
+        }
+
+        // toward edge of capsule cylindrical face
+        rayStart.y = 0.0f;
+        rayStart.z = radius - delta;
+        distance = FLT_MAX;
+        if (!ShapeCollider::findRayIntersectionWithShape(&capsule, rayStart, rayDirection, distance)) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit capsule" << std::endl;
+        }
+        expectedDistance = startDistance - radius * sqrtf(2.0f * delta);    // using small angle approximation of cosine
+        relativeError = fabsf(distance - expectedDistance) / startDistance;
+        // for edge cases we allow a LOT of error
+        if (relativeError > EDGE_CASE_SLOP_FACTOR * EPSILON) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = " << relativeError << std::endl;
+        }
+    }
+    // TODO: test at steep angles near cylinder/cap junction
+}
+
+void ShapeColliderTests::rayMissesCapsule() {
+    // same as edge case hit tests, but shifted in the opposite direction
+    float startDistance = 3.0f;
+    float radius = 1.0f;
+    float halfHeight = 2.0f;
+    glm::vec3 center(0.0f);
+    CapsuleShape capsule(radius, halfHeight);
+
+    { // simple test along xAxis 
+        // toward capsule center
+        glm::vec3 rayStart(startDistance, 0.0f, 0.0f);
+        glm::vec3 rayDirection(-1.0f, 0.0f, 0.0f);
+        float delta = 2.0f * EPSILON;
+
+        // over top cap
+        rayStart.y = halfHeight + radius + delta;
+        float distance = FLT_MAX;
+        if (ShapeCollider::findRayIntersectionWithShape(&capsule, rayStart, rayDirection, distance)) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss capsule" << std::endl;
+        }
+        if (distance != FLT_MAX) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" << std::endl;
+        }
+
+        // below bottom cap
+        rayStart.y = - halfHeight - radius - delta;
+        distance = FLT_MAX;
+        if (ShapeCollider::findRayIntersectionWithShape(&capsule, rayStart, rayDirection, distance)) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss capsule" << std::endl;
+        }
+        if (distance != FLT_MAX) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" << std::endl;
+        }
+
+        // past edge of capsule cylindrical face
+        rayStart.y = 0.0f;
+        rayStart.z = radius + delta;
+        distance = FLT_MAX;
+        if (ShapeCollider::findRayIntersectionWithShape(&capsule, rayStart, rayDirection, distance)) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss capsule" << std::endl;
+        }
+        if (distance != FLT_MAX) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" << std::endl;
+        }
+    }
+    // TODO: test at steep angles near edge
+}
+
+void ShapeColliderTests::rayHitsPlane() {
+    // make a simple plane
+    float planeDistanceFromOrigin = 3.579;
+    glm::vec3 planePosition(0.0f, planeDistanceFromOrigin, 0.0f);
+    PlaneShape plane;
+    plane.setPosition(planePosition);
+
+    // make a simple ray
+    float startDistance = 1.234f;
+    glm::vec3 rayStart(-startDistance, 0.0f, 0.0f);
+    glm::vec3 rayDirection = glm::normalize(glm::vec3(1.0f, 1.0f, 1.0f));
+
+    float distance = FLT_MAX;
+    if (!ShapeCollider::findRayIntersectionWithShape(&plane, rayStart, rayDirection, distance)) {
+        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit plane" << std::endl;
+    }
+
+    float expectedDistance = SQUARE_ROOT_OF_3 * planeDistanceFromOrigin;
+    float relativeError = fabsf(distance - expectedDistance) / planeDistanceFromOrigin;
+    if (relativeError > EPSILON) {
+        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray plane intersection distance error = " << relativeError << std::endl;
+    }
+
+    // rotate the whole system and try again
+    float angle = 37.8f;
+    glm::vec3 axis = glm::normalize( glm::vec3(-7.0f, 2.8f, 9.3f) );
+    glm::quat rotation = glm::angleAxis(angle, axis);
+
+    plane.setPosition(rotation * planePosition);
+    plane.setRotation(rotation);
+    rayStart = rotation * rayStart;
+    rayDirection = rotation * rayDirection;
+
+    distance = FLT_MAX;
+    if (!ShapeCollider::findRayIntersectionWithShape(&plane, rayStart, rayDirection, distance)) {
+        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit plane" << std::endl;
+    }
+
+    expectedDistance = SQUARE_ROOT_OF_3 * planeDistanceFromOrigin;
+    relativeError = fabsf(distance - expectedDistance) / planeDistanceFromOrigin;
+    if (relativeError > EPSILON) {
+        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray plane intersection distance error = " << relativeError << std::endl;
+    }
+}
+
+void ShapeColliderTests::rayMissesPlane() {
+    // make a simple plane
+    float planeDistanceFromOrigin = 3.579;
+    glm::vec3 planePosition(0.0f, planeDistanceFromOrigin, 0.0f);
+    PlaneShape plane;
+    plane.setPosition(planePosition);
+
+    { // parallel rays should miss
+        float startDistance = 1.234f;
+        glm::vec3 rayStart(-startDistance, 0.0f, 0.0f);
+        glm::vec3 rayDirection = glm::normalize(glm::vec3(-1.0f, 0.0f, -1.0f));
+    
+        float distance = FLT_MAX;
+        if (ShapeCollider::findRayIntersectionWithShape(&plane, rayStart, rayDirection, distance)) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss plane" << std::endl;
+        }
+        if (distance != FLT_MAX) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" << std::endl;
+        }
+    
+        // rotate the whole system and try again
+        float angle = 37.8f;
+        glm::vec3 axis = glm::normalize( glm::vec3(-7.0f, 2.8f, 9.3f) );
+        glm::quat rotation = glm::angleAxis(angle, axis);
+    
+        plane.setPosition(rotation * planePosition);
+        plane.setRotation(rotation);
+        rayStart = rotation * rayStart;
+        rayDirection = rotation * rayDirection;
+    
+        distance = FLT_MAX;
+        if (ShapeCollider::findRayIntersectionWithShape(&plane, rayStart, rayDirection, distance)) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss plane" << std::endl;
+        }
+        if (distance != FLT_MAX) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" << std::endl;
+        }
+    }
+
+    { // make a simple ray that points away from plane
+        float startDistance = 1.234f;
+        glm::vec3 rayStart(-startDistance, 0.0f, 0.0f);
+        glm::vec3 rayDirection = glm::normalize(glm::vec3(-1.0f, -1.0f, -1.0f));
+    
+        float distance = FLT_MAX;
+        if (ShapeCollider::findRayIntersectionWithShape(&plane, rayStart, rayDirection, distance)) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss plane" << std::endl;
+        }
+        if (distance != FLT_MAX) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" << std::endl;
+        }
+    
+        // rotate the whole system and try again
+        float angle = 37.8f;
+        glm::vec3 axis = glm::normalize( glm::vec3(-7.0f, 2.8f, 9.3f) );
+        glm::quat rotation = glm::angleAxis(angle, axis);
+    
+        plane.setPosition(rotation * planePosition);
+        plane.setRotation(rotation);
+        rayStart = rotation * rayStart;
+        rayDirection = rotation * rayDirection;
+    
+        distance = FLT_MAX;
+        if (ShapeCollider::findRayIntersectionWithShape(&plane, rayStart, rayDirection, distance)) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss plane" << std::endl;
+        }
+        if (distance != FLT_MAX) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" << std::endl;
+        }
+    }
+}
 
 void ShapeColliderTests::runAllTests() {
     sphereMissesSphere();
@@ -911,4 +1311,12 @@ void ShapeColliderTests::runAllTests() {
     sphereTouchesAACubeFaces();
     sphereTouchesAACubeEdges();
     sphereMissesAACube();
+
+    rayHitsSphere();
+    rayBarelyHitsSphere();
+    rayBarelyMissesSphere();
+    rayHitsCapsule();
+    rayMissesCapsule();
+    rayHitsPlane();
+    rayMissesPlane();
 }

tests/physics/src/ShapeColliderTests.h

@@ -27,6 +27,14 @@ namespace ShapeColliderTests {
     void sphereTouchesAACubeEdges();
     void sphereMissesAACube();
 
+    void rayHitsSphere();
+    void rayBarelyHitsSphere();
+    void rayBarelyMissesSphere();
+    void rayHitsCapsule();
+    void rayMissesCapsule();
+    void rayHitsPlane();
+    void rayMissesPlane();
+
     void runAllTests(); 
 }