Implement raycasting for the BoxShape

src/collision/shapes/BoxShape.cpp

@@ -25,6 +25,7 @@
 
 // Libraries
 #include "BoxShape.h"
+#include "collision/ProxyShape.h"
 #include "configuration.h"
 #include <vector>
 #include <cassert>
@@ -64,18 +65,112 @@ void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const
 // Raycast method
 bool BoxShape::raycast(const Ray& ray, ProxyShape* proxyShape) const {
 
-    // TODO : Normalize the ray direction
+    const Transform localToWorldTransform = proxyShape->getLocalToWorldTransform();
+    const Transform worldToLocalTransform = localToWorldTransform.getInverse();
+    Vector3 origin = worldToLocalTransform * ray.origin;
+    Vector3 rayDirection = worldToLocalTransform.getOrientation() * ray.direction.getUnit();
+    decimal tMin = decimal(0.0);
+    decimal tMax = DECIMAL_LARGEST;
 
-    // TODO : Implement this method
-    return false;
+    // For each of the three slabs
+    for (int i=0; i<3; i++) {
+
+        // If ray is parallel to the slab
+        if (std::abs(rayDirection[i]) < MACHINE_EPSILON) {
+
+            // If the ray's origin is not inside the slab, there is no hit
+            if (origin[i] > mExtent[i] || origin[i] < -mExtent[i]) return false;
+        }
+        else {
+
+            // Compute the intersection of the ray with the near and far plane of the slab
+            decimal oneOverD = decimal(1.0) / rayDirection[i];
+            decimal t1 = (-mExtent[i] - origin[i]) * oneOverD;
+            decimal t2 = (mExtent[i] - origin [i]) * oneOverD;
+
+            // Swap t1 and t2 if need so that t1 is intersection with near plane and
+            // t2 with far plane
+            if (t1 > t2) swap(t1, t2);
+
+            // If t1 is negative, the origin is inside the box and therefore, there is no hit
+            if (t1 < decimal(0.0)) return false;
+
+            // Compute the intersection of the of slab intersection interval with previous slabs
+            tMin = std::max(tMin, t1);
+            tMax = std::min(tMax, t2);
+
+            // If the slabs intersection is empty, there is no hit
+            if (tMin > tMax) return false;
+        }
+    }
+
+    // A hit has been found
+    return true;
 }
 
 // Raycast method with feedback information
 bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,
                        decimal distance) const {
 
-    // TODO : Normalize the ray direction
+    const Transform localToWorldTransform = proxyShape->getLocalToWorldTransform();
+    const Transform worldToLocalTransform = localToWorldTransform.getInverse();
+    Vector3 origin = worldToLocalTransform * ray.origin;
+    Vector3 rayDirection = worldToLocalTransform.getOrientation() * ray.direction.getUnit();
+    decimal tMin = decimal(0.0);
+    decimal tMax = DECIMAL_LARGEST;
+    Vector3 normalDirection(decimal(0), decimal(0), decimal(0));
+    Vector3 currentNormal;
 
-    // TODO : Implement this method
-    return false;
+    // For each of the three slabs
+    for (int i=0; i<3; i++) {
+
+        // If ray is parallel to the slab
+        if (std::abs(rayDirection[i]) < MACHINE_EPSILON) {
+
+            // If the ray's origin is not inside the slab, there is no hit
+            if (origin[i] > mExtent[i] || origin[i] < -mExtent[i]) return false;
+        }
+        else {
+
+            // Compute the intersection of the ray with the near and far plane of the slab
+            decimal oneOverD = decimal(1.0) / rayDirection[i];
+            decimal t1 = (-mExtent[i] - origin[i]) * oneOverD;
+            decimal t2 = (mExtent[i] - origin [i]) * oneOverD;
+            currentNormal = -mExtent;
+
+            // Swap t1 and t2 if need so that t1 is intersection with near plane and
+            // t2 with far plane
+            if (t1 > t2) {
+                swap(t1, t2);
+                currentNormal = -currentNormal;
+            }
+
+            // If t1 is negative, the origin is inside the box and therefore, there is no hit
+            if (t1 < decimal(0.0)) return false;
+
+            // Compute the intersection of the of slab intersection interval with previous slabs
+            if (t1 > tMin) {
+                tMin = t1;
+                normalDirection = currentNormal;
+            }
+            tMax = std::min(tMax, t2);
+
+            // If tMin is larger than the maximum raycasting distance, we return no hit
+            if (tMin > distance) return false;
+
+            // If the slabs intersection is empty, there is no hit
+            if (tMin > tMax) return false;
+        }
+    }
+
+    // The ray intersects the three slabs, we compute the hit point
+    Vector3 localHitPoint = origin + tMin * rayDirection;
+
+    raycastInfo.body = proxyShape->getBody();
+    raycastInfo.proxyShape = proxyShape;
+    raycastInfo.distance = tMin;
+    raycastInfo.worldPoint = localToWorldTransform * localHitPoint;
+    raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
+
+    return true;
 }

src/mathematics/mathematics_functions.h

@@ -51,6 +51,13 @@ inline decimal clamp(decimal value, decimal lowerLimit, decimal upperLimit) {
     return std::min(std::max(value, lowerLimit), upperLimit);
 }
 
+/// Function that swaps two values
+inline void swap(decimal& a, decimal& b) {
+    decimal temp = a;
+    a = b;
+    b = temp;
+}
+
 }
 
 

test/tests/collision/TestRaycast.h

@@ -205,30 +205,30 @@ class TestRaycast : public Test {
             test(mWorld->raycast(ray, raycastInfo));
             test(raycastInfo.body == mBoxBody);
             test(raycastInfo.proxyShape == mBoxShape);
-            test(approxEqual(raycastInfo.distance, 6));
-            test(approxEqual(raycastInfo.worldPoint.x, hitPoint.x));
-            test(approxEqual(raycastInfo.worldPoint.y, hitPoint.y));
-            test(approxEqual(raycastInfo.worldPoint.z, hitPoint.z));
+            test(approxEqual(raycastInfo.distance, 6, epsilon));
+            test(approxEqual(raycastInfo.worldPoint.x, hitPoint.x, epsilon));
+            test(approxEqual(raycastInfo.worldPoint.y, hitPoint.y, epsilon));
+            test(approxEqual(raycastInfo.worldPoint.z, hitPoint.z, epsilon));
 
             // CollisionBody::raycast()
             RaycastInfo raycastInfo2;
             test(mBoxBody->raycast(ray, raycastInfo2));
             test(raycastInfo2.body == mBoxBody);
             test(raycastInfo2.proxyShape == mBoxShape);
-            test(approxEqual(raycastInfo2.distance, 6));
-            test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x));
-            test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y));
-            test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z));
+            test(approxEqual(raycastInfo2.distance, 6, epsilon));
+            test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon));
+            test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon));
+            test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon));
 
             // ProxyCollisionShape::raycast()
             RaycastInfo raycastInfo3;
             test(mBoxShape->raycast(ray, raycastInfo3));
             test(raycastInfo3.body == mBoxBody);
             test(raycastInfo3.proxyShape == mBoxShape);
-            test(approxEqual(raycastInfo3.distance, 6));
-            test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x));
-            test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y));
-            test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z));
+            test(approxEqual(raycastInfo3.distance, 6, epsilon));
+            test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon));
+            test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon));
+            test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon));
 
             Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalToWorldMatrix * Vector3(5, 7, -1));
             Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalToWorldMatrix * Vector3(4, 6, 7));