implement Ray-vs-Capsule and Ray-vs-AACube

libraries/shared/src/AACubeShape.cpp

@@ -9,9 +9,68 @@
 //  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
 //
 
+#include <glm/glm.hpp>
+#include <glm/gtx/norm.hpp>
+
 #include "AACubeShape.h"
+#include "SharedUtil.h" // for SQUARE_ROOT_OF_3 
+
+glm::vec3 planeNormals[3] = { glm::vec3(1.0f, 0.0f, 0.0f), glm::vec3(0.0f, 1.0f, 0.0f), glm::vec3(0.0f, 0.0f, 1.0f) };
 
 bool AACubeShape::findRayIntersection(RayIntersectionInfo& intersection) const {
-    // TODO: Andrew to implement this
-    return false;
+    // A = ray point
+    // B = cube center
+    glm::vec3 BA = _translation - intersection._rayStart;
+
+    // check for ray intersection with cube's bounding sphere
+    // a = distance along line to closest approach to B
+    float a = glm::dot(intersection._rayDirection, BA);
+    // b2 = squared distance from cube center to point of closest approach
+    float b2 = glm::length2(a * intersection._rayDirection - BA);
+    // r = bounding radius of cube
+    float halfSide = 0.5f * _scale;
+    const float r = SQUARE_ROOT_OF_3 * halfSide;
+    if (b2 > r * r) {
+        // line doesn't hit cube's bounding sphere
+        return false;
+    }
+
+    // check for tuncated/short ray
+    const float maxBA = glm::min(intersection._rayLength, intersection._hitDistance) + halfSide;
+    if (maxBA * maxBA > a * a + b2) {
+        // ray is not long enough to reach cube's bounding sphere
+        // NOTE: we don't fall in here when ray's length if FLT_MAX because maxBA^2 will be NaN
+        // and all NaN comparisons are false
+        return false;
+    }
+
+    // the trivial checks have been exhausted, so must trace to each face
+    bool hit = false;
+    for (int i = 0; i < 3; ++i) {
+        for (float sign = -1.0f; sign < 2.0f; sign += 2.0f) {
+            glm::vec3 planeNormal = sign * planeNormals[i];
+            float rayDotPlane = glm::dot(intersection._rayDirection, planeNormal);
+            if (glm::abs(rayDotPlane) > EPSILON) {
+                float distanceToPlane = (halfSide + glm::dot(BA, planeNormal)) / rayDotPlane;
+                if (distanceToPlane >= 0.0f) {
+                    glm::vec3 point = distanceToPlane * intersection._rayDirection - BA;
+                    int j = (i + 1) % 3;
+                    int k = (i + 2) % 3;
+                    glm::vec3 secondNormal = planeNormals[j];
+                    glm::vec3 thirdNormal = planeNormals[k];
+                    if (glm::abs(glm::dot(point, secondNormal)) > halfSide ||
+                            glm::abs(glm::dot(point, thirdNormal)) > halfSide) {
+                        continue;
+                    }
+                    if (distanceToPlane < intersection._hitDistance && distanceToPlane < intersection._rayLength) {
+                        intersection._hitDistance = distanceToPlane;
+                        intersection._hitNormal = planeNormal;
+                        intersection._hitShape = const_cast<AACubeShape*>(this);
+                        hit = true;
+                    }
+                }
+            }
+        }
+    }
+    return hit;
 }

libraries/shared/src/CapsuleShape.cpp

@@ -78,9 +78,135 @@ void CapsuleShape::setEndPoints(const glm::vec3& startPoint, const glm::vec3& en
     updateBoundingRadius();
 }
 
-bool CapsuleShape::findRayIntersection(RayIntersectionInfo& intersection) const {
-        // TODO: Andrew to implement this
+// helper
+bool findRayIntersectionWithCap(const glm::vec3& sphereCenter, float sphereRadius, 
+        const glm::vec3& capsuleCenter, RayIntersectionInfo& intersection) {
+    float r2 = sphereRadius * sphereRadius;
+
+    // compute closest approach (CA)
+    float a = glm::dot(sphereCenter - intersection._rayStart, intersection._rayDirection); // a = distance from ray-start to CA
+    float b2 = glm::distance2(sphereCenter, intersection._rayStart + a * intersection._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 intersection._rayDirection
+    float d2 = glm::distance2(intersection._rayStart, sphereCenter); // d2 = squared distance from sphere-center to ray-start
+    float distance = FLT_MAX;
+    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;
+    }
+    if (distance > 0.0f && distance < intersection._rayLength && distance < intersection._hitDistance) {
+        glm::vec3 sphereCenterToHitPoint = intersection._rayStart + distance * intersection._rayDirection - sphereCenter;
+        if (glm::dot(sphereCenterToHitPoint, sphereCenter - capsuleCenter) >= 0.0f) {
+            intersection._hitDistance = distance;
+            intersection._hitNormal = glm::normalize(sphereCenterToHitPoint);
+            return true;
+        }
+    } 
+    return false;
+}
+
+bool CapsuleShape::findRayIntersectionWithCaps(const glm::vec3& capsuleCenter, RayIntersectionInfo& intersection) const {
+    glm::vec3 capCenter;
+    getStartPoint(capCenter);
+    bool hit = findRayIntersectionWithCap(capCenter, _radius, capsuleCenter, intersection);
+    getEndPoint(capCenter);
+    hit = findRayIntersectionWithCap(capCenter, _radius, capsuleCenter, intersection) || hit;
+    if (hit) {
+        intersection._hitShape = const_cast<CapsuleShape*>(this);
+    }
+    return hit;
+}
+
+bool CapsuleShape::findRayIntersection(RayIntersectionInfo& intersection) const {
+    // ray is U, capsule is V
+    glm::vec3 axisV;
+    computeNormalizedAxis(axisV);
+    glm::vec3 centerV = getTranslation();
+
+    // first handle parallel case
+    float uDotV = glm::dot(axisV, intersection._rayDirection);
+    glm::vec3 UV = intersection._rayStart - centerV;
+    if (glm::abs(1.0f - glm::abs(uDotV)) < EPSILON) {
+        // line and cylinder are parallel
+        float distanceV = glm::dot(UV, intersection._rayDirection);
+        if (glm::length2(UV - distanceV * intersection._rayDirection) <= _radius * _radius) {
+            // ray is inside cylinder's radius and might intersect caps
+            return findRayIntersectionWithCaps(centerV, intersection);
+        }
+        return false;
+    }
+    
+    // Given a line with point 'U' and normalized direction 'u' and 
+    // a cylinder with axial point 'V', radius 'r', and normalized direction 'v'
+    // the intersection of the two is on the line at distance 't' from 'U'.
+    //
+    // Determining the values of t reduces to solving a quadratic equation: At^2 + Bt + C = 0 
+    //
+    // where:
+    //
+    // UV = U-V
+    // w = u-(u.v)v
+    // Q = UV-(UV.v)v
+    //       
+    // A = w^2
+    // B = 2(w.Q)
+    // C = Q^2 - r^2
+
+    glm::vec3 w = intersection._rayDirection - uDotV * axisV;
+    glm::vec3 Q = UV - glm::dot(UV, axisV) * axisV;
+
+    // we save a few multiplies by storing 2*A rather than just A
+    float A2 = 2.0f * glm::dot(w, w);
+    float B = 2.0f * glm::dot(w, Q);
+
+    // since C is only ever used once (in the determinant) we compute it inline
+    float determinant = B * B - 2.0f * A2 * (glm::dot(Q, Q) - _radius * _radius);
+    if (determinant < 0.0f) {
+        return false;
+    }
+    float hitLow  = (-B - sqrtf(determinant)) / A2;
+    float hitHigh = -(hitLow + 2.0f * B / A2);
+
+    if (hitLow > hitHigh) {
+        // re-arrange so hitLow is always the smaller value
+        float temp = hitHigh;
+        hitHigh = hitLow;
+        hitLow = temp;
+    }
+    if (hitLow < 0.0f) {
+        if (hitHigh < 0.0f) {
+            // capsule is completely behind rayStart
+            return false;
+        }
+        hitLow = hitHigh;
+    }
+
+    glm::vec3 p = intersection._rayStart + hitLow * intersection._rayDirection;
+    float d = glm::dot(p - centerV, axisV);
+    if (glm::abs(d) <= getHalfHeight()) {
+        // we definitely hit the cylinder wall
+        intersection._hitDistance = hitLow;
+        intersection._hitNormal = glm::normalize(p - centerV - d * axisV);
+        intersection._hitShape = const_cast<CapsuleShape*>(this);
+        return true;
+    }
+
+    // ray still might hit the caps
+    return findRayIntersectionWithCaps(centerV, intersection);
 }
 
 // static

libraries/shared/src/CapsuleShape.h

@@ -52,6 +52,7 @@ public:
     virtual float getVolume() const { return (PI * _radius * _radius) * (1.3333333333f * _radius + getHalfHeight()); }
 
 protected:
+    bool findRayIntersectionWithCaps(const glm::vec3& capsuleCenter, RayIntersectionInfo& intersection) const;
     virtual void updateBoundingRadius() { _boundingRadius = _radius + getHalfHeight(); }
     static glm::quat computeNewRotation(const glm::vec3& newAxis);
 

libraries/shared/src/PlaneShape.cpp

@@ -48,6 +48,7 @@ bool PlaneShape::findRayIntersection(RayIntersectionInfo& intersection) const {
             // ray starts on the plane
             intersection._hitDistance = 0.0f;
             intersection._hitNormal = n;
+            intersection._hitShape = const_cast<PlaneShape*>(this);
             return true;
         }
     } else {
@@ -56,6 +57,7 @@ bool PlaneShape::findRayIntersection(RayIntersectionInfo& intersection) const {
             // ray points toward plane
             intersection._hitDistance = d;
             intersection._hitNormal = n;
+            intersection._hitShape = const_cast<PlaneShape*>(this);
             return true;
         }
     }

libraries/shared/src/SphereShape.cpp

@@ -44,6 +44,7 @@ bool SphereShape::findRayIntersection(RayIntersectionInfo& intersection) const {
     if (distance > 0.0f && distance < intersection._rayLength && distance < intersection._hitDistance) {
         intersection._hitDistance = distance;
         intersection._hitNormal = glm::normalize(intersection._rayStart + distance * intersection._rayDirection - _translation);
+        intersection._hitShape = const_cast<SphereShape*>(this);
         return true;
     }
     return false;

tests/physics/src/ShapeColliderTests.cpp

@@ -2150,6 +2150,7 @@ void ShapeColliderTests::rayHitsPlane() {
 }
 
 void ShapeColliderTests::rayMissesPlane() {
+    // TODO: Andrew to test RayIntersectionInfo::_hitShape
     // make a simple plane
     float planeDistanceFromOrigin = 3.579f;
     glm::vec3 planePosition(0.0f, planeDistanceFromOrigin, 0.0f);
@@ -2229,6 +2230,14 @@ void ShapeColliderTests::rayMissesPlane() {
     }
 }
 
+void ShapeColliderTests::rayHitsAACube() {
+    // TODO: Andrew to implement this
+}
+
+void ShapeColliderTests::rayMissesAACube() {
+    // TODO: Andrew to implement this
+}
+
 void ShapeColliderTests::measureTimeOfCollisionDispatch() {
     /* KEEP for future manual testing
     // create two non-colliding spheres
@@ -2288,4 +2297,7 @@ void ShapeColliderTests::runAllTests() {
     rayMissesCapsule();
     rayHitsPlane();
     rayMissesPlane();
+
+    rayHitsAACube();
+    rayMissesAACube();
 }

tests/physics/src/ShapeColliderTests.h

@@ -38,6 +38,8 @@ namespace ShapeColliderTests {
     void rayMissesCapsule();
     void rayHitsPlane();
     void rayMissesPlane();
+    void rayHitsAACube();
+    void rayMissesAACube();
 
     void measureTimeOfCollisionDispatch();