diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp index 9ce0dcdb..5caa0ce4 100644 --- a/src/body/CollisionBody.cpp +++ b/src/body/CollisionBody.cpp @@ -203,7 +203,7 @@ void CollisionBody::askForBroadPhaseCollisionCheck() const { bool CollisionBody::testPointInside(const Vector3& worldPoint) const { // For each collision shape of the body - for(ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { + for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { // Test if the point is inside the collision shape if (shape->testPointInside(worldPoint)) return true; @@ -214,12 +214,32 @@ bool CollisionBody::testPointInside(const Vector3& worldPoint) const { // Raycast method bool CollisionBody::raycast(const Ray& ray, decimal distance) { - // TODO : Implement this method + + // 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; + } + return false; } // Raycast method with feedback information +/// The method returns the closest hit among all the collision shapes of the body bool CollisionBody::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) { - // TODO : Implement this method - return false; + + bool isHit = false; + + // 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, raycastInfo, distance)) { + distance = raycastInfo.distance; + isHit = true; + } + } + + return isHit; } diff --git a/src/body/CollisionBody.h b/src/body/CollisionBody.h index 7e12f783..3958e43b 100644 --- a/src/body/CollisionBody.h +++ b/src/body/CollisionBody.h @@ -179,6 +179,7 @@ class CollisionBody : public Body { friend class CollisionDetection; friend class BroadPhaseAlgorithm; friend class ConvexMeshShape; + friend class ProxyShape; }; // Return the type of the body diff --git a/src/collision/ProxyShape.h b/src/collision/ProxyShape.h index 8abf88e1..5fe67a01 100644 --- a/src/collision/ProxyShape.h +++ b/src/collision/ProxyShape.h @@ -93,6 +93,9 @@ class ProxyShape { /// Return the local to parent body transform const Transform& getLocalToBodyTransform() const; + /// Return the local to world transform + const Transform getLocalToWorldTransform() const; + /// Return true if a point is inside the collision shape bool testPointInside(const Vector3& worldPoint); @@ -146,6 +149,11 @@ inline const Transform& ProxyShape::getLocalToBodyTransform() const { return mLocalToBodyTransform; } +// Return the local to world transform +inline const Transform ProxyShape::getLocalToWorldTransform() const { + return mBody->mTransform * mLocalToBodyTransform; +} + // Return a local support point in a given direction with the object margin inline Vector3 ProxyShape::getLocalSupportPointWithMargin(const Vector3& direction) { return mCollisionShape->getLocalSupportPointWithMargin(direction, &mCachedCollisionData); @@ -163,12 +171,12 @@ inline decimal ProxyShape::getMargin() const { // Raycast method inline bool ProxyShape::raycast(const Ray& ray, decimal distance) { - return mCollisionShape->raycast(ray, distance); + return mCollisionShape->raycast(ray, this, distance); } // Raycast method with feedback information inline bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) { - return mCollisionShape->raycast(ray, raycastInfo, distance); + return mCollisionShape->raycast(ray, raycastInfo, this, distance); } } diff --git a/src/collision/RaycastInfo.h b/src/collision/RaycastInfo.h index ee2722f9..9b90e4d3 100644 --- a/src/collision/RaycastInfo.h +++ b/src/collision/RaycastInfo.h @@ -60,6 +60,9 @@ struct RaycastInfo { /// Hit point in world-space coordinates Vector3 worldPoint; + /// Surface normal at hit point in world-space coordinates + Vector3 worldNormal; + /// Distance from the ray origin to the hit point decimal distance; diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index 88992d94..77dd52e7 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -392,3 +392,104 @@ bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* collis // The point is inside the collision shape return true; } + + +// Ray casting algorithm agains a convex collision shape using the GJK Algorithm +/// This method implements the GJK ray casting algorithm described by Gino Van Den Bergen in +/// "Ray Casting against General Convex Objects with Application to Continuous Collision Detection". +bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* collisionShape, RaycastInfo& raycastInfo, + decimal maxDistance) { + + Vector3 suppA; // Current lower bound point on the ray (starting at ray's origin) + Vector3 suppB; // Support point on the collision shape + const decimal machineEpsilonSquare = MACHINE_EPSILON * MACHINE_EPSILON; + const decimal epsilon = decimal(0.0001); + + // Convert the ray origin and direction into the local-space of the collision shape + const Transform localToWorldTransform = collisionShape->getLocalToWorldTransform(); + const Transform worldToLocalTransform = localToWorldTransform.getInverse(); + Vector3 origin = worldToLocalTransform * ray.origin; + Vector3 rayDirection = worldToLocalTransform.getOrientation() * ray.direction.getUnit(); + Vector3 w; + + // Create a simplex set + Simplex simplex; + + Vector3 n(decimal(0.0), decimal(0.0), decimal(0.0)); + decimal lambda = decimal(0.0); + suppA = origin; // Current lower bound point on the ray (starting at ray's origin) + suppB = collisionShape->getLocalSupportPointWithoutMargin(rayDirection); + Vector3 v = suppA - suppB; + decimal vDotW, vDotR; + decimal distSquare = v.lengthSquare(); + int nbIterations = 0; + + // GJK Algorithm loop + while (distSquare > epsilon && nbIterations < MAX_ITERATIONS_GJK_RAYCAST) { + + // Compute the support points + suppB = collisionShape->getLocalSupportPointWithoutMargin(v); + w = suppA - suppB; + + vDotW = v.dot(w); + + if (vDotW > decimal(0)) { + + vDotR = v.dot(rayDirection); + + if (vDotR >= -machineEpsilonSquare) { + return false; + } + else { + + // We have found a better lower bound for the hit point along the ray + lambda = lambda - vDotW / vDotR; + suppA = origin + lambda * rayDirection; + w = suppA - suppB; + n = v; + } + } + + // Add the new support point to the simplex + if (!simplex.isPointInSimplex(w)) { + simplex.addPoint(w, suppA, suppB); + } + + // Compute the closest point + if (simplex.computeClosestPoint(v)) { + + distSquare = v.lengthSquare(); + } + else { + distSquare = decimal(0.0); + } + + // If the current lower bound distance is larger than the maximum raycasting distance + if (lambda > maxDistance) return false; + + nbIterations++; + } + + // If the origin was inside the shape, we return no hit + if (lambda < MACHINE_EPSILON) return false; + + // Compute the closet points of both objects (without the margins) + Vector3 pointA; + Vector3 pointB; + simplex.computeClosestPointsOfAandB(pointA, pointB); + + // A raycast hit has been found, we fill in the raycast info object + raycastInfo.distance = lambda; + raycastInfo.worldPoint = localToWorldTransform * pointB; + raycastInfo.body = collisionShape->getBody(); + raycastInfo.proxyShape = collisionShape; + + if (n.lengthSquare() >= machineEpsilonSquare) { // The normal vector is valid + raycastInfo.worldNormal = localToWorldTransform.getOrientation() * n.getUnit(); + } + else { // Degenerated normal vector, we return a zero normal vector + raycastInfo.worldNormal = Vector3(decimal(0), decimal(0), decimal(0)); + } + + return true; +} diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.h b/src/collision/narrowphase/GJK/GJKAlgorithm.h index b3040be5..0c671d78 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.h +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.h @@ -39,6 +39,7 @@ namespace reactphysics3d { // Constants const decimal REL_ERROR = decimal(1.0e-3); const decimal REL_ERROR_SQUARE = REL_ERROR * REL_ERROR; +const int MAX_ITERATIONS_GJK_RAYCAST = 32; // Class GJKAlgorithm /** @@ -95,7 +96,11 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm { ContactPointInfo*& contactInfo); /// Use the GJK Algorithm to find if a point is inside a convex collision shape - bool testPointInside(const Vector3& localPoint, ProxyShape *collisionShape); + bool testPointInside(const Vector3& localPoint, ProxyShape* collisionShape); + + /// Ray casting algorithm agains a convex collision shape using the GJK Algorithm + bool raycast(const Ray& ray, ProxyShape* collisionShape, RaycastInfo& raycastInfo, + decimal maxDistance); }; } diff --git a/src/collision/narrowphase/GJK/Simplex.h b/src/collision/narrowphase/GJK/Simplex.h index aa7cc41b..2a840852 100644 --- a/src/collision/narrowphase/GJK/Simplex.h +++ b/src/collision/narrowphase/GJK/Simplex.h @@ -130,7 +130,7 @@ class Simplex { /// Return true if the simplex contains 4 points bool isFull() const; - /// Return true if the simple is empty + /// Return true if the simplex is empty bool isEmpty() const; /// Return the points of the simplex diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp index 314251e8..314f504d 100644 --- a/src/collision/shapes/BoxShape.cpp +++ b/src/collision/shapes/BoxShape.cpp @@ -62,13 +62,20 @@ void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const } // Raycast method -bool BoxShape::raycast(const Ray& ray, decimal distance) const { +bool BoxShape::raycast(const Ray& ray, ProxyShape* proxyShape, decimal distance) const { + + // TODO : Normalize the ray direction + // TODO : Implement this method return false; } // Raycast method with feedback information -bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) const { +bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, + decimal distance) const { + + // TODO : Normalize the ray direction + // TODO : Implement this method return false; } diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h index 76b6a4d1..af9741e7 100644 --- a/src/collision/shapes/BoxShape.h +++ b/src/collision/shapes/BoxShape.h @@ -78,6 +78,14 @@ class BoxShape : public CollisionShape { /// Return true if a point is inside the collision shape 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; + + /// Raycast method with feedback information + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, + decimal distance = RAYCAST_INFINITY_DISTANCE) const; + public : // -------------------- Methods -------------------- // @@ -105,13 +113,6 @@ class BoxShape : public CollisionShape { /// Test equality between two box shapes virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const; - - /// Raycast method - virtual bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const; - - /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, - decimal distance = RAYCAST_INFINITY_DISTANCE) const; }; // Allocate and return a copy of the object diff --git a/src/collision/shapes/CapsuleShape.cpp b/src/collision/shapes/CapsuleShape.cpp index 74f19f1d..d2faa183 100644 --- a/src/collision/shapes/CapsuleShape.cpp +++ b/src/collision/shapes/CapsuleShape.cpp @@ -126,18 +126,6 @@ void CapsuleShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) co 0.0, 0.0, IxxAndzz); } -// Raycast method -bool CapsuleShape::raycast(const Ray& ray, decimal distance) const { - // TODO : Implement this method - return false; -} - -// Raycast method with feedback information -bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) const { - // TODO : Implement this method - return false; -} - // Return true if a point is inside the collision shape bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const { @@ -153,3 +141,22 @@ bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyS (xSquare + zSquare + diffYCenterSphere1 * diffYCenterSphere1) < squareRadius || (xSquare + zSquare + diffYCenterSphere2 * diffYCenterSphere2) < squareRadius; } + +// Raycast method +bool CapsuleShape::raycast(const Ray& ray, ProxyShape* proxyShape, decimal distance) const { + + // TODO : Normalize the ray direction + + // TODO : Implement this method + return false; +} + +// Raycast method with feedback information +bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, + decimal distance) const { + + // TODO : Normalize the ray direction + + // TODO : Implement this method + return false; +} diff --git a/src/collision/shapes/CapsuleShape.h b/src/collision/shapes/CapsuleShape.h index 3759db0c..b987aa9b 100644 --- a/src/collision/shapes/CapsuleShape.h +++ b/src/collision/shapes/CapsuleShape.h @@ -75,6 +75,14 @@ class CapsuleShape : public CollisionShape { /// Return true if a point is inside the collision shape 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; + + /// Raycast method with feedback information + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, + decimal distance = RAYCAST_INFINITY_DISTANCE) const; + public : // -------------------- Methods -------------------- // @@ -105,13 +113,6 @@ class CapsuleShape : public CollisionShape { /// Test equality between two capsule shapes virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const; - - /// Raycast method - virtual bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const; - - /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, - decimal distance = RAYCAST_INFINITY_DISTANCE) const; }; /// Allocate and return a copy of the object diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h index e7581a77..174bdab8 100644 --- a/src/collision/shapes/CollisionShape.h +++ b/src/collision/shapes/CollisionShape.h @@ -86,10 +86,11 @@ class CollisionShape { virtual bool testPointInside(const Vector3& worldPoint, ProxyShape* proxyShape) const=0; /// Raycast method - virtual bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const=0; + virtual bool raycast(const Ray& ray, ProxyShape* proxyShape, + decimal distance = RAYCAST_INFINITY_DISTANCE) const=0; /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, decimal distance = RAYCAST_INFINITY_DISTANCE) const=0; public : diff --git a/src/collision/shapes/ConeShape.cpp b/src/collision/shapes/ConeShape.cpp index ad39ed41..f3d65e5f 100644 --- a/src/collision/shapes/ConeShape.cpp +++ b/src/collision/shapes/ConeShape.cpp @@ -95,13 +95,20 @@ Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction, } // Raycast method -bool ConeShape::raycast(const Ray& ray, decimal distance) const { +bool ConeShape::raycast(const Ray& ray, ProxyShape* proxyShape, decimal distance) const { + + // TODO : Normalize the ray direction + // TODO : Implement this method return false; } // Raycast method with feedback information -bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) const { +bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, + decimal distance) const { + + // TODO : Normalize the ray direction + // TODO : Implement this method return false; } diff --git a/src/collision/shapes/ConeShape.h b/src/collision/shapes/ConeShape.h index ef56bc5b..50ce0b84 100644 --- a/src/collision/shapes/ConeShape.h +++ b/src/collision/shapes/ConeShape.h @@ -82,6 +82,14 @@ class ConeShape : public CollisionShape { /// Return true if a point is inside the collision shape 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; + + /// Raycast method with feedback information + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, + decimal distance = RAYCAST_INFINITY_DISTANCE) const; public : @@ -113,13 +121,6 @@ class ConeShape : public CollisionShape { /// Test equality between two cone shapes virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const; - - /// Raycast method - virtual bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const; - - /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, - decimal distance = RAYCAST_INFINITY_DISTANCE) const; }; // Allocate and return a copy of the object diff --git a/src/collision/shapes/ConvexMeshShape.cpp b/src/collision/shapes/ConvexMeshShape.cpp index 8387f77c..64467568 100644 --- a/src/collision/shapes/ConvexMeshShape.cpp +++ b/src/collision/shapes/ConvexMeshShape.cpp @@ -156,14 +156,14 @@ Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direct } else { // If the edges information is not used - decimal maxDotProduct = DECIMAL_SMALLEST; + double maxDotProduct = DECIMAL_SMALLEST; uint indexMaxDotProduct = 0; // For each vertex of the mesh for (uint i=0; i maxDotProduct) { @@ -231,13 +231,15 @@ bool ConvexMeshShape::isEqualTo(const CollisionShape& otherCollisionShape) const } // Raycast method -bool ConvexMeshShape::raycast(const Ray& ray, decimal distance) const { - // TODO : Implement this method - return false; +bool ConvexMeshShape::raycast(const Ray& ray, ProxyShape* proxyShape, decimal distance) const { + RaycastInfo raycastInfo; + return proxyShape->mBody->mWorld.mCollisionDetection.mNarrowPhaseGJKAlgorithm.raycast( + ray, proxyShape, raycastInfo, distance); } // Raycast method with feedback information -bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) const { - // TODO : Implement this method - return false; +bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, + decimal distance) const { + return proxyShape->mBody->mWorld.mCollisionDetection.mNarrowPhaseGJKAlgorithm.raycast( + ray, proxyShape, raycastInfo, distance); } diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h index e57d1243..18077ddf 100644 --- a/src/collision/shapes/ConvexMeshShape.h +++ b/src/collision/shapes/ConvexMeshShape.h @@ -104,6 +104,14 @@ class ConvexMeshShape : public CollisionShape { /// Return true if a point is inside the collision shape 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; + + /// Raycast method with feedback information + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, + decimal distance = RAYCAST_INFINITY_DISTANCE) const; + public : // -------------------- Methods -------------------- // @@ -145,13 +153,6 @@ class ConvexMeshShape : public CollisionShape { /// Set the variable to know if the edges information is used to speed up the /// collision detection void setIsEdgesInformationUsed(bool isEdgesUsed); - - /// Raycast method - virtual bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const; - - /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, - decimal distance = RAYCAST_INFINITY_DISTANCE) const; }; // Allocate and return a copy of the object diff --git a/src/collision/shapes/CylinderShape.cpp b/src/collision/shapes/CylinderShape.cpp index 9a809e7d..72afb6ef 100644 --- a/src/collision/shapes/CylinderShape.cpp +++ b/src/collision/shapes/CylinderShape.cpp @@ -88,13 +88,20 @@ Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& directio } // Raycast method -bool CylinderShape::raycast(const Ray& ray, decimal distance) const { +bool CylinderShape::raycast(const Ray& ray, ProxyShape* proxyShape, decimal distance) const { + + // TODO : Normalize the ray direction + // TODO : Implement this method return false; } // Raycast method with feedback information -bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) const { +bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, + decimal distance) const { + + // TODO : Normalize the ray direction + // TODO : Implement this method return false; } diff --git a/src/collision/shapes/CylinderShape.h b/src/collision/shapes/CylinderShape.h index 46106f5d..8a2ad9ce 100644 --- a/src/collision/shapes/CylinderShape.h +++ b/src/collision/shapes/CylinderShape.h @@ -80,6 +80,14 @@ class CylinderShape : public CollisionShape { /// Return true if a point is inside the collision shape 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; + + /// Raycast method with feedback information + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, + decimal distance = RAYCAST_INFINITY_DISTANCE) const; + public : // -------------------- Methods -------------------- // @@ -110,13 +118,6 @@ class CylinderShape : public CollisionShape { /// Test equality between two cylinder shapes virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const; - - /// Raycast method - virtual bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const; - - /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, - decimal distance = RAYCAST_INFINITY_DISTANCE) const; }; /// Allocate and return a copy of the object diff --git a/src/collision/shapes/SphereShape.cpp b/src/collision/shapes/SphereShape.cpp index ec252200..50d5e7f0 100644 --- a/src/collision/shapes/SphereShape.cpp +++ b/src/collision/shapes/SphereShape.cpp @@ -47,13 +47,21 @@ SphereShape::~SphereShape() { } // Raycast method -bool SphereShape::raycast(const Ray& ray, decimal distance) const { +bool SphereShape::raycast(const Ray& ray, ProxyShape* proxyShape, decimal distance) const { + + // TODO : Normalize the ray direction + + // TODO : Implement this method return false; } // Raycast method with feedback information -bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) const { +bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, + decimal distance) const { + + // TODO : Normalize the ray direction + // TODO : Implement this method return false; } diff --git a/src/collision/shapes/SphereShape.h b/src/collision/shapes/SphereShape.h index 45a42439..fa70ece4 100644 --- a/src/collision/shapes/SphereShape.h +++ b/src/collision/shapes/SphereShape.h @@ -70,6 +70,14 @@ class SphereShape : public CollisionShape { /// Return true if a point is inside the collision shape 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; + + /// Raycast method with feedback information + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, + decimal distance = RAYCAST_INFINITY_DISTANCE) const; + public : // -------------------- Methods -------------------- // @@ -100,13 +108,6 @@ class SphereShape : public CollisionShape { /// Test equality between two sphere shapes virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const; - - /// Raycast method - virtual bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const; - - /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, - decimal distance = RAYCAST_INFINITY_DISTANCE) const; }; /// Allocate and return a copy of the object diff --git a/src/configuration.h b/src/configuration.h index 61d5d9e2..7fd95554 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -132,7 +132,7 @@ const decimal DYNAMIC_TREE_AABB_GAP = decimal(0.1); const decimal DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER = decimal(1.7); /// Raycasting infinity distance constant -const decimal RAYCAST_INFINITY_DISTANCE = decimal(-1.0); +const decimal RAYCAST_INFINITY_DISTANCE = std::numeric_limits::infinity(); } diff --git a/test/Test.cpp b/test/Test.cpp index 6208d8fe..6d1495d9 100644 --- a/test/Test.cpp +++ b/test/Test.cpp @@ -66,7 +66,7 @@ void Test::applyFail(const std::string& testText, const char* filename, long lin if (mOutputStream) { // Display the failure message - *mOutputStream << mName << "failure : (" << testText << "), " << + *mOutputStream << mName << " failure : (" << testText << "), " << filename << "(line " << lineNumber << ")" << std::endl; } diff --git a/test/tests/collision/TestPointInside.h b/test/tests/collision/TestPointInside.h index 325b20eb..8be91ce8 100644 --- a/test/tests/collision/TestPointInside.h +++ b/test/tests/collision/TestPointInside.h @@ -89,7 +89,7 @@ class TestPointInside : public Test { // Body transform Vector3 position(-3, 2, 7); Quaternion orientation(PI / 5, PI / 6, PI / 7); - //mBodyTransform = Transform(position, orientation); // TODO : Uncomment this + mBodyTransform = Transform(position, orientation); // Create the bodies mBoxBody = mWorld->createCollisionBody(mBodyTransform); @@ -104,7 +104,7 @@ class TestPointInside : public Test { // Collision shape transform Vector3 shapePosition(1, -4, -3); Quaternion shapeOrientation(3 * PI / 6 , -PI / 8, PI / 3); - //mShapeTransform = Transform(shapePosition, shapeOrientation); // TODO : Uncomment this + mShapeTransform = Transform(shapePosition, shapeOrientation); // Compute the the transform from a local shape point to world-space mLocalShapeToWorld = mBodyTransform * mShapeTransform; @@ -156,7 +156,8 @@ class TestPointInside : public Test { convexMeshShapeEdgesInfo.addEdge(3, 7); convexMeshShapeEdgesInfo.setIsEdgesInformationUsed(true); mConvexMeshShapeEdgesInfo = mConvexMeshBodyEdgesInfo->addCollisionShape( - convexMeshShapeEdgesInfo); + convexMeshShapeEdgesInfo, + mShapeTransform); CylinderShape cylinderShape(3, 8, 0); mCylinderShape = mCylinderBody->addCollisionShape(cylinderShape, mShapeTransform); diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h index 5c6a3437..36e68823 100644 --- a/test/tests/collision/TestRaycast.h +++ b/test/tests/collision/TestRaycast.h @@ -50,6 +50,9 @@ class TestRaycast : public Test { // ---------- Atributes ---------- // + // Epsilon + decimal epsilon; + // Physics world CollisionWorld* mWorld; @@ -85,6 +88,8 @@ class TestRaycast : public Test { /// Constructor TestRaycast(const std::string& name) : Test(name) { + epsilon = 0.0001; + // Create the world mWorld = new CollisionWorld(); @@ -125,40 +130,41 @@ class TestRaycast : public Test { mConeShape = mConeBody->addCollisionShape(coneShape, mShapeTransform); ConvexMeshShape convexMeshShape(0); // Box of dimension (2, 3, 4) - convexMeshShape.addVertex(Vector3(-2, -3, 4)); + convexMeshShape.addVertex(Vector3(-2, -3, -4)); + convexMeshShape.addVertex(Vector3(2, -3, -4)); convexMeshShape.addVertex(Vector3(2, -3, 4)); convexMeshShape.addVertex(Vector3(-2, -3, 4)); - convexMeshShape.addVertex(Vector3(2, -3, -4)); - convexMeshShape.addVertex(Vector3(-2, 3, 4)); - convexMeshShape.addVertex(Vector3(2, 3, 4)); convexMeshShape.addVertex(Vector3(-2, 3, -4)); convexMeshShape.addVertex(Vector3(2, 3, -4)); + convexMeshShape.addVertex(Vector3(2, 3, 4)); + convexMeshShape.addVertex(Vector3(-2, 3, 4)); mConvexMeshShape = mConvexMeshBody->addCollisionShape(convexMeshShape, mShapeTransform); ConvexMeshShape convexMeshShapeEdgesInfo(0); - convexMeshShapeEdgesInfo.addVertex(Vector3(-2, -3, 4)); + convexMeshShapeEdgesInfo.addVertex(Vector3(-2, -3, -4)); + convexMeshShapeEdgesInfo.addVertex(Vector3(2, -3, -4)); convexMeshShapeEdgesInfo.addVertex(Vector3(2, -3, 4)); convexMeshShapeEdgesInfo.addVertex(Vector3(-2, -3, 4)); - convexMeshShapeEdgesInfo.addVertex(Vector3(2, -3, -4)); - convexMeshShapeEdgesInfo.addVertex(Vector3(-2, 3, 4)); - convexMeshShapeEdgesInfo.addVertex(Vector3(2, 3, 4)); convexMeshShapeEdgesInfo.addVertex(Vector3(-2, 3, -4)); convexMeshShapeEdgesInfo.addVertex(Vector3(2, 3, -4)); + convexMeshShapeEdgesInfo.addVertex(Vector3(2, 3, 4)); + convexMeshShapeEdgesInfo.addVertex(Vector3(-2, 3, 4)); convexMeshShapeEdgesInfo.addEdge(0, 1); - convexMeshShapeEdgesInfo.addEdge(1, 3); + convexMeshShapeEdgesInfo.addEdge(1, 2); convexMeshShapeEdgesInfo.addEdge(2, 3); - convexMeshShapeEdgesInfo.addEdge(0, 2); + convexMeshShapeEdgesInfo.addEdge(0, 3); convexMeshShapeEdgesInfo.addEdge(4, 5); - convexMeshShapeEdgesInfo.addEdge(5, 7); + convexMeshShapeEdgesInfo.addEdge(5, 6); convexMeshShapeEdgesInfo.addEdge(6, 7); - convexMeshShapeEdgesInfo.addEdge(4, 6); + convexMeshShapeEdgesInfo.addEdge(4, 7); convexMeshShapeEdgesInfo.addEdge(0, 4); convexMeshShapeEdgesInfo.addEdge(1, 5); convexMeshShapeEdgesInfo.addEdge(2, 6); convexMeshShapeEdgesInfo.addEdge(3, 7); convexMeshShapeEdgesInfo.setIsEdgesInformationUsed(true); mConvexMeshShapeEdgesInfo = mConvexMeshBodyEdgesInfo->addCollisionShape( - convexMeshShapeEdgesInfo); + convexMeshShapeEdgesInfo, + mShapeTransform); CylinderShape cylinderShape(2, 5, 0); mCylinderShape = mCylinderBody->addCollisionShape(cylinderShape, mShapeTransform); @@ -823,50 +829,50 @@ class TestRaycast : public Test { test(mWorld->raycast(ray, raycastInfo)); test(raycastInfo.body == mConvexMeshBody); test(raycastInfo.proxyShape == mConvexMeshShape); - 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, 6, 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(mConvexMeshBody->raycast(ray, raycastInfo2)); test(raycastInfo2.body == mConvexMeshBody); test(raycastInfo2.proxyShape == mConvexMeshShape); - 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, 6, 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(mConvexMeshBodyEdgesInfo->raycast(ray, raycastInfo3)); test(raycastInfo3.body == mConvexMeshBodyEdgesInfo); test(raycastInfo3.proxyShape == mConvexMeshShapeEdgesInfo); - 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, 6, 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)); // ProxyCollisionShape::raycast() RaycastInfo raycastInfo4; test(mConvexMeshShape->raycast(ray, raycastInfo4)); test(raycastInfo4.body == mConvexMeshBody); test(raycastInfo4.proxyShape == mConvexMeshShape); - test(approxEqual(raycastInfo4.distance, 6)); - test(approxEqual(raycastInfo4.worldPoint.x, hitPoint.x)); - test(approxEqual(raycastInfo4.worldPoint.y, hitPoint.y)); - test(approxEqual(raycastInfo4.worldPoint.z, hitPoint.z)); + test(approxEqual(raycastInfo4.distance, 6, epsilon)); + test(approxEqual(raycastInfo4.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(raycastInfo4.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(raycastInfo4.worldPoint.z, hitPoint.z, epsilon)); // ProxyCollisionShape::raycast() RaycastInfo raycastInfo5; test(mConvexMeshShapeEdgesInfo->raycast(ray, raycastInfo5)); test(raycastInfo5.body == mConvexMeshBodyEdgesInfo); test(raycastInfo5.proxyShape == mConvexMeshShapeEdgesInfo); - test(approxEqual(raycastInfo5.distance, 6)); - test(approxEqual(raycastInfo5.worldPoint.x, hitPoint.x)); - test(approxEqual(raycastInfo5.worldPoint.y, hitPoint.y)); - test(approxEqual(raycastInfo5.worldPoint.z, hitPoint.z)); + test(approxEqual(raycastInfo5.distance, 6, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.x, hitPoint.x, epsilon)); + test(approxEqual(raycastInfo5.worldPoint.y, hitPoint.y, epsilon)); + test(approxEqual(raycastInfo5.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));