Implement raycasting for capsule shape

src/collision/shapes/CapsuleShape.cpp

@@ -25,6 +25,7 @@
 
 // Libraries
 #include "CapsuleShape.h"
+#include "collision/ProxyShape.h"
 #include "configuration.h"
 #include <cassert>
 
@@ -145,18 +146,279 @@ bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyS
 // Raycast method
 bool CapsuleShape::raycast(const Ray& ray, ProxyShape* proxyShape) const {
 
-    // TODO : Normalize the ray direction
+    // Transform the ray direction and origin in local-space coordinates
+    const Transform localToWorldTransform = proxyShape->getLocalToWorldTransform();
+    const Transform worldToLocalTransform = localToWorldTransform.getInverse();
+    Vector3 origin = worldToLocalTransform * ray.origin;
+    Vector3 n = worldToLocalTransform.getOrientation() * ray.direction.getUnit();
 
-    // TODO : Implement this method
+    const decimal epsilon = decimal(0.00001);
+    Vector3 p(decimal(0), -mHalfHeight, decimal(0));
+    Vector3 q(decimal(0), mHalfHeight, decimal(0));
+    Vector3 d = q - p;
+    Vector3 m = origin - p;
+    decimal t;
+
+    decimal mDotD = m.dot(d);
+    decimal nDotD = n.dot(d);
+    decimal dDotD = d.dot(d);
+    decimal mDotN = m.dot(n);
+
+    decimal a = dDotD - nDotD * nDotD;
+    decimal k = m.dot(m) - mRadius * mRadius;
+    decimal c = dDotD * k - mDotD * mDotD;
+
+    // If the ray is parallel to the cylinder axis
+    if (std::abs(a) < epsilon) {
+
+        // If the origin is outside the surface of the cylinder, we return no hit
+        if (c > decimal(0.0)) return false;
+
+        // Here we know that the segment intersect an endcap of the cylinder
+
+        // If the ray intersects with the "p" endcap of the capsule
+        if (mDotD < decimal(0.0)) {
+
+            // Check intersection with the sphere "p" endcap of the capsule
+            return raycastWithSphereEndCap(origin, n, p);
+        }
+        else if (mDotD > dDotD) {   // If the ray intersects with the "q" endcap of the cylinder
+
+            // Check intersection with the sphere "q" endcap of the capsule
+            return raycastWithSphereEndCap(origin, n, q);
+        }
+        else {  // If the origin is inside the cylinder, we return no hit
             return false;
+        }
+    }
+    decimal b = dDotD * mDotN - nDotD * mDotD;
+    decimal discriminant = b * b - a * c;
+
+    // If the discriminant is negative, no real roots and therfore, no hit
+    if (discriminant < decimal(0.0)) return false;
+
+    // Compute the smallest root (first intersection along the ray)
+    decimal t0 = t = (-b - std::sqrt(discriminant)) / a;
+
+    // If the intersection is outside the cylinder on "p" endcap side
+    decimal value = mDotD + t * nDotD;
+    if (value < decimal(0.0)) {
+
+        // Check intersection with the sphere "p" endcap of the capsule
+        return raycastWithSphereEndCap(origin, n, p);
+    }
+    else if (value > dDotD) {   // If the intersection is outside the cylinder on the "q" side
+
+        // Check intersection with the sphere "q" endcap of the capsule
+        return raycastWithSphereEndCap(origin, n, q);
+    }
+
+    // If the intersection is behind the origin of the ray, we return no hit
+    return (t0 >= decimal(0.0));
 }
 
 // Raycast method with feedback information
 bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,
                            decimal distance) const {
 
-    // TODO : Normalize the ray direction
+    // Transform the ray direction and origin in local-space coordinates
+    const Transform localToWorldTransform = proxyShape->getLocalToWorldTransform();
+    const Transform worldToLocalTransform = localToWorldTransform.getInverse();
+    Vector3 origin = worldToLocalTransform * ray.origin;
+    Vector3 n = worldToLocalTransform.getOrientation() * ray.direction.getUnit();
+
+    const decimal epsilon = decimal(0.00001);
+    Vector3 p(decimal(0), -mHalfHeight, decimal(0));
+    Vector3 q(decimal(0), mHalfHeight, decimal(0));
+    Vector3 d = q - p;
+    Vector3 m = origin - p;
+    decimal t;
+
+    decimal mDotD = m.dot(d);
+    decimal nDotD = n.dot(d);
+    decimal dDotD = d.dot(d);
+    decimal mDotN = m.dot(n);
+
+    decimal a = dDotD - nDotD * nDotD;
+    decimal k = m.dot(m) - mRadius * mRadius;
+    decimal c = dDotD * k - mDotD * mDotD;
+
+    // If the ray is parallel to the capsule axis
+    if (std::abs(a) < epsilon) {
+
+        // If the origin is outside the surface of the capusle's cylinder, we return no hit
+        if (c > decimal(0.0)) return false;
+
+        // Here we know that the segment intersect an endcap of the capsule
+
+        // If the ray intersects with the "p" endcap of the capsule
+        if (mDotD < decimal(0.0)) {
+
+            // Check intersection between the ray and the "p" sphere endcap of the capsule
+            Vector3 hitLocalPoint;
+            decimal hitDistance;
+            if (raycastWithSphereEndCap(origin, n, p, distance, hitLocalPoint, hitDistance)) {
+                raycastInfo.body = proxyShape->getBody();
+                raycastInfo.proxyShape = proxyShape;
+                raycastInfo.distance = hitDistance;
+                raycastInfo.worldPoint = localToWorldTransform * hitLocalPoint;
+                Vector3 normalDirection = (hitLocalPoint - p).getUnit();
+                raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
+
+                return true;
+            }
 
-    // TODO : Implement this method
             return false;
+        }
+        else if (mDotD > dDotD) {   // If the ray intersects with the "q" endcap of the cylinder
+
+            // Check intersection between the ray and the "q" sphere endcap of the capsule
+            Vector3 hitLocalPoint;
+            decimal hitDistance;
+            if (raycastWithSphereEndCap(origin, n, q, distance, hitLocalPoint, hitDistance)) {
+                raycastInfo.body = proxyShape->getBody();
+                raycastInfo.proxyShape = proxyShape;
+                raycastInfo.distance = hitDistance;
+                raycastInfo.worldPoint = localToWorldTransform * hitLocalPoint;
+                Vector3 normalDirection = (hitLocalPoint - q).getUnit();
+                raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
+
+                return true;
+            }
+
+            return false;
+        }
+        else {  // If the origin is inside the cylinder, we return no hit
+            return false;
+        }
+    }
+    decimal b = dDotD * mDotN - nDotD * mDotD;
+    decimal discriminant = b * b - a * c;
+
+    // If the discriminant is negative, no real roots and therfore, no hit
+    if (discriminant < decimal(0.0)) return false;
+
+    // Compute the smallest root (first intersection along the ray)
+    decimal t0 = t = (-b - std::sqrt(discriminant)) / a;
+
+    // If the intersection is outside the finite cylinder of the capsule on "p" endcap side
+    decimal value = mDotD + t * nDotD;
+    if (value < decimal(0.0)) {
+
+        // Check intersection between the ray and the "p" sphere endcap of the capsule
+        Vector3 hitLocalPoint;
+        decimal hitDistance;
+        if (raycastWithSphereEndCap(origin, n, p, distance, hitLocalPoint, hitDistance)) {
+            raycastInfo.body = proxyShape->getBody();
+            raycastInfo.proxyShape = proxyShape;
+            raycastInfo.distance = hitDistance;
+            raycastInfo.worldPoint = localToWorldTransform * hitLocalPoint;
+            Vector3 normalDirection = (hitLocalPoint - p).getUnit();
+            raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
+
+            return true;
+        }
+
+        return false;
+    }
+    else if (value > dDotD) {  // If the intersection is outside the finite cylinder on the "q" side
+
+        // Check intersection between the ray and the "q" sphere endcap of the capsule
+        Vector3 hitLocalPoint;
+        decimal hitDistance;
+        if (raycastWithSphereEndCap(origin, n, q, distance, hitLocalPoint, hitDistance)) {
+            raycastInfo.body = proxyShape->getBody();
+            raycastInfo.proxyShape = proxyShape;
+            raycastInfo.distance = hitDistance;
+            raycastInfo.worldPoint = localToWorldTransform * hitLocalPoint;
+            Vector3 normalDirection = (hitLocalPoint - q).getUnit();
+            raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
+
+            return true;
+        }
+
+        return false;
+    }
+
+    t = t0;
+
+    // If the intersection is behind the origin of the ray or beyond the maximum
+    // raycasting distance, we return no hit
+    if (t < decimal(0.0) || t > distance) return false;
+
+    // Compute the hit information
+    Vector3 localHitPoint = origin + t * n;
+    raycastInfo.body = proxyShape->getBody();
+    raycastInfo.proxyShape = proxyShape;
+    raycastInfo.distance = t;
+    raycastInfo.worldPoint = localToWorldTransform * localHitPoint;
+    Vector3 v = localHitPoint - p;
+    Vector3 w = (v.dot(d) / d.lengthSquare()) * d;
+    Vector3 normalDirection = (localHitPoint - (p + w)).getUnit();
+    raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
+
+    return true;
+}
+
+// Raycasting method between a ray one of the two spheres end cap of the capsule
+bool CapsuleShape::raycastWithSphereEndCap(const Vector3& rayOrigin, const Vector3& rayDirection,
+                                           const Vector3& sphereCenter, decimal maxDistance,
+                                           Vector3& hitLocalPoint, decimal& hitDistance) const {
+
+    Vector3 m = rayOrigin - sphereCenter;
+    decimal c = m.dot(m) - mRadius * mRadius;
+
+    // If the origin of the ray is inside the sphere, we return no intersection
+    if (c < decimal(0.0)) return false;
+
+    decimal b = m.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 > maxDistance) return false;
+
+    // Compute the hit point and distance
+    hitLocalPoint = rayOrigin + t * rayDirection;
+    hitDistance = t;
+
+    return true;
+}
+
+// Raycasting method between a ray one of the two spheres end cap of the capsule
+/// This method returns true if there is an intersection and false otherwise but does not
+/// compute the intersection point.
+bool CapsuleShape::raycastWithSphereEndCap(const Vector3& rayOrigin, const Vector3& rayDirection,
+                                           const Vector3& sphereCenter) const {
+
+    Vector3 m = rayOrigin - sphereCenter;
+    decimal c = m.dot(m) - mRadius * mRadius;
+
+    // If the origin of the ray is inside the sphere, we return no intersection
+    if (c < decimal(0.0)) return false;
+
+    decimal b = m.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
+    return (discriminant >= decimal(0.0));
 }

src/collision/shapes/CapsuleShape.h

@@ -82,6 +82,15 @@ class CapsuleShape : public CollisionShape {
         virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,
                              decimal distance = RAYCAST_INFINITY_DISTANCE) const;
 
+        /// Raycasting method between a ray one of the two spheres end cap of the capsule
+        bool raycastWithSphereEndCap(const Vector3& rayOrigin, const Vector3& rayDirection,
+                                     const Vector3& sphereCenter, decimal maxDistance,
+                                     Vector3& hitLocalPoint, decimal& hitDistance) const;
+
+        // Raycasting method between a ray one of the two spheres end cap of the capsule
+        bool raycastWithSphereEndCap(const Vector3& rayOrigin, const Vector3& rayDirection,
+                                     const Vector3& sphereCenter) const;
+
     public :
 
         // -------------------- Methods -------------------- //

test/tests/collision/TestRaycast.h

@@ -506,41 +506,70 @@ class TestRaycast : public Test {
         void testCapsule() {
 
             // ----- Test feedback data ----- //
-            Vector3 origin = mLocalShapeToWorld * Vector3(0 , 10, 0);
+            Vector3 origin = mLocalShapeToWorld * Vector3(6 , 1, 0);
             const Matrix3x3 mLocalToWorldMatrix = mLocalShapeToWorld.getOrientation().getMatrix();
-            Vector3 direction = mLocalToWorldMatrix * Vector3(0, -3, 0);
+            Vector3 direction = mLocalToWorldMatrix * Vector3(-2, 0, 0);
             Ray ray(origin, direction);
-            Vector3 hitPoint = mLocalShapeToWorld * Vector3(0, 7, 0);
+            Vector3 hitPoint = mLocalShapeToWorld * Vector3(2, 1, 0);
+
+            Vector3 origin2 = mLocalShapeToWorld * Vector3(0 , 10, 0);
+            Vector3 direction2 = mLocalToWorldMatrix * Vector3(0, -3, 0);
+            Ray rayTop(origin2, direction2);
+            Vector3 hitPointTop = mLocalShapeToWorld * Vector3(0, decimal(4.5), 0);
+
+            Vector3 origin3 = mLocalShapeToWorld * Vector3(0 , -10, 0);
+            Vector3 direction3 = mLocalToWorldMatrix * Vector3(0, 3, 0);
+            Ray rayBottom(origin3, direction3);
+            Vector3 hitPointBottom = mLocalShapeToWorld * Vector3(0, decimal(-4.5), 0);
 
             // CollisionWorld::raycast()
             RaycastInfo raycastInfo;
             test(mWorld->raycast(ray, raycastInfo));
             test(raycastInfo.body == mCapsuleBody);
             test(raycastInfo.proxyShape == mCapsuleShape);
-            test(approxEqual(raycastInfo.distance, 6));
+            test(approxEqual(raycastInfo.distance, 4, 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(mCapsuleBody->raycast(ray, raycastInfo2));
             test(raycastInfo2.body == mCapsuleBody);
             test(raycastInfo2.proxyShape == mCapsuleShape);
-            test(approxEqual(raycastInfo2.distance, 6));
+            test(approxEqual(raycastInfo2.distance, 4, 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(mCapsuleShape->raycast(ray, raycastInfo3));
             test(raycastInfo3.body == mCapsuleBody);
             test(raycastInfo3.proxyShape == mCapsuleShape);
-            test(approxEqual(raycastInfo3.distance, 6));
+            test(approxEqual(raycastInfo3.distance, 4, 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));
+
+            RaycastInfo raycastInfo4;
+            test(mCapsuleShape->raycast(rayTop, raycastInfo4));
+            test(raycastInfo4.body == mCapsuleBody);
+            test(raycastInfo4.proxyShape == mCapsuleShape);
+            test(approxEqual(raycastInfo4.distance, decimal(5.5), epsilon));
+            test(approxEqual(raycastInfo4.worldPoint.x, hitPointTop.x, epsilon));
+            test(approxEqual(raycastInfo4.worldPoint.y, hitPointTop.y, epsilon));
+            test(approxEqual(raycastInfo4.worldPoint.z, hitPointTop.z, epsilon));
+
+            // ProxyCollisionShape::raycast()
+            RaycastInfo raycastInfo5;
+            test(mCapsuleShape->raycast(rayBottom, raycastInfo5));
+            test(raycastInfo5.body == mCapsuleBody);
+            test(raycastInfo5.proxyShape == mCapsuleShape);
+            test(approxEqual(raycastInfo5.distance, decimal(5.5), epsilon));
+            test(approxEqual(raycastInfo5.worldPoint.x, hitPointBottom.x, epsilon));
+            test(approxEqual(raycastInfo5.worldPoint.y, hitPointBottom.y, epsilon));
+            test(approxEqual(raycastInfo5.worldPoint.z, hitPointBottom.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));
@@ -552,7 +581,7 @@ class TestRaycast : public Test {
             Ray ray8(mLocalShapeToWorld * Vector3(-4, 9, 0), mLocalToWorldMatrix * Vector3(1, 0, 0));
             Ray ray9(mLocalShapeToWorld * Vector3(0, -9, -4), mLocalToWorldMatrix * Vector3(0, 5, 0));
             Ray ray10(mLocalShapeToWorld * Vector3(-4, 0, -6), mLocalToWorldMatrix * Vector3(0, 0, 8));
-            Ray ray11(mLocalShapeToWorld * Vector3(4, 1, 2), mLocalToWorldMatrix * Vector3(-4, 0, 0));
+            Ray ray11(mLocalShapeToWorld * Vector3(4, 1, 1.5), mLocalToWorldMatrix * Vector3(-4, 0, 0));
             Ray ray12(mLocalShapeToWorld * Vector3(1, 9, -1), mLocalToWorldMatrix * Vector3(0, -3, 0));
             Ray ray13(mLocalShapeToWorld * Vector3(-1, 2, 3), mLocalToWorldMatrix * Vector3(0, 0, -8));
             Ray ray14(mLocalShapeToWorld * Vector3(-3, 2, -2), mLocalToWorldMatrix * Vector3(4, 0, 0));