diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp index 5caa0ce4..be539988 100644 --- a/src/body/CollisionBody.cpp +++ b/src/body/CollisionBody.cpp @@ -213,13 +213,13 @@ bool CollisionBody::testPointInside(const Vector3& worldPoint) const { } // Raycast method -bool CollisionBody::raycast(const Ray& ray, decimal distance) { +bool CollisionBody::raycast(const Ray& ray) { // For each collision shape of the body for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { // Test if the ray hits the collision shape - if (shape->raycast(ray, distance)) return true; + if (shape->raycast(ray)) return true; } return false; diff --git a/src/body/CollisionBody.h b/src/body/CollisionBody.h index 3958e43b..16d50c45 100644 --- a/src/body/CollisionBody.h +++ b/src/body/CollisionBody.h @@ -166,7 +166,7 @@ class CollisionBody : public Body { bool testPointInside(const Vector3& worldPoint) const; /// Raycast method - bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE); + bool raycast(const Ray& ray); /// Raycast method with feedback information bool raycast(const Ray& ray, RaycastInfo& raycastInfo, diff --git a/src/collision/ProxyShape.h b/src/collision/ProxyShape.h index 5fe67a01..65eddaf9 100644 --- a/src/collision/ProxyShape.h +++ b/src/collision/ProxyShape.h @@ -100,7 +100,7 @@ class ProxyShape { bool testPointInside(const Vector3& worldPoint); /// Raycast method - bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE); + bool raycast(const Ray& ray); /// Raycast method with feedback information bool raycast(const Ray& ray, RaycastInfo& raycastInfo, @@ -170,8 +170,8 @@ inline decimal ProxyShape::getMargin() const { } // Raycast method -inline bool ProxyShape::raycast(const Ray& ray, decimal distance) { - return mCollisionShape->raycast(ray, this, distance); +inline bool ProxyShape::raycast(const Ray& ray) { + return mCollisionShape->raycast(ray, this); } // Raycast method with feedback information diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp index 314f504d..a2b960a8 100644 --- a/src/collision/shapes/BoxShape.cpp +++ b/src/collision/shapes/BoxShape.cpp @@ -62,7 +62,7 @@ void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const } // Raycast method -bool BoxShape::raycast(const Ray& ray, ProxyShape* proxyShape, decimal distance) const { +bool BoxShape::raycast(const Ray& ray, ProxyShape* proxyShape) const { // TODO : Normalize the ray direction diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h index af9741e7..ba9b9d4d 100644 --- a/src/collision/shapes/BoxShape.h +++ b/src/collision/shapes/BoxShape.h @@ -79,8 +79,7 @@ class BoxShape : public CollisionShape { virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const; /// Raycast method - virtual bool raycast(const Ray& ray, ProxyShape* proxyShape, - decimal distance = RAYCAST_INFINITY_DISTANCE) const; + virtual bool raycast(const Ray& ray, ProxyShape* proxyShape) const; /// Raycast method with feedback information virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, diff --git a/src/collision/shapes/CapsuleShape.cpp b/src/collision/shapes/CapsuleShape.cpp index d2faa183..40433475 100644 --- a/src/collision/shapes/CapsuleShape.cpp +++ b/src/collision/shapes/CapsuleShape.cpp @@ -143,7 +143,7 @@ bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyS } // Raycast method -bool CapsuleShape::raycast(const Ray& ray, ProxyShape* proxyShape, decimal distance) const { +bool CapsuleShape::raycast(const Ray& ray, ProxyShape* proxyShape) const { // TODO : Normalize the ray direction diff --git a/src/collision/shapes/CapsuleShape.h b/src/collision/shapes/CapsuleShape.h index b987aa9b..97184a77 100644 --- a/src/collision/shapes/CapsuleShape.h +++ b/src/collision/shapes/CapsuleShape.h @@ -76,8 +76,7 @@ class CapsuleShape : public CollisionShape { virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const; /// Raycast method - virtual bool raycast(const Ray& ray, ProxyShape* proxyShape, - decimal distance = RAYCAST_INFINITY_DISTANCE) const; + virtual bool raycast(const Ray& ray, ProxyShape* proxyShape) const; /// Raycast method with feedback information virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h index 174bdab8..4e12bc3e 100644 --- a/src/collision/shapes/CollisionShape.h +++ b/src/collision/shapes/CollisionShape.h @@ -86,8 +86,7 @@ class CollisionShape { virtual bool testPointInside(const Vector3& worldPoint, ProxyShape* proxyShape) const=0; /// Raycast method - virtual bool raycast(const Ray& ray, ProxyShape* proxyShape, - decimal distance = RAYCAST_INFINITY_DISTANCE) const=0; + virtual bool raycast(const Ray& ray, ProxyShape* proxyShape) const=0; /// Raycast method with feedback information virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, diff --git a/src/collision/shapes/ConeShape.cpp b/src/collision/shapes/ConeShape.cpp index f3d65e5f..d74a6430 100644 --- a/src/collision/shapes/ConeShape.cpp +++ b/src/collision/shapes/ConeShape.cpp @@ -95,7 +95,7 @@ Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction, } // Raycast method -bool ConeShape::raycast(const Ray& ray, ProxyShape* proxyShape, decimal distance) const { +bool ConeShape::raycast(const Ray& ray, ProxyShape* proxyShape) const { // TODO : Normalize the ray direction diff --git a/src/collision/shapes/ConeShape.h b/src/collision/shapes/ConeShape.h index 50ce0b84..ec8d9e23 100644 --- a/src/collision/shapes/ConeShape.h +++ b/src/collision/shapes/ConeShape.h @@ -84,8 +84,7 @@ class ConeShape : public CollisionShape { virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const; /// Raycast method - virtual bool raycast(const Ray& ray, ProxyShape* proxyShape, - decimal distance = RAYCAST_INFINITY_DISTANCE) const; + virtual bool raycast(const Ray& ray, ProxyShape* proxyShape) const; /// Raycast method with feedback information virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, diff --git a/src/collision/shapes/ConvexMeshShape.cpp b/src/collision/shapes/ConvexMeshShape.cpp index 64467568..c4f8f4db 100644 --- a/src/collision/shapes/ConvexMeshShape.cpp +++ b/src/collision/shapes/ConvexMeshShape.cpp @@ -231,10 +231,10 @@ bool ConvexMeshShape::isEqualTo(const CollisionShape& otherCollisionShape) const } // Raycast method -bool ConvexMeshShape::raycast(const Ray& ray, ProxyShape* proxyShape, decimal distance) const { +bool ConvexMeshShape::raycast(const Ray& ray, ProxyShape* proxyShape) const { RaycastInfo raycastInfo; return proxyShape->mBody->mWorld.mCollisionDetection.mNarrowPhaseGJKAlgorithm.raycast( - ray, proxyShape, raycastInfo, distance); + ray, proxyShape, raycastInfo, RAYCAST_INFINITY_DISTANCE); } // Raycast method with feedback information diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h index 18077ddf..a2c09880 100644 --- a/src/collision/shapes/ConvexMeshShape.h +++ b/src/collision/shapes/ConvexMeshShape.h @@ -105,8 +105,7 @@ class ConvexMeshShape : public CollisionShape { virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const; /// Raycast method - virtual bool raycast(const Ray& ray, ProxyShape* proxyShape, - decimal distance = RAYCAST_INFINITY_DISTANCE) const; + virtual bool raycast(const Ray& ray, ProxyShape* proxyShape) const; /// Raycast method with feedback information virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, diff --git a/src/collision/shapes/CylinderShape.cpp b/src/collision/shapes/CylinderShape.cpp index 72afb6ef..6ac43e87 100644 --- a/src/collision/shapes/CylinderShape.cpp +++ b/src/collision/shapes/CylinderShape.cpp @@ -88,7 +88,7 @@ Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& directio } // Raycast method -bool CylinderShape::raycast(const Ray& ray, ProxyShape* proxyShape, decimal distance) const { +bool CylinderShape::raycast(const Ray& ray, ProxyShape* proxyShape) const { // TODO : Normalize the ray direction diff --git a/src/collision/shapes/CylinderShape.h b/src/collision/shapes/CylinderShape.h index 8a2ad9ce..d1c7a8f5 100644 --- a/src/collision/shapes/CylinderShape.h +++ b/src/collision/shapes/CylinderShape.h @@ -81,8 +81,7 @@ class CylinderShape : public CollisionShape { virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const; /// Raycast method - virtual bool raycast(const Ray& ray, ProxyShape* proxyShape, - decimal distance = RAYCAST_INFINITY_DISTANCE) const; + virtual bool raycast(const Ray& ray, ProxyShape* proxyShape) const; /// Raycast method with feedback information virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, diff --git a/src/collision/shapes/SphereShape.cpp b/src/collision/shapes/SphereShape.cpp index 50d5e7f0..d8eefaf8 100644 --- a/src/collision/shapes/SphereShape.cpp +++ b/src/collision/shapes/SphereShape.cpp @@ -25,6 +25,7 @@ // Libraries #include "SphereShape.h" +#include "collision/ProxyShape.h" #include "configuration.h" #include @@ -47,21 +48,74 @@ SphereShape::~SphereShape() { } // Raycast method -bool SphereShape::raycast(const Ray& ray, ProxyShape* proxyShape, decimal distance) const { +bool SphereShape::raycast(const Ray& ray, ProxyShape* proxyShape) const { - // TODO : Normalize the ray direction + const Transform localToWorldTransform = proxyShape->getLocalToWorldTransform(); + const Transform worldToLocalTransform = localToWorldTransform.getInverse(); + Vector3 origin = worldToLocalTransform * ray.origin; + decimal c = origin.dot(origin) - mRadius * mRadius; + // If the origin of the ray is inside the sphere, we return no intersection + if (c < decimal(0.0)) return false; - // TODO : Implement this method - return false; + Vector3 rayDirection = worldToLocalTransform.getOrientation() * ray.direction.getUnit(); + decimal b = origin.dot(rayDirection); + + // If the origin of the ray is outside the sphere and the ray + // is pointing away from the sphere and there is no intersection + if (c >= decimal(0.0) && b > decimal(0.0)) return false; + + // Compute the discriminant of the quadratic equation + decimal discriminant = b*b - c; + + // If the discriminant is negative, there is no intersection + if (discriminant < decimal(0.0)) return false; + + // There is an intersection + return true; } // Raycast method with feedback information bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, decimal distance) const { - // TODO : Normalize the ray direction + const Transform localToWorldTransform = proxyShape->getLocalToWorldTransform(); + const Transform worldToLocalTransform = localToWorldTransform.getInverse(); + Vector3 origin = worldToLocalTransform * ray.origin; + decimal c = origin.dot(origin) - mRadius * mRadius; - // TODO : Implement this method - return false; + // If the origin of the ray is inside the sphere, we return no intersection + if (c < decimal(0.0)) return false; + + Vector3 rayDirection = worldToLocalTransform.getOrientation() * ray.direction.getUnit(); + decimal b = origin.dot(rayDirection); + + // If the origin of the ray is outside the sphere and the ray + // is pointing away from the sphere and there is no intersection + if (c >= decimal(0.0) && b > decimal(0.0)) return false; + + // Compute the discriminant of the quadratic equation + decimal discriminant = b*b - c; + + // If the discriminant is negative, there is no intersection + if (discriminant < decimal(0.0)) return false; + + // Compute the solution "t" closest to the origin + decimal t = -b - std::sqrt(discriminant); + + assert(t >= decimal(0.0)); + + // If the intersection distance is larger than the allowed distance, return no intersection + if (t > distance) return false; + + // Compute the intersection information + Vector3 localPoint = origin + t * rayDirection; + raycastInfo.body = proxyShape->getBody(); + raycastInfo.proxyShape = proxyShape; + raycastInfo.distance = t; + raycastInfo.worldPoint = localToWorldTransform * localPoint; + raycastInfo.worldNormal = (raycastInfo.worldPoint - + localToWorldTransform.getPosition()).getUnit(); + + return true; } diff --git a/src/collision/shapes/SphereShape.h b/src/collision/shapes/SphereShape.h index fa70ece4..bf3a7a11 100644 --- a/src/collision/shapes/SphereShape.h +++ b/src/collision/shapes/SphereShape.h @@ -71,8 +71,7 @@ class SphereShape : public CollisionShape { virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const; /// Raycast method - virtual bool raycast(const Ray& ray, ProxyShape* proxyShape, - decimal distance = RAYCAST_INFINITY_DISTANCE) const; + virtual bool raycast(const Ray& ray, ProxyShape* proxyShape) const; /// Raycast method with feedback information virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h index 36e68823..6442e105 100644 --- a/test/tests/collision/TestRaycast.h +++ b/test/tests/collision/TestRaycast.h @@ -361,30 +361,30 @@ class TestRaycast : public Test { test(mWorld->raycast(ray, raycastInfo)); test(raycastInfo.body == mSphereBody); test(raycastInfo.proxyShape == mSphereShape); - test(approxEqual(raycastInfo.distance, 6)); - test(approxEqual(raycastInfo.worldPoint.x, hitPoint.x)); - test(approxEqual(raycastInfo.worldPoint.y, hitPoint.y)); - test(approxEqual(raycastInfo.worldPoint.z, hitPoint.z)); + test(approxEqual(raycastInfo.distance, 5, epsilon)); + test(approxEqual(raycastInfo.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(raycastInfo.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(raycastInfo.worldPoint.z, hitPoint.z, epsilon)); // CollisionBody::raycast() RaycastInfo raycastInfo2; test(mSphereBody->raycast(ray, raycastInfo2)); test(raycastInfo2.body == mSphereBody); test(raycastInfo2.proxyShape == mSphereShape); - test(approxEqual(raycastInfo2.distance, 6)); - test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x)); - test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y)); - test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z)); + test(approxEqual(raycastInfo2.distance, 5, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); // ProxyCollisionShape::raycast() RaycastInfo raycastInfo3; test(mSphereShape->raycast(ray, raycastInfo3)); test(raycastInfo3.body == mSphereBody); test(raycastInfo3.proxyShape == mSphereShape); - test(approxEqual(raycastInfo3.distance, 6)); - test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x)); - test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y)); - test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z)); + test(approxEqual(raycastInfo3.distance, 5, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalToWorldMatrix * Vector3(5, 7, -1)); Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalToWorldMatrix * Vector3(4, 6, 7));