Implement raycasting with sphere shape

src/body/CollisionBody.cpp

@@ -213,13 +213,13 @@ bool CollisionBody::testPointInside(const Vector3& worldPoint) const {
 }
 
 // Raycast method
-bool CollisionBody::raycast(const Ray& ray, decimal distance) {
+bool CollisionBody::raycast(const Ray& ray) {
 
     // For each collision shape of the body
     for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
 
         // Test if the ray hits the collision shape
-        if (shape->raycast(ray, distance)) return true;
+        if (shape->raycast(ray)) return true;
     }
 
     return false;

src/body/CollisionBody.h

@@ -166,7 +166,7 @@ class CollisionBody : public Body {
         bool testPointInside(const Vector3& worldPoint) const;
 
         /// Raycast method
-        bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE);
+        bool raycast(const Ray& ray);
 
         /// Raycast method with feedback information
         bool raycast(const Ray& ray, RaycastInfo& raycastInfo,

src/collision/ProxyShape.h

@@ -100,7 +100,7 @@ class ProxyShape {
         bool testPointInside(const Vector3& worldPoint);
 
         /// Raycast method
-        bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE);
+        bool raycast(const Ray& ray);
 
         /// Raycast method with feedback information
         bool raycast(const Ray& ray, RaycastInfo& raycastInfo,
@@ -170,8 +170,8 @@ inline decimal ProxyShape::getMargin() const {
 }
 
 // Raycast method
-inline bool ProxyShape::raycast(const Ray& ray, decimal distance) {
+inline bool ProxyShape::raycast(const Ray& ray) {
-    return mCollisionShape->raycast(ray, this, distance);
+    return mCollisionShape->raycast(ray, this);
 }
 
 // Raycast method with feedback information

src/collision/shapes/BoxShape.cpp

@@ -62,7 +62,7 @@ void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const
 }
 
 // Raycast method
-bool BoxShape::raycast(const Ray& ray, ProxyShape* proxyShape, decimal distance) const {
+bool BoxShape::raycast(const Ray& ray, ProxyShape* proxyShape) const {
 
     // TODO : Normalize the ray direction
 

src/collision/shapes/BoxShape.h

@@ -79,8 +79,7 @@ class BoxShape : public CollisionShape {
         virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
 
         /// Raycast method
-        virtual bool raycast(const Ray& ray, ProxyShape* proxyShape,
+        virtual bool raycast(const Ray& ray, ProxyShape* proxyShape) const;
-                             decimal distance = RAYCAST_INFINITY_DISTANCE) const;
 
         /// Raycast method with feedback information
         virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,

src/collision/shapes/CapsuleShape.cpp

@@ -143,7 +143,7 @@ bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyS
 }
 
 // Raycast method
-bool CapsuleShape::raycast(const Ray& ray, ProxyShape* proxyShape, decimal distance) const {
+bool CapsuleShape::raycast(const Ray& ray, ProxyShape* proxyShape) const {
 
     // TODO : Normalize the ray direction
 

src/collision/shapes/CapsuleShape.h

@@ -76,8 +76,7 @@ class CapsuleShape : public CollisionShape {
         virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
 
         /// Raycast method
-        virtual bool raycast(const Ray& ray, ProxyShape* proxyShape,
+        virtual bool raycast(const Ray& ray, ProxyShape* proxyShape) const;
-                             decimal distance = RAYCAST_INFINITY_DISTANCE) const;
 
         /// Raycast method with feedback information
         virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,

src/collision/shapes/CollisionShape.h

@@ -86,8 +86,7 @@ class CollisionShape {
         virtual bool testPointInside(const Vector3& worldPoint, ProxyShape* proxyShape) const=0;
 
         /// Raycast method
-        virtual bool raycast(const Ray& ray, ProxyShape* proxyShape,
+        virtual bool raycast(const Ray& ray, ProxyShape* proxyShape) const=0;
-                             decimal distance = RAYCAST_INFINITY_DISTANCE) const=0;
 
         /// Raycast method with feedback information
         virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,

src/collision/shapes/ConeShape.cpp

@@ -95,7 +95,7 @@ Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
 }
 
 // Raycast method
-bool ConeShape::raycast(const Ray& ray, ProxyShape* proxyShape, decimal distance) const {
+bool ConeShape::raycast(const Ray& ray, ProxyShape* proxyShape) const {
 
     // TODO : Normalize the ray direction
 

src/collision/shapes/ConeShape.h

@@ -84,8 +84,7 @@ class ConeShape : public CollisionShape {
         virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
 
         /// Raycast method
-        virtual bool raycast(const Ray& ray, ProxyShape* proxyShape,
+        virtual bool raycast(const Ray& ray, ProxyShape* proxyShape) const;
-                             decimal distance = RAYCAST_INFINITY_DISTANCE) const;
 
         /// Raycast method with feedback information
         virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,

src/collision/shapes/ConvexMeshShape.cpp

@@ -231,10 +231,10 @@ bool ConvexMeshShape::isEqualTo(const CollisionShape& otherCollisionShape) const
 }
 
 // Raycast method
-bool ConvexMeshShape::raycast(const Ray& ray, ProxyShape* proxyShape, decimal distance) const {
+bool ConvexMeshShape::raycast(const Ray& ray, ProxyShape* proxyShape) const {
     RaycastInfo raycastInfo;
     return proxyShape->mBody->mWorld.mCollisionDetection.mNarrowPhaseGJKAlgorithm.raycast(
-                ray, proxyShape, raycastInfo, distance);
+                ray, proxyShape, raycastInfo, RAYCAST_INFINITY_DISTANCE);
 }
 
 // Raycast method with feedback information

src/collision/shapes/ConvexMeshShape.h

@@ -105,8 +105,7 @@ class ConvexMeshShape : public CollisionShape {
         virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
 
         /// Raycast method
-        virtual bool raycast(const Ray& ray, ProxyShape* proxyShape,
+        virtual bool raycast(const Ray& ray, ProxyShape* proxyShape) const;
-                             decimal distance = RAYCAST_INFINITY_DISTANCE) const;
 
         /// Raycast method with feedback information
         virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,

src/collision/shapes/CylinderShape.cpp

@@ -88,7 +88,7 @@ Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& directio
 }
 
 // Raycast method
-bool CylinderShape::raycast(const Ray& ray, ProxyShape* proxyShape, decimal distance) const {
+bool CylinderShape::raycast(const Ray& ray, ProxyShape* proxyShape) const {
 
     // TODO : Normalize the ray direction
 

src/collision/shapes/CylinderShape.h

@@ -81,8 +81,7 @@ class CylinderShape : public CollisionShape {
         virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
 
         /// Raycast method
-        virtual bool raycast(const Ray& ray, ProxyShape* proxyShape,
+        virtual bool raycast(const Ray& ray, ProxyShape* proxyShape) const;
-                             decimal distance = RAYCAST_INFINITY_DISTANCE) const;
 
         /// Raycast method with feedback information
         virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,

src/collision/shapes/SphereShape.cpp

@@ -25,6 +25,7 @@
 
 // Libraries
 #include "SphereShape.h"
+#include "collision/ProxyShape.h"
 #include "configuration.h"
 #include <cassert>
 
@@ -47,21 +48,74 @@ SphereShape::~SphereShape() {
 }
 
 // Raycast method
-bool SphereShape::raycast(const Ray& ray, ProxyShape* proxyShape, decimal distance) const {
+bool SphereShape::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;
+    decimal c = origin.dot(origin) - mRadius * mRadius;
 
+    // If the origin of the ray is inside the sphere, we return no intersection
+    if (c < decimal(0.0)) return false;
 
-    // TODO : Implement this method
+    Vector3 rayDirection = worldToLocalTransform.getOrientation() * ray.direction.getUnit();
-    return false;
+    decimal b = origin.dot(rayDirection);
+
+    // If the origin of the ray is outside the sphere and the ray
+    // is pointing away from the sphere and there is no intersection
+    if (c >= decimal(0.0) && b > decimal(0.0)) return false;
+
+    // Compute the discriminant of the quadratic equation
+    decimal discriminant = b*b - c;
+
+    // If the discriminant is negative, there is no intersection
+    if (discriminant < decimal(0.0)) return false;
+
+    // There is an intersection
+    return true;
 }
 
 // Raycast method with feedback information
 bool SphereShape::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;
+    decimal c = origin.dot(origin) - mRadius * mRadius;
 
-    // TODO : Implement this method
+    // If the origin of the ray is inside the sphere, we return no intersection
-    return false;
+    if (c < decimal(0.0)) return false;
+
+    Vector3 rayDirection = worldToLocalTransform.getOrientation() * ray.direction.getUnit();
+    decimal b = origin.dot(rayDirection);
+
+    // If the origin of the ray is outside the sphere and the ray
+    // is pointing away from the sphere and there is no intersection
+    if (c >= decimal(0.0) && b > decimal(0.0)) return false;
+
+    // Compute the discriminant of the quadratic equation
+    decimal discriminant = b*b - c;
+
+    // If the discriminant is negative, there is no intersection
+    if (discriminant < decimal(0.0)) return false;
+
+    // Compute the solution "t" closest to the origin
+    decimal t = -b - std::sqrt(discriminant);
+
+    assert(t >= decimal(0.0));
+
+    // If the intersection distance is larger than the allowed distance, return no intersection
+    if (t > distance) return false;
+
+    // Compute the intersection information
+    Vector3 localPoint = origin + t * rayDirection;
+    raycastInfo.body = proxyShape->getBody();
+    raycastInfo.proxyShape = proxyShape;
+    raycastInfo.distance = t;
+    raycastInfo.worldPoint = localToWorldTransform * localPoint;
+    raycastInfo.worldNormal = (raycastInfo.worldPoint -
+                               localToWorldTransform.getPosition()).getUnit();
+
+    return true;
 }

src/collision/shapes/SphereShape.h

@@ -71,8 +71,7 @@ class SphereShape : public CollisionShape {
         virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
 
         /// Raycast method
-        virtual bool raycast(const Ray& ray, ProxyShape* proxyShape,
+        virtual bool raycast(const Ray& ray, ProxyShape* proxyShape) const;
-                             decimal distance = RAYCAST_INFINITY_DISTANCE) const;
 
         /// Raycast method with feedback information
         virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,

test/tests/collision/TestRaycast.h

@@ -361,30 +361,30 @@ class TestRaycast : public Test {
             test(mWorld->raycast(ray, raycastInfo));
             test(raycastInfo.body == mSphereBody);
             test(raycastInfo.proxyShape == mSphereShape);
-            test(approxEqual(raycastInfo.distance, 6));
+            test(approxEqual(raycastInfo.distance, 5, epsilon));
-            test(approxEqual(raycastInfo.worldPoint.x, hitPoint.x));
+            test(approxEqual(raycastInfo.worldPoint.x, hitPoint.x, epsilon));
-            test(approxEqual(raycastInfo.worldPoint.y, hitPoint.y));
+            test(approxEqual(raycastInfo.worldPoint.y, hitPoint.y, epsilon));
-            test(approxEqual(raycastInfo.worldPoint.z, hitPoint.z));
+            test(approxEqual(raycastInfo.worldPoint.z, hitPoint.z, epsilon));
 
             // CollisionBody::raycast()
             RaycastInfo raycastInfo2;
             test(mSphereBody->raycast(ray, raycastInfo2));
             test(raycastInfo2.body == mSphereBody);
             test(raycastInfo2.proxyShape == mSphereShape);
-            test(approxEqual(raycastInfo2.distance, 6));
+            test(approxEqual(raycastInfo2.distance, 5, epsilon));
-            test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x));
+            test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon));
-            test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y));
+            test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon));
-            test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z));
+            test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon));
 
             // ProxyCollisionShape::raycast()
             RaycastInfo raycastInfo3;
             test(mSphereShape->raycast(ray, raycastInfo3));
             test(raycastInfo3.body == mSphereBody);
             test(raycastInfo3.proxyShape == mSphereShape);
-            test(approxEqual(raycastInfo3.distance, 6));
+            test(approxEqual(raycastInfo3.distance, 5, epsilon));
-            test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x));
+            test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon));
-            test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y));
+            test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon));
-            test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z));
+            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));