Continue the implementation of convex shape raycasting

src/body/CollisionBody.cpp

@@ -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;
 }

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

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);
 }
 
 }

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;
 

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;
+}

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
 /**
@@ -96,6 +97,10 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
 
         /// Use the GJK Algorithm to find if a point is inside a convex collision shape
         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);
 };
 
 }

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

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;
 }

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

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;
+}

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

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 :

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;
 }

src/collision/shapes/ConeShape.h

@@ -83,6 +83,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 :
 
         // -------------------- Methods -------------------- //
@@ -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

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<mNbVertices; i++) {
 
             // Compute the dot product of the current vertex
-            decimal dotProduct = direction.dot(mVertices[i]);
+            double dotProduct = direction.dot(mVertices[i]);
 
             // If the current dot product is larger than the maximum one
             if (dotProduct > maxDotProduct) {
@@ -231,13 +231,15 @@ bool ConvexMeshShape::isEqualTo(const CollisionShape& otherCollisionShape) const
 }
 
 // Raycast method
-bool ConvexMeshShape::raycast(const Ray& ray, decimal distance) const {
+bool ConvexMeshShape::raycast(const Ray& ray, ProxyShape* proxyShape, decimal distance) const {
-    // TODO : Implement this method
+    RaycastInfo raycastInfo;
-    return false;
+    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 {
+bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,
-    // TODO : Implement this method
+                              decimal distance) const {
-    return false;
+    return proxyShape->mBody->mWorld.mCollisionDetection.mNarrowPhaseGJKAlgorithm.raycast(
+                ray, proxyShape, raycastInfo, distance);
 }

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

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;
 }

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

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;
 }

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

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<decimal>::infinity();
 
 }
 

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);

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.distance, 6, epsilon));
-            test(approxEqual(raycastInfo.worldPoint.x, hitPoint.x));
+            test(approxEqual(raycastInfo.worldPoint.x, hitPoint.x, epsilon));
-            test(approxEqual(raycastInfo.worldPoint.y, hitPoint.y));
+            test(approxEqual(raycastInfo.worldPoint.y, hitPoint.y, epsilon));
-            test(approxEqual(raycastInfo.worldPoint.z, hitPoint.z));
+            test(approxEqual(raycastInfo.worldPoint.z, hitPoint.z, epsilon));
 
             // CollisionBody::raycast()
             RaycastInfo raycastInfo2;
             test(mConvexMeshBody->raycast(ray, raycastInfo2));
             test(raycastInfo2.body == mConvexMeshBody);
             test(raycastInfo2.proxyShape == mConvexMeshShape);
-            test(approxEqual(raycastInfo2.distance, 6));
+            test(approxEqual(raycastInfo2.distance, 6, epsilon));
-            test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x));
+            test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon));
-            test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y));
+            test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon));
-            test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z));
+            test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon));
 
             // ProxyCollisionShape::raycast()
             RaycastInfo raycastInfo3;
             test(mConvexMeshBodyEdgesInfo->raycast(ray, raycastInfo3));
             test(raycastInfo3.body == mConvexMeshBodyEdgesInfo);
             test(raycastInfo3.proxyShape == mConvexMeshShapeEdgesInfo);
-            test(approxEqual(raycastInfo3.distance, 6));
+            test(approxEqual(raycastInfo3.distance, 6, epsilon));
-            test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x));
+            test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon));
-            test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y));
+            test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon));
-            test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z));
+            test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon));
 
             // 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.distance, 6, epsilon));
-            test(approxEqual(raycastInfo4.worldPoint.x, hitPoint.x));
+            test(approxEqual(raycastInfo4.worldPoint.x, hitPoint.x, epsilon));
-            test(approxEqual(raycastInfo4.worldPoint.y, hitPoint.y));
+            test(approxEqual(raycastInfo4.worldPoint.y, hitPoint.y, epsilon));
-            test(approxEqual(raycastInfo4.worldPoint.z, hitPoint.z));
+            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.distance, 6, epsilon));
-            test(approxEqual(raycastInfo5.worldPoint.x, hitPoint.x));
+            test(approxEqual(raycastInfo5.worldPoint.x, hitPoint.x, epsilon));
-            test(approxEqual(raycastInfo5.worldPoint.y, hitPoint.y));
+            test(approxEqual(raycastInfo5.worldPoint.y, hitPoint.y, epsilon));
-            test(approxEqual(raycastInfo5.worldPoint.z, hitPoint.z));
+            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));