Merge pull request #3030 from AndrewMeadows/inertia

raycasts against Shapes

libraries/shared/src/CapsuleShape.cpp

@@ -13,6 +13,8 @@
 #include <glm/gtx/vector_angle.hpp>
 
 #include "CapsuleShape.h"
+
+#include "GeometryUtil.h"
 #include "SharedUtil.h" 
 
 
@@ -84,3 +86,11 @@ void CapsuleShape::setEndPoints(const glm::vec3& startPoint, const glm::vec3& en
     updateBoundingRadius();
 }
 
+bool CapsuleShape::findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const {
+        glm::vec3 capsuleStart, capsuleEnd;
+        getStartPoint(capsuleStart);
+        getEndPoint(capsuleEnd);
+        // NOTE: findRayCapsuleIntersection returns 'true' with distance = 0 when rayStart is inside capsule.
+        // TODO: implement the raycast to return inside surface intersection for the internal rayStart.
+        return findRayCapsuleIntersection(rayStart, rayDirection, capsuleStart, capsuleEnd, _radius, distance);
+}

libraries/shared/src/CapsuleShape.h

@@ -39,6 +39,8 @@ public:
     void setRadiusAndHalfHeight(float radius, float height);
     void setEndPoints(const glm::vec3& startPoint, const glm::vec3& endPoint);
 
+    bool findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const;
+
 protected:
     void updateBoundingRadius() { _boundingRadius = _radius + _halfHeight; }
 

libraries/shared/src/ListShape.h

@@ -55,6 +55,9 @@ public:
 
     void setShapes(QVector<ListShapeEntry>& shapes);
 
+    // TODO: either implement this or remove ListShape altogether
+    bool findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const { return false; }
+
 protected:
     void clear();
     void computeBoundingRadius();

libraries/shared/src/PlaneShape.cpp

@@ -30,7 +30,28 @@ PlaneShape::PlaneShape(const glm::vec4& coefficients) :
     }
 }
 
+glm::vec3 PlaneShape::getNormal() const {
+    return _rotation * UNROTATED_NORMAL;
+}
+
 glm::vec4 PlaneShape::getCoefficients() const {
     glm::vec3 normal = _rotation * UNROTATED_NORMAL;
     return glm::vec4(normal.x, normal.y, normal.z, -glm::dot(normal, _position));
 }
+
+bool PlaneShape::findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const {
+    glm::vec3 n = getNormal();
+    float denominator = glm::dot(n, rayDirection);
+    if (fabsf(denominator) < EPSILON) {
+        // line is parallel to plane
+        return glm::dot(_position - rayStart, n) < EPSILON;
+    } else {
+        float d = glm::dot(_position - rayStart, n) / denominator;
+        if (d > 0.0f) {
+            // ray points toward plane
+            distance = d;
+            return true;
+        }
+    }
+    return false;
+}

libraries/shared/src/PlaneShape.h

@@ -18,7 +18,10 @@ class PlaneShape : public Shape {
 public:
     PlaneShape(const glm::vec4& coefficients = glm::vec4(0.0f, 1.0f, 0.0f, 0.0f));
     
+    glm::vec3 getNormal() const;
     glm::vec4 getCoefficients() const;
+
+    bool findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const;
 };
 
 #endif // hifi_PlaneShape_h

libraries/shared/src/Shape.h

@@ -38,6 +38,8 @@ public:
     virtual void setPosition(const glm::vec3& position) { _position = position; }
     virtual void setRotation(const glm::quat& rotation) { _rotation = rotation; }
 
+    virtual bool findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const = 0;
+
 protected:
     // these ctors are protected (used by derived classes only)
     Shape(Type type) : _type(type), _boundingRadius(0.f), _position(0.f), _rotation() {}

libraries/shared/src/ShapeCollider.cpp

@@ -765,5 +765,24 @@ bool capsuleAACube(const CapsuleShape* capsuleA, const glm::vec3& cubeCenter, fl
     return sphereAACube(nearestApproach, capsuleA->getRadius(), cubeCenter, cubeSide, collisions);
 }
 
+bool findRayIntersectionWithShapes(const QVector<Shape*> shapes, const glm::vec3& rayStart, const glm::vec3& rayDirection, float& minDistance) {
+    float hitDistance = FLT_MAX;
+    int numShapes = shapes.size();
+    for (int i = 0; i < numShapes; ++i) {
+        Shape* shape = shapes.at(i);
+        if (shape) {
+            float distance;
+            if (shape->findRayIntersection(rayStart, rayDirection, distance)) {
+                if (distance < hitDistance) {
+                    hitDistance = distance;
+                }
+            }
+        }
+    }
+    if (hitDistance < FLT_MAX) {
+        minDistance = hitDistance;
+    }
+    return false;
+}
 
 }   // namespace ShapeCollider

libraries/shared/src/ShapeCollider.h

@@ -21,8 +21,8 @@
 
 namespace ShapeCollider {
 
-    /// \param shapeA pointer to first shape
-    /// \param shapeB pointer to second shape
+    /// \param shapeA pointer to first shape (cannot be NULL)
+    /// \param shapeB pointer to second shape (cannot be NULL)
     /// \param collisions[out] collision details
     /// \return true if shapes collide
     bool collideShapes(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions);
@@ -33,123 +33,130 @@ namespace ShapeCollider {
     /// \return true if any shapes collide
     bool collideShapesCoarse(const QVector<const Shape*>& shapesA, const QVector<const Shape*>& shapesB, CollisionInfo& collision);
 
-    /// \param shapeA a pointer to a shape
+    /// \param shapeA a pointer to a shape (cannot be NULL)
     /// \param cubeCenter center of cube
     /// \param cubeSide lenght of side of cube
     /// \param collisions[out] average collision details
     /// \return true if shapeA collides with axis aligned cube
     bool collideShapeWithAACube(const Shape* shapeA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions);
 
-    /// \param sphereA pointer to first shape
-    /// \param sphereB pointer to second shape
+    /// \param sphereA pointer to first shape (cannot be NULL)
+    /// \param sphereB pointer to second shape (cannot be NULL)
     /// \param[out] collisions where to append collision details
     /// \return true if shapes collide
     bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, CollisionList& collisions);
 
-    /// \param sphereA pointer to first shape
-    /// \param capsuleB pointer to second shape
+    /// \param sphereA pointer to first shape (cannot be NULL)
+    /// \param capsuleB pointer to second shape (cannot be NULL)
     /// \param[out] collisions where to append collision details
     /// \return true if shapes collide
     bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, CollisionList& collisions);
 
-    /// \param sphereA pointer to first shape
-    /// \param planeB pointer to second shape
+    /// \param sphereA pointer to first shape (cannot be NULL)
+    /// \param planeB pointer to second shape (cannot be NULL)
     /// \param[out] collisions where to append collision details
     /// \return true if shapes collide
     bool spherePlane(const SphereShape* sphereA, const PlaneShape* planeB, CollisionList& collisions);
     
-    /// \param capsuleA pointer to first shape
-    /// \param sphereB pointer to second shape
+    /// \param capsuleA pointer to first shape (cannot be NULL)
+    /// \param sphereB pointer to second shape (cannot be NULL)
     /// \param[out] collisions where to append collision details
     /// \return true if shapes collide
     bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, CollisionList& collisions);
 
-    /// \param capsuleA pointer to first shape
-    /// \param capsuleB pointer to second shape
+    /// \param capsuleA pointer to first shape (cannot be NULL)
+    /// \param capsuleB pointer to second shape (cannot be NULL)
     /// \param[out] collisions where to append collision details
     /// \return true if shapes collide
     bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, CollisionList& collisions);
 
-    /// \param capsuleA pointer to first shape
-    /// \param planeB pointer to second shape
+    /// \param capsuleA pointer to first shape (cannot be NULL)
+    /// \param planeB pointer to second shape (cannot be NULL)
     /// \param[out] collisions where to append collision details
     /// \return true if shapes collide
     bool capsulePlane(const CapsuleShape* capsuleA, const PlaneShape* planeB, CollisionList& collisions);
     
-    /// \param planeA pointer to first shape
-    /// \param sphereB pointer to second shape
+    /// \param planeA pointer to first shape (cannot be NULL)
+    /// \param sphereB pointer to second shape (cannot be NULL)
     /// \param[out] collisions where to append collision details
     /// \return true if shapes collide
     bool planeSphere(const PlaneShape* planeA, const SphereShape* sphereB, CollisionList& collisions);
 
-    /// \param planeA pointer to first shape
-    /// \param capsuleB pointer to second shape
+    /// \param planeA pointer to first shape (cannot be NULL)
+    /// \param capsuleB pointer to second shape (cannot be NULL)
     /// \param[out] collisions where to append collision details
     /// \return true if shapes collide
     bool planeCapsule(const PlaneShape* planeA, const CapsuleShape* capsuleB, CollisionList& collisions);
 
-    /// \param planeA pointer to first shape
-    /// \param planeB pointer to second shape
+    /// \param planeA pointer to first shape (cannot be NULL)
+    /// \param planeB pointer to second shape (cannot be NULL)
     /// \param[out] collisions where to append collision details
     /// \return true if shapes collide
     bool planePlane(const PlaneShape* planeA, const PlaneShape* planeB, CollisionList& collisions);
     
-    /// \param sphereA pointer to first shape
-    /// \param listB pointer to second shape
+    /// \param sphereA pointer to first shape (cannot be NULL)
+    /// \param listB pointer to second shape (cannot be NULL)
     /// \param[out] collisions where to append collision details
     /// \return true if shapes collide
     bool sphereList(const SphereShape* sphereA, const ListShape* listB, CollisionList& collisions);
 
-    /// \param capuleA pointer to first shape
-    /// \param listB pointer to second shape
+    /// \param capuleA pointer to first shape (cannot be NULL)
+    /// \param listB pointer to second shape (cannot be NULL)
     /// \param[out] collisions where to append collision details
     /// \return true if shapes collide
     bool capsuleList(const CapsuleShape* capsuleA, const ListShape* listB, CollisionList& collisions);
 
-    /// \param planeA pointer to first shape
-    /// \param listB pointer to second shape
+    /// \param planeA pointer to first shape (cannot be NULL)
+    /// \param listB pointer to second shape (cannot be NULL)
     /// \param[out] collisions where to append collision details
     /// \return true if shapes collide
     bool planeList(const PlaneShape* planeA, const ListShape* listB, CollisionList& collisions);
     
-    /// \param listA pointer to first shape
-    /// \param sphereB pointer to second shape
+    /// \param listA pointer to first shape (cannot be NULL)
+    /// \param sphereB pointer to second shape (cannot be NULL)
     /// \param[out] collisions where to append collision details
     /// \return true if shapes collide
     bool listSphere(const ListShape* listA, const SphereShape* sphereB, CollisionList& collisions);
 
-    /// \param listA pointer to first shape
-    /// \param capsuleB pointer to second shape
+    /// \param listA pointer to first shape (cannot be NULL)
+    /// \param capsuleB pointer to second shape (cannot be NULL)
     /// \param[out] collisions where to append collision details
     /// \return true if shapes collide
     bool listCapsule(const ListShape* listA, const CapsuleShape* capsuleB, CollisionList& collisions);
 
-    /// \param listA pointer to first shape
-    /// \param planeB pointer to second shape
+    /// \param listA pointer to first shape (cannot be NULL)
+    /// \param planeB pointer to second shape (cannot be NULL)
     /// \param[out] collisions where to append collision details
     /// \return true if shapes collide
     bool listPlane(const ListShape* listA, const PlaneShape* planeB, CollisionList& collisions);
     
-    /// \param listA pointer to first shape
-    /// \param capsuleB pointer to second shape
+    /// \param listA pointer to first shape (cannot be NULL)
+    /// \param capsuleB pointer to second shape (cannot be NULL)
     /// \param[out] collisions where to append collision details
     /// \return true if shapes collide
     bool listList(const ListShape* listA, const ListShape* listB, CollisionList& collisions);
 
-    /// \param sphereA pointer to sphere
+    /// \param sphereA pointer to sphere (cannot be NULL)
     /// \param cubeCenter center of cube
     /// \param cubeSide lenght of side of cube
     /// \param[out] collisions where to append collision details
     /// \return true if sphereA collides with axis aligned cube
     bool sphereAACube(const SphereShape* sphereA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions);
 
-    /// \param capsuleA pointer to capsule
+    /// \param capsuleA pointer to capsule (cannot be NULL)
     /// \param cubeCenter center of cube
     /// \param cubeSide lenght of side of cube
     /// \param[out] collisions where to append collision details
     /// \return true if capsuleA collides with axis aligned cube
     bool capsuleAACube(const CapsuleShape* capsuleA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions);
 
+    /// \param shapes list of pointers to shapes (shape pointers may be NULL)
+    /// \param startPoint beginning of ray
+    /// \param direction direction of ray
+    /// \param minDistance[out] shortest distance to intersection of ray with a shapes
+    /// \return true if ray hits any shape in shapes
+    bool findRayIntersectionWithShapes(const QVector<Shape*> shapes, const glm::vec3& startPoint, const glm::vec3& direction, float& minDistance);
+
 }   // namespace ShapeCollider
 
 #endif // hifi_ShapeCollider_h

libraries/shared/src/SphereShape.cpp

@@ -0,0 +1,44 @@
+//
+//  SphereShape.cpp
+//  libraries/shared/src
+//
+//  Created by Andrew Meadows on 2014.06.17
+//  Copyright 2014 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#include <glm/gtx/norm.hpp>
+
+#include "SphereShape.h"
+
+bool SphereShape::findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const {
+    float r2 = _boundingRadius * _boundingRadius;
+
+    // compute closest approach (CA)
+    float a = glm::dot(_position - rayStart, rayDirection); // a = distance from ray-start to CA
+    float b2 = glm::distance2(_position, rayStart + a * rayDirection); // b2 = squared distance from sphere-center to CA
+    if (b2 > r2) {
+        // ray does not hit sphere
+        return false;
+    }
+    float c = sqrtf(r2 - b2); // c = distance from CA to sphere surface along rayDirection
+    float d2 = glm::distance2(rayStart, _position); // d2 = squared distance from sphere-center to ray-start
+    if (a < 0.0f) {
+        // ray points away from sphere-center
+        if (d2 > r2) {
+            // ray starts outside sphere
+            return false;
+        }
+        // ray starts inside sphere
+        distance = c + a;
+    } else if (d2 > r2) {
+        // ray starts outside sphere
+        distance = a - c;
+    } else {
+        // ray starts inside sphere
+        distance = a + c;
+    }
+    return true;
+}

libraries/shared/src/SphereShape.h

@@ -29,6 +29,8 @@ public:
     float getRadius() const { return _boundingRadius; }
 
     void setRadius(float radius) { _boundingRadius = radius; }
+
+    bool findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const;
 };
 
 #endif // hifi_SphereShape_h

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 (!sphere.findRayIntersection(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 (!sphere.findRayIntersection(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 (!sphere.findRayIntersection(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 (!sphere.findRayIntersection(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 (!sphere.findRayIntersection(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 (sphere.findRayIntersection(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 (sphere.findRayIntersection(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 (!capsule.findRayIntersection(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 (!capsule.findRayIntersection(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 (!capsule.findRayIntersection(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 (!capsule.findRayIntersection(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 (!capsule.findRayIntersection(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 (!capsule.findRayIntersection(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 (capsule.findRayIntersection(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 (capsule.findRayIntersection(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 (capsule.findRayIntersection(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 (!plane.findRayIntersection(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 (!plane.findRayIntersection(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 (plane.findRayIntersection(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 (plane.findRayIntersection(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 (plane.findRayIntersection(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 (plane.findRayIntersection(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(); 
 }