diff --git a/include/reactphysics3d/collision/shapes/AABB.h b/include/reactphysics3d/collision/shapes/AABB.h index e1807061..633ff897 100644 --- a/include/reactphysics3d/collision/shapes/AABB.h +++ b/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,8 +295,12 @@ 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); @@ -304,8 +308,8 @@ RP3D_FORCE_INLINE bool AABB::testRayIntersect(const Vector3& rayOrigin, const Ve 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,6 +328,10 @@ 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++) { @@ -331,13 +339,13 @@ RP3D_FORCE_INLINE bool AABB::raycast(const Ray& ray, Vector3& hitPoint) const { 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; } diff --git a/include/reactphysics3d/collision/shapes/CapsuleShape.h b/include/reactphysics3d/collision/shapes/CapsuleShape.h index 59296774..acc578be 100644 --- a/include/reactphysics3d/collision/shapes/CapsuleShape.h +++ b/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 diff --git a/include/reactphysics3d/mathematics/Ray.h b/include/reactphysics3d/mathematics/Ray.h index fcef0a5c..b6a02a77 100644 --- a/include/reactphysics3d/mathematics/Ray.h +++ b/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) { } }; diff --git a/src/collision/Collider.cpp b/src/collision/Collider.cpp index 53bc1374..b28037b0 100644 --- a/src/collision/Collider.cpp +++ b/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()); diff --git a/src/collision/broadphase/DynamicAABBTree.cpp b/src/collision/broadphase/DynamicAABBTree.cpp index 5804567c..358c7e14 100644 --- a/src/collision/broadphase/DynamicAABBTree.cpp +++ b/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); diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp index 9efbfdec..0c7316e6 100644 --- a/src/collision/shapes/BoxShape.cpp +++ b/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; diff --git a/src/collision/shapes/CapsuleShape.cpp b/src/collision/shapes/CapsuleShape.cpp index c774ca32..23c26f9b 100644 --- a/src/collision/shapes/CapsuleShape.cpp +++ b/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; diff --git a/src/collision/shapes/HeightFieldShape.cpp b/src/collision/shapes/HeightFieldShape.cpp index 2765732f..f18cf10a 100644 --- a/src/collision/shapes/HeightFieldShape.cpp +++ b/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)) { diff --git a/src/collision/shapes/SphereShape.cpp b/src/collision/shapes/SphereShape.cpp index 515f9568..31d47517 100644 --- a/src/collision/shapes/SphereShape.cpp +++ b/src/collision/shapes/SphereShape.cpp @@ -61,7 +61,10 @@ AABB SphereShape::computeTransformedAABB(const Transform& transform) const { bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, Collider* collider, MemoryAllocator& /*allocator*/) const { const Vector3 m = ray.point1; - decimal c = m.dot(m) - mMargin * mMargin; + + // "Grow" the sphere with the ray radius + decimal extendedMargin = mMargin + ray.radius; + 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; @@ -97,6 +100,11 @@ bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, Collider* co raycastInfo.worldPoint = ray.point1 + t * rayDirection; raycastInfo.worldNormal = raycastInfo.worldPoint; + // Push back the intersection point to account for ray radius + if (ray.radius > decimal(0.0)) { + raycastInfo.worldPoint -= ray.radius * raycastInfo.worldNormal.getUnit(); + } + return true; } diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp index 349ef873..cfa9ca77 100644 --- a/src/collision/shapes/TriangleShape.cpp +++ b/src/collision/shapes/TriangleShape.cpp @@ -145,9 +145,23 @@ bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, Collider* RP3D_PROFILE("TriangleShape::raycast()", mProfiler); const Vector3 pq = ray.point2 - ray.point1; - const Vector3 pa = mPoints[0] - ray.point1; - const Vector3 pb = mPoints[1] - ray.point1; - const Vector3 pc = mPoints[2] - ray.point1; + + // Compute the triangle face normal + Vector3 normal = (mPoints[1] - mPoints[0]).cross(mPoints[2] - mPoints[0]); + normal.normalize(); + normal = normal.dot(pq) > decimal(0.0) ? -normal : normal; + + // Offset the face to account for ray radius + const Vector3 offset = normal * ray.radius; + const Vector3 extendedPoints[3] = { + mPoints[0] + offset, + mPoints[1] + offset, + mPoints[2] + offset + }; + + const Vector3 pa = extendedPoints[0] - ray.point1; + const Vector3 pb = extendedPoints[1] - ray.point1; + const Vector3 pc = extendedPoints[2] - ray.point1; // Test if the line PQ is inside the eges BC, CA and AB. We use the triple // product for this test. @@ -193,20 +207,15 @@ bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, Collider* w *= denom; // Compute the local hit point using the barycentric coordinates - const Vector3 localHitPoint = u * mPoints[0] + v * mPoints[1] + w * mPoints[2]; + const Vector3 localHitPoint = u * extendedPoints[0] + v * extendedPoints[1] + w * extendedPoints[2]; const Vector3 point1ToHitPoint = localHitPoint - ray.point1; const decimal hitFraction = point1ToHitPoint.dot(pq) / pq.lengthSquare(); if (hitFraction < decimal(0.0) || hitFraction > ray.maxFraction) return false; - // Compute the triangle face normal - Vector3 normal = (mPoints[1] - mPoints[0]).cross(mPoints[2] - mPoints[0]); - normal.normalize(); - normal = normal.dot(pq) > decimal(0.0) ? -normal : normal; - raycastInfo.body = collider->getBody(); raycastInfo.collider = collider; - raycastInfo.worldPoint = localHitPoint; + raycastInfo.worldPoint = localHitPoint - normal * ray.radius; raycastInfo.hitFraction = hitFraction; raycastInfo.worldNormal = normal; diff --git a/src/systems/CollisionDetectionSystem.cpp b/src/systems/CollisionDetectionSystem.cpp index d2a3b833..79c3ba1b 100644 --- a/src/systems/CollisionDetectionSystem.cpp +++ b/src/systems/CollisionDetectionSystem.cpp @@ -1187,6 +1187,8 @@ void CollisionDetectionSystem::raycast(RaycastCallback* raycastCallback, const R RP3D_PROFILE("CollisionDetectionSystem::raycast()", mProfiler); + assert(ray.radius >= decimal(0.0)); + RaycastTest rayCastTest(raycastCallback); // Ask the broad-phase algorithm to call the testRaycastAgainstShape() diff --git a/test/tests/collision/TestAABB.h b/test/tests/collision/TestAABB.h index 2383d87e..cee00307 100644 --- a/test/tests/collision/TestAABB.h +++ b/test/tests/collision/TestAABB.h @@ -303,14 +303,14 @@ class TestAABB : public Test { const Vector3 ray8Direction = ray8.point2 - ray8.point1; const Vector3 ray8DirectionInv(decimal(1.0) / ray8Direction.x, decimal(1.0) / ray8Direction.y, decimal(1.0) / ray8Direction.z); - rp3d_test(mAABB1.testRayIntersect(ray1.point1, ray1DirectionInv, decimal(1.0))); - rp3d_test(!mAABB1.testRayIntersect(ray2.point1, ray2DirectionInv, decimal(1.0))); - rp3d_test(mAABB1.testRayIntersect(ray3.point1, ray3DirectionInv, decimal(1.0))); - rp3d_test(mAABB1.testRayIntersect(ray4.point1, ray4DirectionInv, decimal(1.0))); - rp3d_test(mAABB1.testRayIntersect(ray5.point1, ray5DirectionInv, decimal(1.0))); - rp3d_test(mAABB1.testRayIntersect(ray6.point1, ray6DirectionInv, decimal(1.0))); - rp3d_test(!mAABB1.testRayIntersect(ray7.point1, ray7DirectionInv, decimal(1.0))); - rp3d_test(!mAABB1.testRayIntersect(ray8.point1, ray8DirectionInv, decimal(1.0))); + rp3d_test(mAABB1.testRayIntersect(ray1.point1, ray1DirectionInv, decimal(0.0), decimal(1.0))); + rp3d_test(!mAABB1.testRayIntersect(ray2.point1, ray2DirectionInv, decimal(0.0), decimal(1.0))); + rp3d_test(mAABB1.testRayIntersect(ray3.point1, ray3DirectionInv, decimal(0.0), decimal(1.0))); + rp3d_test(mAABB1.testRayIntersect(ray4.point1, ray4DirectionInv, decimal(0.0), decimal(1.0))); + rp3d_test(mAABB1.testRayIntersect(ray5.point1, ray5DirectionInv, decimal(0.0), decimal(1.0))); + rp3d_test(mAABB1.testRayIntersect(ray6.point1, ray6DirectionInv, decimal(0.0), decimal(1.0))); + rp3d_test(!mAABB1.testRayIntersect(ray7.point1, ray7DirectionInv, decimal(0.0), decimal(1.0))); + rp3d_test(!mAABB1.testRayIntersect(ray8.point1, ray8DirectionInv, decimal(0.0), decimal(1.0))); } }; diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h index e4275556..5125006b 100644 --- a/test/tests/collision/TestRaycast.h +++ b/test/tests/collision/TestRaycast.h @@ -414,10 +414,10 @@ class TestRaycast : public Test { mWorld->raycast(ray1, &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(100.0)), &mCallback); + mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0), decimal(100.0)), &mCallback); rp3d_test(!mCallback.isHit); rp3d_test(!mBoxBody->raycast(ray2, raycastInfo3)); @@ -475,22 +475,22 @@ class TestRaycast : public Test { rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); // ----- Test raycast hits ----- // @@ -500,7 +500,7 @@ class TestRaycast : public Test { mWorld->raycast(ray11, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mBoxBody->raycast(ray12, raycastInfo3)); @@ -509,7 +509,7 @@ class TestRaycast : public Test { mWorld->raycast(ray12, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mBoxBody->raycast(ray13, raycastInfo3)); @@ -518,7 +518,7 @@ class TestRaycast : public Test { mWorld->raycast(ray13, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mBoxBody->raycast(ray14, raycastInfo3)); @@ -527,7 +527,7 @@ class TestRaycast : public Test { mWorld->raycast(ray14, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mBoxBody->raycast(ray15, raycastInfo3)); @@ -536,7 +536,7 @@ class TestRaycast : public Test { mWorld->raycast(ray15, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mBoxBody->raycast(ray16, raycastInfo3)); @@ -545,7 +545,7 @@ class TestRaycast : public Test { mWorld->raycast(ray16, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); } @@ -626,10 +626,10 @@ class TestRaycast : public Test { mWorld->raycast(ray1, &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(100.0)), &mCallback); + mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0), decimal(100.0)), &mCallback); rp3d_test(!mCallback.isHit); rp3d_test(!mSphereBody->raycast(ray2, raycastInfo3)); @@ -687,22 +687,22 @@ class TestRaycast : public Test { rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); // ----- Test raycast hits ----- // @@ -712,7 +712,7 @@ class TestRaycast : public Test { mWorld->raycast(ray11, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mSphereBody->raycast(ray12, raycastInfo3)); @@ -721,7 +721,7 @@ class TestRaycast : public Test { mWorld->raycast(ray12, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mSphereBody->raycast(ray13, raycastInfo3)); @@ -729,7 +729,7 @@ class TestRaycast : public Test { mCallback.reset(); mWorld->raycast(ray13, &mCallback); mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mSphereBody->raycast(ray14, raycastInfo3)); rp3d_test(mSphereCollider->raycast(ray14, raycastInfo3)); @@ -737,7 +737,7 @@ class TestRaycast : public Test { mWorld->raycast(ray14, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mSphereBody->raycast(ray15, raycastInfo3)); @@ -746,7 +746,7 @@ class TestRaycast : public Test { mWorld->raycast(ray15, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mSphereBody->raycast(ray16, raycastInfo3)); @@ -755,7 +755,7 @@ class TestRaycast : public Test { mWorld->raycast(ray16, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); } @@ -865,10 +865,10 @@ class TestRaycast : public Test { mWorld->raycast(ray1, &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0.0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(100.0)), &mCallback); + mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0.0), decimal(100.0)), &mCallback); rp3d_test(!mCallback.isHit); rp3d_test(!mCapsuleBody->raycast(ray2, raycastInfo3)); @@ -925,22 +925,22 @@ class TestRaycast : public Test { rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); // ----- Test raycast hits ----- // @@ -950,7 +950,7 @@ class TestRaycast : public Test { mWorld->raycast(ray11, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mCapsuleBody->raycast(ray12, raycastInfo3)); @@ -959,7 +959,7 @@ class TestRaycast : public Test { mWorld->raycast(ray12, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mCapsuleBody->raycast(ray13, raycastInfo3)); @@ -968,7 +968,7 @@ class TestRaycast : public Test { mWorld->raycast(ray13, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mCapsuleBody->raycast(ray14, raycastInfo3)); @@ -977,7 +977,7 @@ class TestRaycast : public Test { mWorld->raycast(ray14, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mCapsuleBody->raycast(ray15, raycastInfo3)); @@ -986,7 +986,7 @@ class TestRaycast : public Test { mWorld->raycast(ray15, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mCapsuleBody->raycast(ray16, raycastInfo3)); @@ -995,7 +995,7 @@ class TestRaycast : public Test { mWorld->raycast(ray16, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); } @@ -1077,10 +1077,10 @@ class TestRaycast : public Test { mWorld->raycast(ray1, &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(100.0)), &mCallback); + mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0), decimal(100.0)), &mCallback); rp3d_test(!mCallback.isHit); rp3d_test(!mConvexMeshBody->raycast(ray2, raycastInfo3)); @@ -1138,22 +1138,22 @@ class TestRaycast : public Test { rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); // ----- Test raycast hits ----- // @@ -1163,7 +1163,7 @@ class TestRaycast : public Test { mWorld->raycast(ray11, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mConvexMeshBody->raycast(ray12, raycastInfo3)); @@ -1172,7 +1172,7 @@ class TestRaycast : public Test { mWorld->raycast(ray12, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mConvexMeshBody->raycast(ray13, raycastInfo3)); @@ -1181,7 +1181,7 @@ class TestRaycast : public Test { mWorld->raycast(ray13, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mConvexMeshBody->raycast(ray14, raycastInfo3)); @@ -1190,7 +1190,7 @@ class TestRaycast : public Test { mWorld->raycast(ray14, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mConvexMeshBody->raycast(ray15, raycastInfo3)); @@ -1199,7 +1199,7 @@ class TestRaycast : public Test { mWorld->raycast(ray15, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mConvexMeshBody->raycast(ray16, raycastInfo3)); @@ -1208,7 +1208,7 @@ class TestRaycast : public Test { mWorld->raycast(ray16, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); } @@ -1244,7 +1244,7 @@ class TestRaycast : public Test { mWorld->raycast(ray1, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mCompoundBody->raycast(ray2, raycastInfo)); @@ -1252,7 +1252,7 @@ class TestRaycast : public Test { mWorld->raycast(ray2, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray2.point1, ray2.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray2.point1, ray2.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mCompoundBody->raycast(ray3, raycastInfo)); @@ -1260,7 +1260,7 @@ class TestRaycast : public Test { mWorld->raycast(ray3, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray3.point1, ray3.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray3.point1, ray3.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mCompoundBody->raycast(ray4, raycastInfo)); @@ -1268,7 +1268,7 @@ class TestRaycast : public Test { mWorld->raycast(ray4, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray4.point1, ray4.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray4.point1, ray4.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mCompoundBody->raycast(ray5, raycastInfo)); @@ -1276,7 +1276,7 @@ class TestRaycast : public Test { mWorld->raycast(ray5, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray5.point1, ray5.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray5.point1, ray5.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mCompoundBody->raycast(ray6, raycastInfo)); @@ -1284,7 +1284,7 @@ class TestRaycast : public Test { mWorld->raycast(ray6, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray6.point1, ray6.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray6.point1, ray6.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); // Raycast hit agains the capsule shape @@ -1302,7 +1302,7 @@ class TestRaycast : public Test { mWorld->raycast(ray11, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mCompoundBody->raycast(ray12, raycastInfo)); @@ -1310,7 +1310,7 @@ class TestRaycast : public Test { mWorld->raycast(ray12, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mCompoundBody->raycast(ray13, raycastInfo)); @@ -1318,7 +1318,7 @@ class TestRaycast : public Test { mWorld->raycast(ray13, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mCompoundBody->raycast(ray14, raycastInfo)); @@ -1326,7 +1326,7 @@ class TestRaycast : public Test { mWorld->raycast(ray14, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mCompoundBody->raycast(ray15, raycastInfo)); @@ -1334,7 +1334,7 @@ class TestRaycast : public Test { mWorld->raycast(ray15, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mCompoundBody->raycast(ray16, raycastInfo)); @@ -1342,7 +1342,7 @@ class TestRaycast : public Test { mWorld->raycast(ray16, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); } @@ -1442,10 +1442,10 @@ class TestRaycast : public Test { mWorld->raycast(ray1, &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(100.0)), &mCallback); + mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0), decimal(100.0)), &mCallback); rp3d_test(!mCallback.isHit); rp3d_test(!mConcaveMeshBody->raycast(ray2, raycastInfo3)); @@ -1503,22 +1503,22 @@ class TestRaycast : public Test { rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); // ----- Test raycast hits ----- // @@ -1528,7 +1528,7 @@ class TestRaycast : public Test { mWorld->raycast(ray11, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mConcaveMeshBody->raycast(ray12, raycastInfo3)); @@ -1537,7 +1537,7 @@ class TestRaycast : public Test { mWorld->raycast(ray12, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mConcaveMeshBody->raycast(ray13, raycastInfo3)); @@ -1546,7 +1546,7 @@ class TestRaycast : public Test { mWorld->raycast(ray13, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mConcaveMeshBody->raycast(ray14, raycastInfo3)); @@ -1555,7 +1555,7 @@ class TestRaycast : public Test { mWorld->raycast(ray14, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mConcaveMeshBody->raycast(ray15, raycastInfo3)); @@ -1564,7 +1564,7 @@ class TestRaycast : public Test { mWorld->raycast(ray15, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mConcaveMeshBody->raycast(ray16, raycastInfo3)); @@ -1573,7 +1573,7 @@ class TestRaycast : public Test { mWorld->raycast(ray16, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); } @@ -1681,10 +1681,10 @@ class TestRaycast : public Test { mWorld->raycast(ray1, &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0.01)), &mCallback); + mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0), decimal(0.01)), &mCallback); rp3d_test(!mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(100.0)), &mCallback); + mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0), decimal(100.0)), &mCallback); rp3d_test(!mCallback.isHit); rp3d_test(!mHeightFieldBody->raycast(ray2, raycastInfo3)); @@ -1720,7 +1720,7 @@ class TestRaycast : public Test { mWorld->raycast(ray11, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.95)), &mCallback); + mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0), decimal(0.95)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mHeightFieldBody->raycast(ray12, raycastInfo3)); @@ -1729,7 +1729,7 @@ class TestRaycast : public Test { mWorld->raycast(ray12, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.92)), &mCallback); + mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0), decimal(0.92)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mHeightFieldBody->raycast(ray13, raycastInfo3)); @@ -1738,7 +1738,7 @@ class TestRaycast : public Test { mWorld->raycast(ray13, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.8)), &mCallback); + mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0), decimal(0.8)), &mCallback); rp3d_test(mCallback.isHit); rp3d_test(mHeightFieldBody->raycast(ray14, raycastInfo3)); @@ -1747,7 +1747,7 @@ class TestRaycast : public Test { mWorld->raycast(ray14, &mCallback); rp3d_test(mCallback.isHit); mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.82)), &mCallback); + mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0), decimal(0.82)), &mCallback); rp3d_test(mCallback.isHit); } };