Raycast radius

include/reactphysics3d/collision/shapes/AABB.h

@@ -108,7 +108,7 @@ class AABB {
         bool testCollisionTriangleAABB(const Vector3* trianglePoints) const;
 
         /// Return true if the ray intersects the AABB
-        bool testRayIntersect(const Vector3& rayOrigin, const Vector3& rayDirectionInv, decimal rayMaxFraction) const;
+        bool testRayIntersect(const Vector3& rayOrigin, const Vector3& rayDirectionInv, decimal rayRadius, decimal rayMaxFraction) const;
 
         /// Compute the intersection of a ray and the AABB
         bool raycast(const Ray& ray, Vector3& hitPoint) const;
@@ -282,7 +282,7 @@ RP3D_FORCE_INLINE AABB AABB::createAABBForTriangle(const Vector3* trianglePoints
 }
 
 // Return true if the ray intersects the AABB
-RP3D_FORCE_INLINE bool AABB::testRayIntersect(const Vector3& rayOrigin, const Vector3& rayDirectionInverse, decimal rayMaxFraction) const {
+RP3D_FORCE_INLINE bool AABB::testRayIntersect(const Vector3& rayOrigin, const Vector3& rayDirectionInverse, decimal rayRadius, decimal rayMaxFraction) const {
 
     // This algorithm relies on the IEE floating point properties (division by zero). If the rayDirection is zero, rayDirectionInverse and
     // therfore t1 and t2 will be +-INFINITY. If the i coordinate of the ray's origin is inside the AABB (mMinCoordinates[i] < rayOrigin[i] < mMaxCordinates[i)), we have
@@ -295,17 +295,21 @@ RP3D_FORCE_INLINE bool AABB::testRayIntersect(const Vector3& rayOrigin, const Ve
     // the BVH tree. Because this should be rare, it is not really a big issue.
     // Reference: https://tavianator.com/2011/ray_box.html
 
-    decimal t1 = (mMinCoordinates[0] - rayOrigin[0]) * rayDirectionInverse[0];
-    decimal t2 = (mMaxCoordinates[0] - rayOrigin[0]) * rayDirectionInverse[0];
+    // Adjust min and max to account for ray radius
+    Vector3 extendedMinCoordinates = mMinCoordinates - Vector3(rayRadius, rayRadius, rayRadius);
+    Vector3 extendedMaxCoordinates = mMaxCoordinates + Vector3(rayRadius, rayRadius, rayRadius);
+
+    decimal t1 = (extendedMinCoordinates[0] - rayOrigin[0]) * rayDirectionInverse[0];
+    decimal t2 = (extendedMaxCoordinates[0] - rayOrigin[0]) * rayDirectionInverse[0];
 
     decimal tMin = std::min(t1, t2);
     decimal tMax = std::max(t1, t2);
     tMax = std::min(tMax, rayMaxFraction);
 
     for (int i = 1; i < 3; i++) {
 
-        t1 = (mMinCoordinates[i] - rayOrigin[i]) * rayDirectionInverse[i];
-        t2 = (mMaxCoordinates[i] - rayOrigin[i]) * rayDirectionInverse[i];
+        t1 = (extendedMinCoordinates[i] - rayOrigin[i]) * rayDirectionInverse[i];
+        t2 = (extendedMaxCoordinates[i] - rayOrigin[i]) * rayDirectionInverse[i];
 
         tMin = std::max(tMin, std::min(t1, t2));
         tMax = std::min(tMax, std::max(t1, t2));
@@ -324,20 +328,24 @@ RP3D_FORCE_INLINE bool AABB::raycast(const Ray& ray, Vector3& hitPoint) const {
 
     const Vector3 rayDirection = ray.point2 - ray.point1;
 
+    // Adjust min and max to account for ray radius
+    Vector3 extendedMinCoordinates = mMinCoordinates - Vector3(ray.radius, ray.radius, ray.radius);
+    Vector3 extendedMaxCoordinates = mMaxCoordinates + Vector3(ray.radius, ray.radius, ray.radius);
+
     // For all three slabs
     for (int i=0; i < 3; i++) {
 
         // If the ray is parallel to the slab
         if (std::abs(rayDirection[i]) < epsilon) {
 
             // If origin of the ray is not inside the slab, no hit
-            if (ray.point1[i] < mMinCoordinates[i] || ray.point1[i] > mMaxCoordinates[i]) return false;
+            if (ray.point1[i] < extendedMinCoordinates[i] || ray.point1[i] > extendedMaxCoordinates[i]) return false;
         }
         else {
 
             decimal rayDirectionInverse = decimal(1.0) / rayDirection[i];
-            decimal t1 = (mMinCoordinates[i] - ray.point1[i]) * rayDirectionInverse;
-            decimal t2 = (mMaxCoordinates[i] - ray.point1[i]) * rayDirectionInverse;
+            decimal t1 = (extendedMinCoordinates[i] - ray.point1[i]) * rayDirectionInverse;
+            decimal t2 = (extendedMaxCoordinates[i] - ray.point1[i]) * rayDirectionInverse;
 
             if (t1 > t2) {
 
@@ -358,6 +366,36 @@ RP3D_FORCE_INLINE bool AABB::raycast(const Ray& ray, Vector3& hitPoint) const {
     // Compute the hit point
     hitPoint = ray.point1 + tMin * rayDirection;
 
+    // Adjust hit point for sweep radius
+    if (ray.radius > decimal(0.0))
+    {
+        // Clamp point to box
+        Vector3 contactPointOnBox;
+        contactPointOnBox.x = std::clamp(hitPoint.x, extendedMinCoordinates.x, extendedMaxCoordinates.x);
+        contactPointOnBox.y = std::clamp(hitPoint.y, extendedMinCoordinates.y, extendedMaxCoordinates.y);
+        contactPointOnBox.z = std::clamp(hitPoint.z, extendedMinCoordinates.z, extendedMaxCoordinates.z);
+
+        // Solve hit point for box
+        Vector3 oc = ray.point1 - contactPointOnBox;
+        decimal a = rayDirection.lengthSquare(); // a = D.D
+        decimal b = 2.0 * rayDirection.dot(oc);  // b = 2 * (D . OC)
+        decimal c = oc.lengthSquare() - ray.radius * ray.radius; // c = OC.OC - r^2
+
+        // If origin is inside sphere (c < 0)
+        if (c < 0.0) {
+            if (b >= 0.0) return false; // Already overlapping and moving away
+            tMin = 0.0;
+        } else {
+            decimal discriminant = b * b - 4.0 * a * c;
+            if (discriminant < 0.0) return false; // No real solution, so no hit
+            decimal t = (-b - std::sqrt(discriminant)) / (2.0 * a);
+            if (t < 0.0 || t > ray.maxFraction) return false; // Hit is behind or too far
+            tMin = t;
+        }
+
+        hitPoint = ray.point1 + tMin * rayDirection;
+    }
+
     return true;
 }
 

include/reactphysics3d/collision/shapes/CapsuleShape.h

@@ -71,7 +71,7 @@ class CapsuleShape : public ConvexShape {
 
         /// Raycasting method between a ray one of the two spheres end cap of the capsule
         bool raycastWithSphereEndCap(const Vector3& point1, const Vector3& point2,
-                                     const Vector3& sphereCenter, decimal maxFraction,
+                                     const Vector3& sphereCenter, decimal rayRadius, decimal maxFraction,
                                      Vector3& hitLocalPoint, decimal& hitFraction) const;
 
         /// Return the number of bytes used by the collision shape

include/reactphysics3d/mathematics/Ray.h

@@ -34,9 +34,10 @@ namespace reactphysics3d {
 
 // Class Ray
 /**
- * This structure represents a 3D ray represented by two points.
+ * This structure represents a 3D ray represented by two points and a radius.
  * The ray goes from point1 to point1 + maxFraction * (point2 - point1).
- * The points are specified in world-space coordinates.
+ * The points and radius are specified in world-space coordinates.
+ * If the radius is greater than zero, the ray will work as a sphere-sweep. 
  */
 struct Ray {
 
@@ -50,14 +51,17 @@ struct Ray {
         /// Second point of the ray in world-space
         Vector3 point2;
 
+        /// Radius of the ray in world-space
+        decimal radius;
+
         /// Maximum fraction value
         decimal maxFraction;
 
         // -------------------- Methods -------------------- //
 
         /// Constructor with arguments
-        Ray(const Vector3& p1, const Vector3& p2, decimal maxFrac = decimal(1.0))
-           : point1(p1), point2(p2), maxFraction(maxFrac) {
+        Ray(const Vector3& p1, const Vector3& p2, decimal radius = decimal(0.0), decimal maxFrac = decimal(1.0))
+           : point1(p1), point2(p2), radius(radius), maxFraction(maxFrac) {
 
         }
 };

src/collision/Collider.cpp

@@ -178,7 +178,7 @@ bool Collider::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
     // Convert the ray into the local-space of the collision shape
     const Transform& localToWorldTransform = mBody->mWorld.mCollidersComponents.getLocalToWorldTransform(mEntity);
     const Transform worldToLocalTransform = localToWorldTransform.getInverse();
-    Ray rayLocal(worldToLocalTransform * ray.point1, worldToLocalTransform * ray.point2, ray.maxFraction);
+    Ray rayLocal(worldToLocalTransform * ray.point1, worldToLocalTransform * ray.point2, ray.radius, ray.maxFraction);
 
     const CollisionShape* collisionShape = mBody->mWorld.mCollidersComponents.getCollisionShape(mEntity);
     bool isHit = collisionShape->raycast(rayLocal, raycastInfo, this, mMemoryManager.getPoolAllocator());

src/collision/broadphase/DynamicAABBTree.cpp

@@ -708,12 +708,12 @@ void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback& ca
         const TreeNode* node = mNodes + nodeID;
 
         // Test if the ray intersects with the current node AABB
-        if (!node->aabb.testRayIntersect(ray.point1, rayDirectionInverse, maxFraction)) continue;
+        if (!node->aabb.testRayIntersect(ray.point1, rayDirectionInverse, ray.radius, maxFraction)) continue;
 
         // If the node is a leaf of the tree
         if (node->isLeaf()) {
 
-            Ray rayTemp(ray.point1, ray.point2, maxFraction);
+            Ray rayTemp(ray.point1, ray.point2, ray.radius, maxFraction);
 
             // Call the callback that will raycast again the broad-phase shape
             decimal hitFraction = callback.raycastBroadPhaseShape(nodeID, rayTemp);

src/collision/shapes/BoxShape.cpp

@@ -70,21 +70,24 @@ bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, Collider* colli
     // For each of the three slabs
     for (int i=0; i<3; i++) {
 
+        // Account for ray radius
+        decimal expandedHalfExtent = mHalfExtents[i] + ray.radius;
+
         // 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 (ray.point1[i] > mHalfExtents[i] || ray.point1[i] < -mHalfExtents[i]) return false;
+            if (ray.point1[i] > expandedHalfExtent || ray.point1[i] < -expandedHalfExtent) 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 = (-mHalfExtents[i] - ray.point1[i]) * oneOverD;
-            decimal t2 = (mHalfExtents[i] - ray.point1[i]) * oneOverD;
-            currentNormal[0] = (i == 0) ? -mHalfExtents[i] : decimal(0.0);
-            currentNormal[1] = (i == 1) ? -mHalfExtents[i] : decimal(0.0);
-            currentNormal[2] = (i == 2) ? -mHalfExtents[i] : decimal(0.0);
+            decimal t1 = (-expandedHalfExtent - ray.point1[i]) * oneOverD;
+            decimal t2 = (expandedHalfExtent - ray.point1[i]) * oneOverD;
+            currentNormal[0] = (i == 0) ? -expandedHalfExtent : decimal(0.0);
+            currentNormal[1] = (i == 1) ? -expandedHalfExtent : decimal(0.0);
+            currentNormal[2] = (i == 2) ? -expandedHalfExtent : decimal(0.0);
 
             // Swap t1 and t2 if need so that t1 is intersection with near plane and
             // t2 with far plane
@@ -114,6 +117,39 @@ bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, Collider* colli
     // The ray intersects the three slabs, we compute the hit point
     Vector3 localHitPoint = ray.point1 + tMin * rayDirection;
 
+    // Adjust hit point for sweep radius
+    if (ray.radius > decimal(0.0))
+    {
+        // Clamp point to box
+        Vector3 contactPointOnBox;
+        contactPointOnBox.x = std::clamp(localHitPoint.x, -mHalfExtents.x, mHalfExtents.x);
+        contactPointOnBox.y = std::clamp(localHitPoint.y, -mHalfExtents.y, mHalfExtents.y);
+        contactPointOnBox.z = std::clamp(localHitPoint.z, -mHalfExtents.z, mHalfExtents.z);
+
+        // Solve hit point for box
+        Vector3 oc = ray.point1 - contactPointOnBox;
+        decimal a = rayDirection.lengthSquare(); // a = D.D
+        decimal b = 2.0 * rayDirection.dot(oc);  // b = 2 * (D . OC)
+        decimal c = oc.lengthSquare() - ray.radius * ray.radius; // c = OC.OC - r^2
+
+        // If origin is inside sphere (c < 0)
+        if (c < 0.0) {
+            if (b >= 0.0) return false; // Already overlapping and moving away
+            tMin = 0.0;
+        } else {
+            decimal discriminant = b * b - 4.0 * a * c;
+            if (discriminant < 0.0) return false; // No real solution, so no hit
+            decimal t = (-b - std::sqrt(discriminant)) / (2.0 * a);
+            if (t < 0.0 || t > ray.maxFraction) return false; // Hit is behind or too far
+            tMin = t;
+        }
+
+        // From sphere to box
+        Vector3 sphereHitPoint = ray.point1 + tMin * rayDirection;
+        normalDirection = (sphereHitPoint - contactPointOnBox).getUnit();
+        localHitPoint = contactPointOnBox;
+    }
+
     raycastInfo.body = collider->getBody();
     raycastInfo.collider = collider;
     raycastInfo.hitFraction = tMin;

src/collision/shapes/CapsuleShape.cpp

@@ -98,6 +98,9 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, Collider* c
 
     const Vector3 n = ray.point2 - ray.point1;
 
+    // "Grow" the capsule with the ray radius
+    decimal extendedMargin = mMargin + ray.radius;
+
     const decimal epsilon = decimal(0.01);
     Vector3 p(decimal(0), -mHalfHeight, decimal(0));
     Vector3 q(decimal(0), mHalfHeight, decimal(0));
@@ -110,16 +113,16 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, Collider* c
     decimal dDotD = d.dot(d);
 
     // Test if the segment is outside the cylinder
-    decimal vec1DotD = (ray.point1 - Vector3(decimal(0.0), -mHalfHeight - mMargin, decimal(0.0))).dot(d);
+    decimal vec1DotD = (ray.point1 - Vector3(decimal(0.0), -mHalfHeight - extendedMargin, decimal(0.0))).dot(d);
     if (vec1DotD < decimal(0.0) && vec1DotD + nDotD < decimal(0.0)) return false;
-    decimal ddotDExtraCaps = decimal(2.0) * mMargin * d.y;
+    decimal ddotDExtraCaps = decimal(2.0) * extendedMargin * d.y;
     if (vec1DotD > dDotD + ddotDExtraCaps && vec1DotD + nDotD > dDotD + ddotDExtraCaps) return false;
 
     decimal nDotN = n.dot(n);
     decimal mDotN = m.dot(n);
 
     decimal a = dDotD * nDotN - nDotD * nDotD;
-    decimal k = m.dot(m) - mMargin * mMargin;
+    decimal k = m.dot(m) - extendedMargin * extendedMargin;
     decimal c = dDotD * k - mDotD * mDotD;
 
     // If the ray is parallel to the capsule axis
@@ -136,13 +139,14 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, Collider* c
             // Check intersection between the ray and the "p" sphere endcap of the capsule
             Vector3 hitLocalPoint;
             decimal hitFraction;
-            if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
+            if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.radius, ray.maxFraction, hitLocalPoint, hitFraction)) {
                 raycastInfo.body = collider->getBody();
                 raycastInfo.collider = collider;
                 raycastInfo.hitFraction = hitFraction;
                 raycastInfo.worldPoint = hitLocalPoint;
                 Vector3 normalDirection = hitLocalPoint - p;
                 raycastInfo.worldNormal = normalDirection;
+                raycastInfo.worldPoint -= normalDirection.getUnit() * ray.radius;
 
                 return true;
             }
@@ -154,13 +158,14 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, Collider* c
             // Check intersection between the ray and the "q" sphere endcap of the capsule
             Vector3 hitLocalPoint;
             decimal hitFraction;
-            if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
+            if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.radius, ray.maxFraction, hitLocalPoint, hitFraction)) {
                 raycastInfo.body = collider->getBody();
                 raycastInfo.collider = collider;
                 raycastInfo.hitFraction = hitFraction;
                 raycastInfo.worldPoint = hitLocalPoint;
                 Vector3 normalDirection = hitLocalPoint - q;
                 raycastInfo.worldNormal = normalDirection;
+                raycastInfo.worldPoint -= normalDirection.getUnit() * ray.radius;
 
                 return true;
             }
@@ -187,13 +192,14 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, Collider* c
         // Check intersection between the ray and the "p" sphere endcap of the capsule
         Vector3 hitLocalPoint;
         decimal hitFraction;
-        if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
+        if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.radius, ray.maxFraction, hitLocalPoint, hitFraction)) {
             raycastInfo.body = collider->getBody();
             raycastInfo.collider = collider;
             raycastInfo.hitFraction = hitFraction;
             raycastInfo.worldPoint = hitLocalPoint;
             Vector3 normalDirection = hitLocalPoint - p;
             raycastInfo.worldNormal = normalDirection;
+            raycastInfo.worldPoint -= normalDirection.getUnit() * ray.radius;
 
             return true;
         }
@@ -205,13 +211,14 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, Collider* c
         // Check intersection between the ray and the "q" sphere endcap of the capsule
         Vector3 hitLocalPoint;
         decimal hitFraction;
-        if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
+        if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.radius, ray.maxFraction, hitLocalPoint, hitFraction)) {
             raycastInfo.body = collider->getBody();
             raycastInfo.collider = collider;
             raycastInfo.hitFraction = hitFraction;
             raycastInfo.worldPoint = hitLocalPoint;
             Vector3 normalDirection = hitLocalPoint - q;
             raycastInfo.worldNormal = normalDirection;
+            raycastInfo.worldPoint -= normalDirection.getUnit() * ray.radius;
 
             return true;
         }
@@ -235,17 +242,19 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, Collider* c
     Vector3 w = (v.dot(d) / d.lengthSquare()) * d;
     Vector3 normalDirection = (localHitPoint - (p + w)).getUnit();
     raycastInfo.worldNormal = normalDirection;
+    raycastInfo.worldPoint -= normalDirection * ray.radius;
 
     return true;
 }
 
 // Raycasting method between a ray one of the two spheres end cap of the capsule
 bool CapsuleShape::raycastWithSphereEndCap(const Vector3& point1, const Vector3& point2,
-                                           const Vector3& sphereCenter, decimal maxFraction,
+                                           const Vector3& sphereCenter, decimal rayRadius, decimal maxFraction,
                                            Vector3& hitLocalPoint, decimal& hitFraction) const {
 
-     const Vector3 m = point1 - sphereCenter;
-    decimal c = m.dot(m) - mMargin * mMargin;
+    const Vector3 m = point1 - sphereCenter;
+    decimal extendedMargin = mMargin + rayRadius;
+    decimal c = m.dot(m) - extendedMargin * extendedMargin;
 
     // If the origin of the ray is inside the sphere, we return no intersection
     if (c < decimal(0.0)) return false;

src/collision/shapes/HeightFieldShape.cpp

@@ -80,7 +80,7 @@ bool HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, Collide
     // Apply the height-field scale inverse scale factor because the mesh is stored without scaling
     // inside the dynamic AABB tree
     const Vector3 inverseScale(decimal(1.0) / mScale.x, decimal(1.0) / mScale.y, decimal(1.0) / mScale.z);
-    Ray scaledRay(ray.point1 * inverseScale, ray.point2 * inverseScale, ray.maxFraction);
+    Ray scaledRay(ray.point1 * inverseScale, ray.point2 * inverseScale, ray.radius, ray.maxFraction);
 
     if (mHeightField->raycast(scaledRay, raycastInfo, collider, getRaycastTestType(), allocator)) {