Work on the testPointInside() method

src/body/CollisionBody.cpp

@@ -61,7 +61,7 @@ CollisionBody::~CollisionBody() {
 /// the local-space of the body. By default, the second parameter is the identity transform.
 /// This method will return a pointer to the proxy collision shape that links the body with
 /// the collision shape you have added.
-const ProxyShape* CollisionBody::addCollisionShape(const CollisionShape& collisionShape,
+ProxyShape* CollisionBody::addCollisionShape(const CollisionShape& collisionShape,
                                                    const Transform& transform) {
 
     // Create an internal copy of the collision shape into the world (if it does not exist yet)
@@ -199,3 +199,16 @@ void CollisionBody::askForBroadPhaseCollisionCheck() const {
         mWorld.mCollisionDetection.askForBroadPhaseCollisionCheck(shape);
     }
 }
+
+// Return true if a point is inside the collision body
+bool CollisionBody::testPointInside(const Vector3& worldPoint) const {
+
+    // For each collision shape of the body
+    for(ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
+
+        // Test if the point is inside the collision shape
+        if (shape->testPointInside(worldPoint)) return true;
+    }
+
+    return false;
+}

src/body/CollisionBody.h

@@ -141,7 +141,7 @@ class CollisionBody : public Body {
         void setTransform(const Transform& transform);
 
         /// Add a collision shape to the body.
-        const ProxyShape* addCollisionShape(const CollisionShape& collisionShape,
+        ProxyShape* addCollisionShape(const CollisionShape& collisionShape,
                                             const Transform& transform = Transform::identity());
 
         /// Remove a collision shape from the body
@@ -163,17 +163,16 @@ class CollisionBody : public Body {
         const ContactManifoldListElement* getContactManifoldsLists() const;
 
         /// Return true if a point is inside the collision body
-        // TODO : Implement this method
         bool testPointInside(const Vector3& worldPoint) const;
 
         /// Raycast method
         // TODO : Implement this method
-        bool raycast(const Ray& ray, decimal distance = INFINITY_DISTANCE) const;
+        bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const;
 
         /// Raycast method with feedback information
         // TODO : Implement this method
         bool raycast(const Ray& ray, RaycastInfo& raycastInfo,
-                     decimal distance = INFINITY_DISTANCE) const;
+                     decimal distance = RAYCAST_INFINITY_DISTANCE) const;
 
         // -------------------- Friendship -------------------- //
 
@@ -181,6 +180,7 @@ class CollisionBody : public Body {
         friend class DynamicsWorld;
         friend class CollisionDetection;
         friend class BroadPhaseAlgorithm;
+        friend class ProxyConvexMeshShape;
 };
 
 // Return the type of the body

src/body/RigidBody.cpp

@@ -175,7 +175,7 @@ void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, cons
 /// total mass of the rigid body and its inertia tensor). The mass must be positive. The third
 /// parameter is the transformation that transform the local-space of the collision shape into
 /// the local-space of the body. By default, the second parameter is the identity transform.
-const ProxyShape* RigidBody::addCollisionShape(const CollisionShape& collisionShape,
+ProxyShape* RigidBody::addCollisionShape(const CollisionShape& collisionShape,
                                                decimal mass, const Transform& transform) {
 
     assert(mass > decimal(0.0));

src/body/RigidBody.h

@@ -204,7 +204,7 @@ class RigidBody : public CollisionBody {
         void applyTorque(const Vector3& torque);
 
         /// Add a collision shape to the body.
-        const ProxyShape* addCollisionShape(const CollisionShape& collisionShape,
+        ProxyShape* addCollisionShape(const CollisionShape& collisionShape,
                                             decimal mass,
                                             const Transform& transform = Transform::identity());
 

src/collision/CollisionDetection.h

@@ -148,6 +148,7 @@ class CollisionDetection {
         // -------------------- Friendship -------------------- //
 
         friend class DynamicsWorld;
+        friend class ProxyConvexMeshShape;
 };
 
 // Select the narrow-phase collision algorithm to use given two collision shapes

src/collision/RaycastInfo.h

@@ -28,12 +28,15 @@
 
 // Libraries
 #include "../mathematics/Vector3.h"
-#include "../body/CollisionBody.h"
-#include "shapes/CollisionShape.h"
 
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
 
+// Declarations
+class CollisionBody;
+class ProxyShape;
+class CollisionShape;
+
 // Structure RaycastInfo
 /**
  * This structure contains the information about a raycast hit.

src/collision/narrowphase/GJK/GJKAlgorithm.cpp

@@ -47,7 +47,7 @@ GJKAlgorithm::~GJKAlgorithm() {
 
 }
 
-// Return true and compute a contact info if the two bounding volumes collide.
+// Return true and compute a contact info if the two collision shapes collide.
 /// This method implements the Hybrid Technique for computing the penetration depth by
 /// running the GJK algorithm on original objects (without margin).
 /// If the objects don't intersect, this method returns false. If they intersect
@@ -330,3 +330,211 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(ProxyShape* collisi
                                                             transform1, collisionShape2, transform2,
                                                             v, contactInfo);
 }
+
+// Use the GJK Algorithm to find if a point is inside a convex collision shape
+bool GJKAlgorithm::testPointInside(const Vector3& worldPoint, ProxyShape* collisionShape) {
+
+    Vector3 suppA;             // Support point of object A
+    Vector3 w;                 // Support point of Minkowski difference A-B
+    //Vector3 pA;                // Closest point of object A
+    //Vector3 pB;                // Closest point of object B
+    decimal vDotw;
+    decimal prevDistSquare;
+
+    // Get the local-space to world-space transforms
+    const Transform localToWorldTransform = collisionShape->getBody()->getTransform() *
+                                 collisionShape->getLocalToBodyTransform();
+    const Transform worldToLocalTransform = localToWorldTransform.getInverse();
+
+    // Support point of object B (object B is a single point)
+    const Vector3 suppB = worldToLocalTransform * worldPoint;
+
+    // Create a simplex set
+    Simplex simplex;
+
+    // Get the previous point V (last cached separating axis)
+    // TODO : Cache separating axis
+    Vector3 v(1, 1, 1);
+
+    // Initialize the upper bound for the square distance
+    decimal distSquare = DECIMAL_LARGEST;
+
+    do {
+
+        // Compute the support points for original objects (without margins) A and B
+        suppA = collisionShape->getLocalSupportPointWithoutMargin(-v);
+        //suppB = body2Tobody1 *
+        //             collisionShape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v);
+
+        // Compute the support point for the Minkowski difference A-B
+        w = suppA - suppB;
+
+        vDotw = v.dot(w);
+
+        /*
+        // If the enlarge objects (with margins) do not intersect
+        if (vDotw > 0.0 && vDotw * vDotw > distSquare * marginSquare) {
+
+            // Cache the current separating axis for frame coherence
+            mCurrentOverlappingPair->setCachedSeparatingAxis(v);
+
+            // No intersection, we return false
+            return false;
+        }
+        */
+
+        /*
+        // If the objects intersect only in the margins
+        if (simplex.isPointInSimplex(w) || distSquare - vDotw <= distSquare * REL_ERROR_SQUARE) {
+
+            // Compute the closet points of both objects (without the margins)
+            simplex.computeClosestPointsOfAandB(pA, pB);
+
+            // Project those two points on the margins to have the closest points of both
+            // object with the margins
+            decimal dist = sqrt(distSquare);
+            assert(dist > 0.0);
+            pA = (pA - (collisionShape1->getMargin() / dist) * v);
+            pB = body2Tobody1.getInverse() * (pB + (collisionShape2->getMargin() / dist) * v);
+
+            // Compute the contact info
+            Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
+            decimal penetrationDepth = margin - dist;
+
+            // Reject the contact if the penetration depth is negative (due too numerical errors)
+            if (penetrationDepth <= 0.0) return false;
+
+            // Create the contact info object
+            contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
+                                 ContactPointInfo(collisionShape1, collisionShape2, normal,
+                                                  penetrationDepth, pA, pB);
+
+            // There is an intersection, therefore we return true
+            return true;
+        }
+        */
+
+        // Add the new support point to the simplex
+        simplex.addPoint(w, suppA, suppB);
+
+        // If the simplex is affinely dependent
+        if (simplex.isAffinelyDependent()) {
+
+            return false;
+
+            /*
+            // Compute the closet points of both objects (without the margins)
+            simplex.computeClosestPointsOfAandB(pA, pB);
+
+            // Project those two points on the margins to have the closest points of both
+            // object with the margins
+            decimal dist = sqrt(distSquare);
+            assert(dist > 0.0);
+            pA = (pA - (collisionShape1->getMargin() / dist) * v);
+            pB = body2Tobody1.getInverse() * (pB + (collisionShape2->getMargin() / dist) * v);
+
+            // Compute the contact info
+            Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
+            decimal penetrationDepth = margin - dist;
+
+            // Reject the contact if the penetration depth is negative (due too numerical errors)
+            if (penetrationDepth <= 0.0) return false;
+
+            // Create the contact info object
+            contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
+                                   ContactPointInfo(collisionShape1, collisionShape2, normal,
+                                                    penetrationDepth, pA, pB);
+
+            // There is an intersection, therefore we return true
+            return true;
+            */
+        }
+
+        // Compute the point of the simplex closest to the origin
+        // If the computation of the closest point fail
+        if (!simplex.computeClosestPoint(v)) {
+
+            return false;
+
+            /*
+            // Compute the closet points of both objects (without the margins)
+            simplex.computeClosestPointsOfAandB(pA, pB);
+
+            // Project those two points on the margins to have the closest points of both
+            // object with the margins
+            decimal dist = sqrt(distSquare);
+            assert(dist > 0.0);
+            pA = (pA - (collisionShape1->getMargin() / dist) * v);
+            pB = body2Tobody1.getInverse() * (pB + (collisionShape2->getMargin() / dist) * v);
+
+            // Compute the contact info
+            Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
+            decimal penetrationDepth = margin - dist;
+
+            // Reject the contact if the penetration depth is negative (due too numerical errors)
+            if (penetrationDepth <= 0.0) return false;
+
+            // Create the contact info object
+            contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
+                                 ContactPointInfo(collisionShape1, collisionShape2, normal,
+                                                  penetrationDepth, pA, pB);
+
+            // There is an intersection, therefore we return true
+            return true;
+            */
+        }
+
+        // Store and update the squared distance of the closest point
+        prevDistSquare = distSquare;
+        distSquare = v.lengthSquare();
+
+        // If the distance to the closest point doesn't improve a lot
+        if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
+
+            return false;
+
+            /*
+            simplex.backupClosestPointInSimplex(v);
+
+            // Get the new squared distance
+            distSquare = v.lengthSquare();
+
+            // Compute the closet points of both objects (without the margins)
+            simplex.computeClosestPointsOfAandB(pA, pB);
+
+            // Project those two points on the margins to have the closest points of both
+            // object with the margins
+            decimal dist = sqrt(distSquare);
+            assert(dist > 0.0);
+            pA = (pA - (collisionShape1->getMargin() / dist) * v);
+            pB = body2Tobody1.getInverse() * (pB + (collisionShape2->getMargin() / dist) * v);
+
+            // Compute the contact info
+            Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
+            decimal penetrationDepth = margin - dist;
+
+            // Reject the contact if the penetration depth is negative (due too numerical errors)
+            if (penetrationDepth <= 0.0) return false;
+
+            // Create the contact info object
+            contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
+                                   ContactPointInfo(collisionShape1, collisionShape2, normal,
+                                                    penetrationDepth, pA, pB);
+
+            // There is an intersection, therefore we return true
+            return true;
+            */
+        }
+    } while(!simplex.isFull() && distSquare > MACHINE_EPSILON *
+                                 simplex.getMaxLengthSquareOfAPoint());
+
+    // The point is inside the collision shape
+    return true;
+
+    // The objects (without margins) intersect. Therefore, we run the GJK algorithm
+    // again but on the enlarged objects to compute a simplex polytope that contains
+    // the origin. Then, we give that simplex polytope to the EPA algorithm to compute
+    // the correct penetration depth and contact points between the enlarged objects.
+    //return computePenetrationDepthForEnlargedObjects(collisionShape1, transform1, collisionShape2,
+    //                                                 transform2, contactInfo, v);
+}

src/collision/narrowphase/GJK/GJKAlgorithm.h

@@ -93,6 +93,9 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
         /// Return true and compute a contact info if the two bounding volumes collide.
         virtual bool testCollision(ProxyShape* collisionShape1, ProxyShape* collisionShape2,
                                    ContactPointInfo*& contactInfo);
+
+        /// Use the GJK Algorithm to find if a point is inside a convex collision shape
+        bool testPointInside(const Vector3& worldPoint, ProxyShape *collisionShape);
 };
 
 }

src/collision/shapes/BoxShape.cpp

@@ -62,6 +62,24 @@ void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const
                         0.0, 0.0, factor * (xSquare + ySquare));
 }
 
+// Raycast method
+bool BoxShape::raycast(const Ray& ray, decimal distance) const {
+    // TODO : Implement this method
+    return false;
+}
+
+// Raycast method with feedback information
+bool BoxShape::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 BoxShape::testPointInside(const Vector3& localPoint) const {
+    // TODO : Implement this method
+    return false;
+}
+
 // Constructor
 ProxyBoxShape::ProxyBoxShape(BoxShape* shape, CollisionBody* body,
                              const Transform& transform, decimal mass)
@@ -73,3 +91,15 @@ ProxyBoxShape::ProxyBoxShape(BoxShape* shape, CollisionBody* body,
 ProxyBoxShape::~ProxyBoxShape() {
 
 }
+
+// Raycast method
+bool ProxyBoxShape::raycast(const Ray& ray, decimal distance) const {
+    // TODO : Implement this method
+    return false;
+}
+
+// Raycast method with feedback information
+bool ProxyBoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) const {
+    // TODO : Implement this method
+    return false;
+}

src/collision/shapes/BoxShape.h

@@ -29,6 +29,7 @@
 // Libraries
 #include <cfloat>
 #include "CollisionShape.h"
+#include "../../body/CollisionBody.h"
 #include "../../mathematics/mathematics.h"
 
 
@@ -66,6 +67,9 @@ class BoxShape : public CollisionShape {
         /// Private assignment operator
         BoxShape& operator=(const BoxShape& shape);
 
+        /// Return true if a point is inside the collision shape
+        bool testPointInside(const Vector3& localPoint) const;
+
     public :
 
         // -------------------- Methods -------------------- //
@@ -103,6 +107,17 @@ class BoxShape : public CollisionShape {
         /// Create a proxy collision shape for the collision shape
         virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
                                              const Transform& transform, decimal mass);
+
+        /// 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;
+
+        // -------------------- Friendship -------------------- //
+
+        friend class ProxyBoxShape;
 };
 
 // Class ProxyBoxShape
@@ -155,6 +170,16 @@ class ProxyBoxShape : public ProxyShape {
 
         /// Return the current collision shape margin
         virtual decimal getMargin() 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;
+
+        /// Return true if a point is inside the collision shape
+        virtual bool testPointInside(const Vector3& worldPoint);
 };
 
 // Allocate and return a copy of the object
@@ -244,6 +269,13 @@ inline decimal ProxyBoxShape::getMargin() const {
     return mCollisionShape->getMargin();
 }
 
+// Return true if a point is inside the collision shape
+inline bool ProxyBoxShape::testPointInside(const Vector3& worldPoint) {
+    const Transform localToWorld = mBody->getTransform() * mLocalToBodyTransform;
+    const Vector3 localPoint = localToWorld.getInverse() * worldPoint;
+    return mCollisionShape->testPointInside(localPoint);
+}
+
 }
 
 #endif

src/collision/shapes/CapsuleShape.cpp

@@ -124,6 +124,24 @@ 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) const {
+    // TODO : Implement this method
+    return false;
+}
+
 // Constructor
 ProxyCapsuleShape::ProxyCapsuleShape(CapsuleShape* shape, CollisionBody* body,
                                      const Transform& transform, decimal mass)
@@ -135,3 +153,15 @@ ProxyCapsuleShape::ProxyCapsuleShape(CapsuleShape* shape, CollisionBody* body,
 ProxyCapsuleShape::~ProxyCapsuleShape() {
 
 }
+
+// Raycast method
+bool ProxyCapsuleShape::raycast(const Ray& ray, decimal distance) const {
+    // TODO : Implement this method
+    return false;
+}
+
+// Raycast method with feedback information
+bool ProxyCapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) const {
+    // TODO : Implement this method
+    return false;
+}

src/collision/shapes/CapsuleShape.h

@@ -28,6 +28,7 @@
 
 // Libraries
 #include "CollisionShape.h"
+#include "../../body/CollisionBody.h"
 #include "../../mathematics/mathematics.h"
 
 // ReactPhysics3D namespace
@@ -63,6 +64,9 @@ class CapsuleShape : public CollisionShape {
         /// Private assignment operator
         CapsuleShape& operator=(const CapsuleShape& shape);
 
+        /// Return true if a point is inside the collision shape
+        bool testPointInside(const Vector3& localPoint) const;
+
     public :
 
         // -------------------- Methods -------------------- //
@@ -103,6 +107,17 @@ class CapsuleShape : public CollisionShape {
         /// Create a proxy collision shape for the collision shape
         virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
                                              const Transform& transform, decimal mass);
+
+        /// 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;
+
+        // -------------------- Friendship -------------------- //
+
+        friend class ProxyCapsuleShape;
 };
 
 // Class ProxyCapsuleShape
@@ -155,6 +170,16 @@ class ProxyCapsuleShape : public ProxyShape {
 
         /// Return the current collision shape margin
         virtual decimal getMargin() 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;
+
+        /// Return true if a point is inside the collision shape
+        virtual bool testPointInside(const Vector3& worldPoint);
 };
 
 /// Allocate and return a copy of the object
@@ -235,6 +260,13 @@ inline decimal ProxyCapsuleShape::getMargin() const {
     return mCollisionShape->getMargin();
 }
 
+// Return true if a point is inside the collision shape
+inline bool ProxyCapsuleShape::testPointInside(const Vector3& worldPoint) {
+    const Transform localToWorld = mBody->getTransform() * mLocalToBodyTransform;
+    const Vector3 localPoint = localToWorld.getInverse() * worldPoint;
+    return mCollisionShape->testPointInside(localPoint);
+}
+
 }
 
 #endif

src/collision/shapes/CollisionShape.h

@@ -123,6 +123,13 @@ class CollisionShape {
         /// Create a proxy collision shape for the collision shape
         virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
                                              const Transform& transform, decimal mass)=0;
+
+        /// Raycast method
+        virtual bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const=0;
+
+        /// Raycast method with feedback information
+        virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo,
+                             decimal distance = RAYCAST_INFINITY_DISTANCE) const=0;
 };
 
 
@@ -138,7 +145,7 @@ class CollisionShape {
  */
 class ProxyShape {
 
-    private:
+    protected:
 
         // -------------------- Attributes -------------------- //
 
@@ -202,12 +209,15 @@ class ProxyShape {
         /// Return the current object margin
         virtual decimal getMargin() const=0;
 
+        /// Return true if a point is inside the collision shape
+        virtual bool testPointInside(const Vector3& worldPoint)=0;
+
         /// Raycast method
-        virtual bool raycast(const Ray& ray, decimal distance = INFINITY_DISTANCE) const=0;
+        virtual bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const=0;
 
         /// Raycast method with feedback information
         virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo,
-                             decimal distance = INFINITY_DISTANCE) const=0;
+                             decimal distance = RAYCAST_INFINITY_DISTANCE) const=0;
 
         // -------------------- Friendship -------------------- //
 

src/collision/shapes/ConeShape.cpp

@@ -93,6 +93,24 @@ Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction) c
     return supportPoint;
 }
 
+// Raycast method
+bool ConeShape::raycast(const Ray& ray, decimal distance) const {
+    // TODO : Implement this method
+    return false;
+}
+
+// Raycast method with feedback information
+bool ConeShape::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 ConeShape::testPointInside(const Vector3& localPoint) const {
+    // TODO : Implement this method
+    return false;
+}
+
 // Constructor
 ProxyConeShape::ProxyConeShape(ConeShape* shape, CollisionBody* body,
                                const Transform& transform, decimal mass)
@@ -104,3 +122,15 @@ ProxyConeShape::ProxyConeShape(ConeShape* shape, CollisionBody* body,
 ProxyConeShape::~ProxyConeShape() {
 
 }
+
+// Raycast method
+bool ProxyConeShape::raycast(const Ray& ray, decimal distance) const {
+    // TODO : Implement this method
+    return false;
+}
+
+// Raycast method with feedback information
+bool ProxyConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) const {
+    // TODO : Implement this method
+    return false;
+}

src/collision/shapes/ConeShape.h

@@ -28,6 +28,7 @@
 
 // Libraries
 #include "CollisionShape.h"
+#include "../../body/CollisionBody.h"
 #include "../../mathematics/mathematics.h"
 
 /// ReactPhysics3D namespace
@@ -71,6 +72,9 @@ class ConeShape : public CollisionShape {
         /// Private assignment operator
         ConeShape& operator=(const ConeShape& shape);
 
+        /// Return true if a point is inside the collision shape
+        bool testPointInside(const Vector3& localPoint) const;
+        
     public :
 
         // -------------------- Methods -------------------- //
@@ -111,6 +115,17 @@ class ConeShape : public CollisionShape {
         /// Create a proxy collision shape for the collision shape
         virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
                                              const Transform& transform, decimal mass);
+
+        /// 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;
+
+        // -------------------- Friendship -------------------- //
+
+        friend class ProxyConeShape;
 };
 
 // Class ProxyConeShape
@@ -163,6 +178,16 @@ class ProxyConeShape : public ProxyShape {
 
         /// Return the current collision shape margin
         virtual decimal getMargin() 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;
+
+        /// Return true if a point is inside the collision body
+        virtual bool testPointInside(const Vector3& worldPoint);
 };
 
 // Allocate and return a copy of the object
@@ -251,6 +276,13 @@ inline decimal ProxyConeShape::getMargin() const {
     return mCollisionShape->getMargin();
 }
 
+// Return true if a point is inside the collision shape
+inline bool ProxyConeShape::testPointInside(const Vector3& worldPoint) {
+    const Transform localToWorld = mBody->getTransform() * mLocalToBodyTransform;
+    const Vector3 localPoint = localToWorld.getInverse() * worldPoint;
+    return mCollisionShape->testPointInside(localPoint);
+}
+
 }
 
 #endif

src/collision/shapes/ConvexMeshShape.cpp

@@ -224,6 +224,18 @@ bool ConvexMeshShape::isEqualTo(const CollisionShape& otherCollisionShape) const
     return true;
 }
 
+// Raycast method
+bool ConvexMeshShape::raycast(const Ray& ray, decimal distance) const {
+    // TODO : Implement this method
+    return false;
+}
+
+// Raycast method with feedback information
+bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) const {
+    // TODO : Implement this method
+    return false;
+}
+
 // Constructor
 ProxyConvexMeshShape::ProxyConvexMeshShape(ConvexMeshShape* shape, CollisionBody* body,
                                            const Transform& transform, decimal mass)
@@ -236,3 +248,16 @@ ProxyConvexMeshShape::ProxyConvexMeshShape(ConvexMeshShape* shape, CollisionBody
 ProxyConvexMeshShape::~ProxyConvexMeshShape() {
 
 }
+
+// Raycast method
+bool ProxyConvexMeshShape::raycast(const Ray& ray, decimal distance) const {
+    // TODO : Implement this method
+    return false;
+}
+
+// Raycast method with feedback information
+bool ProxyConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo,
+                                   decimal distance) const {
+    // TODO : Implement this method
+    return false;
+}

src/collision/shapes/ConvexMeshShape.h

@@ -28,7 +28,9 @@
 
 // Libraries
 #include "CollisionShape.h"
+#include "../../engine/CollisionWorld.h"
 #include "../../mathematics/mathematics.h"
+#include "../narrowphase/GJK/GJKAlgorithm.h"
 #include <vector>
 #include <set>
 #include <map>
@@ -36,6 +38,9 @@
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
 
+// Declaration
+class CollisionWorld;
+
 // Class ConvexMeshShape
 /**
  * This class represents a convex mesh shape. In order to create a convex mesh shape, you
@@ -141,6 +146,17 @@ class ConvexMeshShape : public CollisionShape {
         /// Create a proxy collision shape for the collision shape
         virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
                                              const Transform& transform, decimal mass);
+
+        /// 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;
+
+        // -------------------- Friendship -------------------- //
+
+        friend class ProxyConvexMeshShape;
 };
 
 
@@ -196,6 +212,16 @@ class ProxyConvexMeshShape : public ProxyShape {
 
         /// Return the current collision shape margin
         virtual decimal getMargin() 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;
+
+        /// Return true if a point is inside the collision shape
+        virtual bool testPointInside(const Vector3& worldPoint);
 };
 
 // Allocate and return a copy of the object
@@ -319,6 +345,11 @@ inline decimal ProxyConvexMeshShape::getMargin() const {
     return mCollisionShape->getMargin();
 }
 
+// Return true if a point is inside the collision shape
+inline bool ProxyConvexMeshShape::testPointInside(const Vector3& worldPoint)  {
+    return mBody->mWorld.mCollisionDetection.mNarrowPhaseGJKAlgorithm.testPointInside(worldPoint,this);
+}
+
 }
 
 #endif

src/collision/shapes/CylinderShape.cpp

@@ -86,6 +86,24 @@ Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& directio
     return supportPoint;
 }
 
+// Raycast method
+bool CylinderShape::raycast(const Ray& ray, decimal distance) const {
+    // TODO : Implement this method
+    return false;
+}
+
+// Raycast method with feedback information
+bool CylinderShape::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 CylinderShape::testPointInside(const Vector3& localPoint) const {
+    // TODO : Implement this method
+    return false;
+}
+
 // Constructor
 ProxyCylinderShape::ProxyCylinderShape(CylinderShape* cylinderShape, CollisionBody* body,
                                        const Transform& transform, decimal mass)
@@ -97,3 +115,15 @@ ProxyCylinderShape::ProxyCylinderShape(CylinderShape* cylinderShape, CollisionBo
 ProxyCylinderShape::~ProxyCylinderShape() {
 
 }
+
+// Raycast method
+bool ProxyCylinderShape::raycast(const Ray& ray, decimal distance) const {
+    // TODO : Implement this method
+    return false;
+}
+
+// Raycast method with feedback information
+bool ProxyCylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) const {
+    // TODO : Implement this method
+    return false;
+}

src/collision/shapes/CylinderShape.h

@@ -28,6 +28,7 @@
 
 // Libraries
 #include "CollisionShape.h"
+#include "../../body/CollisionBody.h"
 #include "../../mathematics/mathematics.h"
 
 
@@ -68,6 +69,9 @@ class CylinderShape : public CollisionShape {
         /// Private assignment operator
         CylinderShape& operator=(const CylinderShape& shape);
 
+        /// Return true if a point is inside the collision shape
+        bool testPointInside(const Vector3& localPoint) const;
+
     public :
 
         // -------------------- Methods -------------------- //
@@ -109,6 +113,16 @@ class CylinderShape : public CollisionShape {
         virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
                                              const Transform& transform, decimal mass);
 
+        /// 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;
+
+        // -------------------- Friendship -------------------- //
+
+        friend class ProxyCylinderShape;
 };
 
 // Class ProxyCylinderShape
@@ -160,6 +174,16 @@ class ProxyCylinderShape : public ProxyShape {
 
         /// Return the current collision shape margin
         virtual decimal getMargin() 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;
+
+        /// Return true if a point is inside the collision shape
+        virtual bool testPointInside(const Vector3& worldPoint);
 };
 
 /// Allocate and return a copy of the object
@@ -248,6 +272,13 @@ inline decimal ProxyCylinderShape::getMargin() const {
     return mCollisionShape->getMargin();
 }
 
+// Return true if a point is inside the collision shape
+inline bool ProxyCylinderShape::testPointInside(const Vector3& worldPoint) {
+    const Transform localToWorld = mBody->getTransform() * mLocalToBodyTransform;
+    const Vector3 localPoint = localToWorld.getInverse() * worldPoint;
+    return mCollisionShape->testPointInside(localPoint);
+}
+
 }
 
 #endif

src/collision/shapes/SphereShape.cpp

@@ -46,6 +46,24 @@ SphereShape::~SphereShape() {
 
 }
 
+// Raycast method
+bool SphereShape::raycast(const Ray& ray, decimal distance) const {
+    // TODO : Implement this method
+    return false;
+}
+
+// Raycast method with feedback information
+bool SphereShape::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 SphereShape::testPointInside(const Vector3& localPoint) const {
+    // TODO : Implement this method
+    return false;
+}
+
 // Constructor
 ProxySphereShape::ProxySphereShape(SphereShape* shape, CollisionBody* body,
                                    const Transform& transform, decimal mass)
@@ -57,3 +75,15 @@ ProxySphereShape::ProxySphereShape(SphereShape* shape, CollisionBody* body,
 ProxySphereShape::~ProxySphereShape() {
 
 }
+
+// Raycast method
+bool ProxySphereShape::raycast(const Ray& ray, decimal distance) const {
+    // TODO : Implement this method
+    return false;
+}
+
+// Raycast method with feedback information
+bool ProxySphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) const {
+    // TODO : Implement this method
+    return false;
+}

src/collision/shapes/SphereShape.h

@@ -28,6 +28,7 @@
 
 // Libraries
 #include "CollisionShape.h"
+#include "../../body/CollisionBody.h"
 #include "../../mathematics/mathematics.h"
 
 // ReactPhysics3D namespace
@@ -58,6 +59,9 @@ class SphereShape : public CollisionShape {
         /// Private assignment operator
         SphereShape& operator=(const SphereShape& shape);
 
+        /// Return true if a point is inside the collision shape
+        bool testPointInside(const Vector3& localPoint) const;
+
     public :
 
         // -------------------- Methods -------------------- //
@@ -98,6 +102,17 @@ class SphereShape : public CollisionShape {
         /// Create a proxy collision shape for the collision shape
         virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
                                              const Transform& transform, decimal mass);
+
+        /// 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;
+
+        // -------------------- Friendship -------------------- //
+
+        friend class ProxySphereShape;
 };
 
 
@@ -151,6 +166,16 @@ class ProxySphereShape : public ProxyShape {
 
         /// Return the current collision shape margin
         virtual decimal getMargin() 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;
+
+        /// Return true if a point is inside the collision shape
+        virtual bool testPointInside(const Vector3& worldPoint);
 };
 
 /// Allocate and return a copy of the object
@@ -267,6 +292,13 @@ inline decimal ProxySphereShape::getMargin() const {
     return mCollisionShape->getMargin();
 }
 
+// Return true if a point is inside the collision shape
+inline bool ProxySphereShape::testPointInside(const Vector3& worldPoint) {
+    const Transform localToWorld = mBody->getTransform() * mLocalToBodyTransform;
+    const Vector3 localPoint = localToWorld.getInverse() * worldPoint;
+    return mCollisionShape->testPointInside(localPoint);
+}
+
 }
 
 #endif

src/configuration.h

@@ -131,6 +131,9 @@ const decimal DYNAMIC_TREE_AABB_GAP = decimal(0.1);
 /// followin constant with the linear velocity and the elapsed time between two frames.
 const decimal DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER = decimal(1.7);
 
+/// Raycasting infinity distance constant
+const decimal RAYCAST_INFINITY_DISTANCE = decimal(-1.0);
+
 }
 
 #endif

src/engine/CollisionWorld.h

@@ -122,18 +122,19 @@ class CollisionWorld {
 
         /// Raycast method
         // TODO : Implement this method
-        bool raycast(const Ray& ray, decimal distance = INFINITY_DISTANCE) const;
+        bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const;
 
         /// Raycast method with feedback information
         // TODO : Implement this method
         bool raycast(const Ray& ray, RaycastInfo& raycastInfo,
-                     decimal distance = INFINITY_DISTANCE) const;
+                     decimal distance = RAYCAST_INFINITY_DISTANCE) const;
 
         // -------------------- Friendship -------------------- //
 
         friend class CollisionDetection;
         friend class CollisionBody;
         friend class RigidBody;
+        friend class ProxyConvexMeshShape;
 };
 
 // Return an iterator to the beginning of the bodies of the physics world

test/tests/collision/TestPointInside.h

@@ -27,14 +27,13 @@
 #define TEST_POINT_INSIDE_H
 
 // Libraries
-#include "../../Test.h"
+#include "Test.h"
-#include "../../src/engine/CollisionWorld.h"
+#include "collision/shapes/BoxShape.h"
-#include "../../src/collision/shapes/BoxShape.h"
+#include "collision/shapes/SphereShape.h"
-#include "../../src/collision/shapes/SphereShape.h"
+#include "collision/shapes/CapsuleShape.h"
-#include "../../src/collision/shapes/CapsuleShape.h"
+#include "collision/shapes/ConeShape.h"
-#include "../../src/collision/shapes/ConeShape.h"
+#include "collision/shapes/ConvexMeshShape.h"
-#include "../../src/collision/shapes/ConvexMeshShape.h"
+#include "collision/shapes/CylinderShape.h"
-#include "../../src/collision/shapes/CylinderShape.h"
 
 /// Reactphysics3D namespace
 namespace reactphysics3d {
@@ -50,7 +49,7 @@ class TestPointInside : public Test {
         // ---------- Atributes ---------- //
 
         // Physics world
-        DynamicsWorld* mWorld;
+        CollisionWorld* mWorld;
 
         // Bodies
         CollisionBody* mBoxBody;
@@ -70,7 +69,7 @@ class TestPointInside : public Test {
 
         // Collision Shapes
         ProxyBoxShape* mBoxShape;
-        ProxySphereShape* mSpherShape;
+        ProxySphereShape* mSphereShape;
         ProxyCapsuleShape* mCapsuleShape;
         ProxyConeShape* mConeShape;
         ProxyConvexMeshShape* mConvexMeshShape;
@@ -85,7 +84,7 @@ class TestPointInside : public Test {
         TestPointInside() {
 
             // Create the world
-            mWorld = new rp3d::CollisionWorld();
+            mWorld = new CollisionWorld();
 
             // Body transform
             Vector3 position(-3, 2, 7);
@@ -93,13 +92,13 @@ class TestPointInside : public Test {
             mBodyTransform = Transform(position, orientation);
 
             // Create the bodies
-            mBoxBody = mWorld->createCollisionBody(bodyTransform);
+            mBoxBody = mWorld->createCollisionBody(mBodyTransform);
-            mSphereBody = mWorld->createCollisionBody(bodyTransform);
+            mSphereBody = mWorld->createCollisionBody(mBodyTransform);
-            mCapsuleBody = mWorld->createCollisionBody(bodyTransform);
+            mCapsuleBody = mWorld->createCollisionBody(mBodyTransform);
-            mConeBody = mWorld->createCollisionBody(bodyTransform);
+            mConeBody = mWorld->createCollisionBody(mBodyTransform);
-            mConvexMeshBody = mWorld->createCollisionBody(bodyTransform);
+            mConvexMeshBody = mWorld->createCollisionBody(mBodyTransform);
-            mConvexMeshBodyEdgesInfo = mWorld->createCollisionBody(bodyTransform);
+            mConvexMeshBodyEdgesInfo = mWorld->createCollisionBody(mBodyTransform);
-            mCylinderBody = mWorld->createCollisionBody(bodyTransform);
+            mCylinderBody = mWorld->createCollisionBody(mBodyTransform);
 
             // Collision shape transform
             Vector3 shapePosition(1, -4, -3);
@@ -111,16 +110,16 @@ class TestPointInside : public Test {
 
             // Create collision shapes
             BoxShape boxShape(Vector3(2, 3, 4), 0);
-            mBoxShape = mBoxBody->addCollisionShape(boxShape, shapeTransform);
+            mBoxShape = mBoxBody->addCollisionShape(boxShape, mShapeTransform);
 
             SphereShape sphereShape(3);
-            mSphereShape = mSphereBody->addCollisionShape(sphereShape, shapeTransform);
+            mSphereShape = mSphereBody->addCollisionShape(sphereShape, mShapeTransform);
 
             CapsuleShape capsuleShape(2, 10);
-            mCapsuleShape = mCapsuleBody->addCollisionShape(capsuleShape, shapeTransform);
+            mCapsuleShape = mCapsuleBody->addCollisionShape(capsuleShape, mShapeTransform);
 
             ConeShape coneShape(2, 6, 0);
-            mConeShape = mConeBody->addCollisionShape(coneShape, shapeTransform);
+            mConeShape = mConeBody->addCollisionShape(coneShape, mShapeTransform);
 
             ConvexMeshShape convexMeshShape(0);             // Box of dimension (2, 3, 4)
             convexMeshShape.addVertex(Vector3(-2, -3, 4));