Add testRayIntersect() method to AABB and use in raycasting broadphase

src/collision/broadphase/DynamicAABBTree.cpp

@@ -637,11 +637,6 @@ void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &ca
 
     decimal maxFraction = ray.maxFraction;
 
-    // Create an AABB for the ray
-    Vector3 endPoint = ray.point1 + maxFraction * (ray.point2 - ray.point1);
-    AABB rayAABB(Vector3::min(ray.point1, endPoint),
-                 Vector3::max(ray.point1, endPoint));
-
     Stack<int, 128> stack;
     stack.push(mRootNodeID);
 
@@ -658,14 +653,14 @@ void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &ca
         // Get the corresponding node
         const TreeNode* node = mNodes + nodeID;
 
-        // Test if the node AABB overlaps with the ray AABB
+        Ray rayTemp(ray.point1, ray.point2, maxFraction);
-        if (!rayAABB.testCollision(node->aabb)) continue;
+
+        // Test if the ray intersects with the current node AABB
+        if (!node->aabb.testRayIntersect(rayTemp)) continue;
 
         // If the node is a leaf of the tree
         if (node->isLeaf()) {
 
-            Ray rayTemp(ray.point1, ray.point2, maxFraction);
-
             // Call the callback that will raycast again the broad-phase shape
             decimal hitFraction = callback.raycastBroadPhaseShape(nodeID, rayTemp);
 
@@ -683,9 +678,6 @@ void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &ca
                 if (hitFraction < maxFraction) {
                     maxFraction = hitFraction;
                 }
-                endPoint = ray.point1 + maxFraction * (ray.point2 - ray.point1);
-                rayAABB.mMinCoordinates = Vector3::min(ray.point1, endPoint);
-                rayAABB.mMaxCoordinates = Vector3::max(ray.point1, endPoint);
             }
 
             // If the user returned a negative fraction, we continue

src/collision/shapes/AABB.cpp

@@ -113,3 +113,38 @@ AABB AABB::createAABBForTriangle(const Vector3* trianglePoints) {
 
     return AABB(minCoords, maxCoords);
 }
+
+// Return true if the ray intersects the AABB
+/// This method use the line vs AABB raycasting technique described in
+/// Real-time Collision Detection by Christer Ericson.
+bool AABB::testRayIntersect(const Ray& ray) const {
+
+    const Vector3 point2 = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1);
+    const Vector3 e = mMaxCoordinates - mMinCoordinates;
+    const Vector3 d = point2 - ray.point1;
+    const Vector3 m = ray.point1 + point2 - mMinCoordinates - mMaxCoordinates;
+
+    // Test if the AABB face normals are separating axis
+    decimal adx = std::abs(d.x);
+    if (std::abs(m.x) > e.x + adx) return false;
+    decimal ady = std::abs(d.y);
+    if (std::abs(m.y) > e.y + ady) return false;
+    decimal adz = std::abs(d.z);
+    if (std::abs(m.z) > e.z + adz) return false;
+
+    // Add in an epsilon term to counteract arithmetic errors when segment is
+    // (near) parallel to a coordinate axis (see text for detail)
+    const decimal epsilon = 0.00001;
+    adx += epsilon;
+    ady += epsilon;
+    adz += epsilon;
+
+    // Test if the cross products between face normals and ray direction are
+    // separating axis
+    if (std::abs(m.y * d.z - m.z * d.y) > e.y * adz + e.z * ady) return false;
+    if (std::abs(m.z * d.x - m.x * d.z) > e.x * adz + e.z * adx) return false;
+    if (std::abs(m.x * d.y - m.y * d.x) > e.x * ady + e.y * adx) return false;
+
+    // No separating axis has been found
+    return true;
+}

src/collision/shapes/AABB.h

@@ -100,6 +100,9 @@ class AABB {
         /// Return true if the AABB of a triangle intersects the AABB
         bool testCollisionTriangleAABB(const Vector3* trianglePoints) const;
 
+        /// Return true if the ray intersects the AABB
+        bool testRayIntersect(const Ray& ray) const;
+
         /// Create and return an AABB for a triangle
         static AABB createAABBForTriangle(const Vector3* trianglePoints);
 

src/collision/shapes/BoxShape.cpp

@@ -117,7 +117,7 @@ bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pro
     }
 
     // If tMin is negative, we return no hit
-    if (tMin < decimal(0.0)) return false;
+    if (tMin < decimal(0.0) || tMin > ray.maxFraction) return false;
 
     // The ray intersects the three slabs, we compute the hit point
     Vector3 localHitPoint = ray.point1 + tMin * rayDirection;

src/collision/shapes/TriangleShape.cpp

@@ -109,6 +109,9 @@ bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
 
     // Compute the local hit point using the barycentric coordinates
     const Vector3 localHitPoint = u * mPoints[0] + v * mPoints[1] + w * mPoints[2];
+    const decimal hitFraction = (localHitPoint - ray.point1).length() / pq.length();
+
+    if (hitFraction < decimal(0.0) || hitFraction > ray.maxFraction) return false;
 
     Vector3 localHitNormal = (mPoints[1] - mPoints[0]).cross(mPoints[2] - mPoints[0]);
     if (localHitNormal.dot(pq) > decimal(0.0)) localHitNormal = -localHitNormal;
@@ -116,7 +119,7 @@ bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
     raycastInfo.body = proxyShape->getBody();
     raycastInfo.proxyShape = proxyShape;
     raycastInfo.worldPoint = localHitPoint;
-    raycastInfo.hitFraction = (localHitPoint - ray.point1).length() / pq.length();
+    raycastInfo.hitFraction = hitFraction;
     raycastInfo.worldNormal = localHitNormal;
 
     return true;