Use a CollisionShapeInfo instead of a ProxyShape in the collision detection algorithms

CMakeLists.txt

@@ -108,6 +108,7 @@ SET (REACTPHYSICS3D_SOURCES
     "src/collision/TriangleMesh.cpp"
     "src/collision/CollisionDetection.h"
     "src/collision/CollisionDetection.cpp"
+    "src/collision/CollisionShapeInfo.h"
     "src/constraint/BallAndSocketJoint.h"
     "src/constraint/BallAndSocketJoint.cpp"
     "src/constraint/ContactPoint.h"

src/collision/CollisionDetection.cpp

@@ -216,13 +216,22 @@ void CollisionDetection::computeNarrowPhase() {
         const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType();
         const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
         NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Type][shape2Type];
+
+        // If there is no collision algorithm between those two kinds of shapes
+        if (narrowPhaseAlgorithm == NULL) continue;
         
         // Notify the narrow-phase algorithm about the overlapping pair we are going to test
         narrowPhaseAlgorithm->setCurrentOverlappingPair(pair);
+
+        // Create the CollisionShapeInfo objects
+        CollisionShapeInfo shape1Info(shape1, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(),
+                                      shape1->getCachedCollisionData());
+        CollisionShapeInfo shape2Info(shape2, shape2->getCollisionShape(), shape2->getLocalToWorldTransform(),
+                                      shape2->getCachedCollisionData());
         
         // Use the narrow-phase collision detection algorithm to check
         // if there really is a collision
-        if (narrowPhaseAlgorithm->testCollision(shape1, shape2, contactInfo)) {
+        if (narrowPhaseAlgorithm->testCollision(shape1Info, shape2Info, contactInfo)) {
             assert(contactInfo != NULL);
 
             // If it is the first contact since the pair are overlapping
@@ -324,12 +333,21 @@ void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* call
         const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
         NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Type][shape2Type];
 
+        // If there is no collision algorithm between those two kinds of shapes
+        if (narrowPhaseAlgorithm == NULL) continue;
+
         // Notify the narrow-phase algorithm about the overlapping pair we are going to test
         narrowPhaseAlgorithm->setCurrentOverlappingPair(pair);
 
+        // Create the CollisionShapeInfo objects
+        CollisionShapeInfo shape1Info(shape1, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(),
+                                      shape1->getCachedCollisionData());
+        CollisionShapeInfo shape2Info(shape2, shape2->getCollisionShape(), shape2->getLocalToWorldTransform(),
+                                      shape2->getCachedCollisionData());
+
         // Use the narrow-phase collision detection algorithm to check
         // if there really is a collision
-        if (narrowPhaseAlgorithm->testCollision(shape1, shape2, contactInfo)) {
+        if (narrowPhaseAlgorithm->testCollision(shape1Info, shape2Info, contactInfo)) {
             assert(contactInfo != NULL);
 
             // Create a new contact
@@ -464,7 +482,6 @@ void CollisionDetection::fillInCollisionMatrix() {
     for (int i=0; i<NB_COLLISION_SHAPE_TYPES; i++) {
         for (int j=0; j<NB_COLLISION_SHAPE_TYPES; j++) {
             mCollisionMatrix[i][j] = mCollisionDispatch->selectAlgorithm(i, j);
-            assert(mCollisionMatrix[i][j] != NULL);
         }
     }
 }

src/collision/CollisionDetection.h

@@ -121,6 +121,10 @@ class CollisionDetection {
 
         /// Fill-in the collision detection matrix
         void fillInCollisionMatrix();
+
+        /// Return the Narrow-phase collision detection algorithm to use between two types of shapes
+        NarrowPhaseAlgorithm* getCollisionAlgorithm(CollisionShapeType shape1Type,
+                                                    CollisionShapeType shape2Type) const;
    
     public :
 
@@ -193,11 +197,17 @@ class CollisionDetection {
         friend class ConvexMeshShape;
 };
 
-/// Set the collision dispatch configuration
+// Return the Narrow-phase collision detection algorithm to use between two types of shapes
+inline NarrowPhaseAlgorithm* CollisionDetection::getCollisionAlgorithm(CollisionShapeType shape1Type,
+                                                                       CollisionShapeType shape2Type) const {
+    return mCollisionMatrix[shape1Type][shape2Type];
+}
+
+// Set the collision dispatch configuration
 inline void CollisionDetection::setCollisionDispatch(CollisionDispatch* collisionDispatch) {
     mCollisionDispatch = collisionDispatch;
 
-    mCollisionDispatch->init(&mMemoryAllocator);
+    mCollisionDispatch->init(this, &mMemoryAllocator);
 
     // Fill-in the collision matrix with the new algorithms to use
     fillInCollisionMatrix();

src/collision/CollisionShapeInfo.h

@@ -0,0 +1,71 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2015 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_COLLISION_SHAPE_INFO_H
+#define REACTPHYSICS3D_COLLISION_SHAPE_INFO_H
+
+// Libraries
+#include "shapes/CollisionShape.h"
+
+/// Namespace ReactPhysics3D
+namespace reactphysics3d {
+
+// Class CollisionShapeInfo
+/**
+ * This structure regroups different things about a collision shape. This is
+ * used to pass information about a collision shape to a collision algorithm.
+ */
+struct CollisionShapeInfo {
+
+    public:
+
+        /// Constructor
+        CollisionShapeInfo(ProxyShape* proxyCollisionShape, const CollisionShape* shape,
+                           const Transform& shapeLocalToWorldTransform,
+                           void** cachedData)
+              : proxyShape(proxyCollisionShape), collisionShape(shape),
+                shapeToWorldTransform(shapeLocalToWorldTransform),
+                cachedCollisionData(cachedData) {
+
+        }
+
+        /// Proxy shape
+        ProxyShape* proxyShape;
+
+        /// Pointer to the collision shape
+        const CollisionShape* collisionShape;
+
+        /// Transform that maps from collision shape local-space to world-space
+        Transform shapeToWorldTransform;
+
+        /// Cached collision data of the proxy shape
+        void** cachedCollisionData;
+
+};
+
+}
+
+#endif
+

src/collision/TriangleVertexArray.cpp

@@ -30,13 +30,16 @@ using namespace reactphysics3d;
 
 // Constructor
 TriangleVertexArray::TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride,
-                                         uint nbTriangles, void* indexesStart, int indexesStride) {
+                                         uint nbTriangles, void* indexesStart, int indexesStride,
+                                         VertexDataType vertexDataType, IndexDataType indexDataType) {
     mNbVertices = nbVertices;
     mVerticesStart = reinterpret_cast<unsigned char*>(verticesStart);
     mVerticesStride = verticesStride;
     mNbTriangles = nbTriangles;
-    mIndexesStart = reinterpret_cast<unsigned char*>(indexesStart);
+    mIndicesStart = reinterpret_cast<unsigned char*>(indexesStart);
-    mIndexesStride = indexesStride;
+    mIndicesStride = indexesStride;
+    mVertexDataType = vertexDataType;
+    mIndexDataType = indexDataType;
 }
 
 // Destructor

src/collision/TriangleVertexArray.h

@@ -43,6 +43,14 @@ namespace reactphysics3d {
  */
 class TriangleVertexArray {
 
+    public:
+
+        /// Data type for the vertices in the array
+        enum VertexDataType {VERTEX_FLOAT_TYPE, VERTEX_DOUBLE_TYPE};
+
+        /// Data type for the indices in the array
+        enum IndexDataType {INDEX_INTEGER_TYPE, INDEX_SHORT_TYPE};
+
     protected:
 
         /// Number of vertices in the array
@@ -59,26 +67,93 @@ class TriangleVertexArray {
         uint mNbTriangles;
 
         /// Pointer to the first vertex index of the array
-        unsigned char* mIndexesStart;
+        unsigned char* mIndicesStart;
 
-        /// Stride (number of bytes) between the beginning of two indexes in
+        /// Stride (number of bytes) between the beginning of two indices in
         /// the array
-        int mIndexesStride;
+        int mIndicesStride;
+
+        /// Data type of the vertices in the array
+        VertexDataType mVertexDataType;
+
+        /// Data type of the indices in the array
+        IndexDataType mIndexDataType;
 
     public:
 
         /// Constructor
         TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride,
-                            uint nbTriangles, void* indexesStart, int indexesStride);
+                            uint nbTriangles, void* indexesStart, int indexesStride,
+                            VertexDataType vertexDataType, IndexDataType indexDataType);
 
         /// Destructor
         ~TriangleVertexArray();
 
+        /// Return the vertex data type
+        VertexDataType getVertexDataType() const;
 
+        /// Return the index data type
+        IndexDataType getIndexDataType() const;
 
+        /// Return the number of vertices
+        uint getNbVertices() const;
 
+        /// Return the number of triangles
+        uint getNbTriangles() const;
+
+        /// Return the vertices stride (number of bytes)
+        int getVerticesStride() const;
+
+        /// Return the indices stride (number of bytes)
+        int getIndicesStride() const;
+
+        /// Return the pointer to the start of the vertices array
+        unsigned char* getVerticesStart() const;
+
+        /// Return the pointer to the start of the indices array
+        unsigned char* getIndicesStart() const;
 };
 
+// Return the vertex data type
+inline TriangleVertexArray::VertexDataType TriangleVertexArray::getVertexDataType() const {
+    return mVertexDataType;
+}
+
+// Return the index data type
+inline TriangleVertexArray::IndexDataType TriangleVertexArray::getIndexDataType() const {
+   return mIndexDataType;
+}
+
+// Return the number of vertices
+inline uint TriangleVertexArray::getNbVertices() const {
+    return mNbVertices;
+}
+
+// Return the number of triangles
+inline uint TriangleVertexArray::getNbTriangles() const {
+    return mNbTriangles;
+}
+
+// Return the vertices stride (number of bytes)
+inline int TriangleVertexArray::getVerticesStride() const {
+    return mVerticesStride;
+}
+
+// Return the indices stride (number of bytes)
+inline int TriangleVertexArray::getIndicesStride() const {
+    return mIndicesStride;
+}
+
+// Return the pointer to the start of the vertices array
+inline unsigned char* TriangleVertexArray::getVerticesStart() const {
+    return mVerticesStart;
+}
+
+// Return the pointer to the start of the indices array
+inline unsigned char* TriangleVertexArray::getIndicesStart() const {
+    return mIndicesStart;
+}
+
 }
 
 #endif

src/collision/narrowphase/CollisionDispatch.h

@@ -50,7 +50,8 @@ class CollisionDispatch {
         virtual ~CollisionDispatch() {}
 
         /// Initialize the collision dispatch configuration
-        virtual void init(MemoryAllocator* memoryAllocator) {
+        virtual void init(const CollisionDetection* collisionDetection,
+                          MemoryAllocator* memoryAllocator) {
 
         }
 

src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp

@@ -24,4 +24,61 @@
 ********************************************************************************/
 
 // Libraries
+#include "collision/shapes/ConvexShape.h"
+#include "collision/shapes/ConcaveShape.h"
 #include "ConcaveVsConvexAlgorithm.h"
+
+using namespace reactphysics3d;
+
+// Constructor
+ConcaveVsConvexAlgorithm::ConcaveVsConvexAlgorithm() {
+
+}
+
+// Destructor
+ConcaveVsConvexAlgorithm::~ConcaveVsConvexAlgorithm() {
+
+}
+
+// Return true and compute a contact info if the two bounding volumes collide
+bool ConcaveVsConvexAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
+                                             const CollisionShapeInfo& shape2Info,
+                                             ContactPointInfo*& contactInfo) {
+
+    const ProxyShape* convexProxyShape;
+    const ProxyShape* concaveProxyShape;
+    const ConvexShape* convexShape;
+    const ConcaveShape* concaveShape;
+
+    // Collision shape 1 is convex, collision shape 2 is concave
+    if (shape1Info.collisionShape->isConvex()) {
+        convexProxyShape = shape1Info.proxyShape;
+        convexShape = static_cast<const ConvexShape*>(shape1Info.collisionShape);
+        concaveProxyShape = shape2Info.proxyShape;
+        concaveShape = static_cast<const ConcaveShape*>(shape2Info.collisionShape);
+    }
+    else {  // Collision shape 2 is convex, collision shape 1 is concave
+        convexProxyShape = shape2Info.proxyShape;
+        convexShape = static_cast<const ConvexShape*>(shape2Info.collisionShape);
+        concaveProxyShape = shape1Info.proxyShape;
+        concaveShape = static_cast<const ConcaveShape*>(shape1Info.collisionShape);
+    }
+
+    // Set the parameters of the callback object
+    mConvexVsTriangleCallback.setConvexShape(convexShape);
+
+    // Compute the convex shape AABB in the local-space of the concave shape
+    AABB aabb;
+    convexShape->computeAABB(aabb, convexProxyShape->getLocalToWorldTransform());
+    concaveShape->computeAABB(aabb, concaveProxyShape->getLocalToWorldTransform().getInverse());
+
+    // Call the convex vs triangle callback for each triangle of the concave shape
+    concaveShape->testAllTriangles(mConvexVsTriangleCallback, aabb);
+}
+
+// Test collision between a triangle and the convex mesh shape
+void ConvexVsTriangleCallback::reportTriangle(const Vector3* trianglePoints) {
+
+    // Create
+
+}

src/collision/narrowphase/ConcaveVsConvexAlgorithm.h

@@ -28,10 +28,37 @@
 
 // Libraries
 #include "NarrowPhaseAlgorithm.h"
+#include "collision/shapes/ConvexShape.h"
+#include "collision/shapes/ConcaveShape.h"
 
 /// Namespace ReactPhysics3D
 namespace reactphysics3d {
 
+// Class ConvexVsTriangleCallback
+/**
+ * This class is used to encapsulate a callback method for
+ * collision detection between the triangle of a concave mesh shape
+ * and a convex shape.
+ */
+class ConvexVsTriangleCallback : public TriangleCallback {
+
+    private:
+
+        /// Convex collision shape to test collision with
+        const ConvexShape* mConvexShape;
+
+    public:
+
+        /// Set the convex collision shape to test collision with
+        void setConvexShape(const ConvexShape* convexShape) {
+            mConvexShape = convexShape;
+        }
+
+        /// Test collision between a triangle and the convex mesh shape
+        virtual void reportTriangle(const Vector3* trianglePoints);
+
+};
+
 // Class ConcaveVsConvexAlgorithm
 /**
  * This class is used to compute the narrow-phase collision detection
@@ -43,6 +70,11 @@ class ConcaveVsConvexAlgorithm : public NarrowPhaseAlgorithm {
 
     protected :
 
+        // -------------------- Attributes -------------------- //
+
+        /// Convex vs Triangle callback
+        ConvexVsTriangleCallback mConvexVsTriangleCallback;
+
         // -------------------- Methods -------------------- //
 
         /// Private copy-constructor
@@ -62,7 +94,8 @@ class ConcaveVsConvexAlgorithm : public NarrowPhaseAlgorithm {
         virtual ~ConcaveVsConvexAlgorithm();
 
         /// Return true and compute a contact info if the two bounding volume collide
-        virtual bool testCollision(ProxyShape* collisionShape1, ProxyShape* collisionShape2,
+        virtual bool testCollision(const CollisionShapeInfo& shape1Info,
+                                   const CollisionShapeInfo& shape2Info,
                                    ContactPointInfo*& contactInfo);
 };
 

src/collision/narrowphase/DefaultCollisionDispatch.cpp

@@ -25,6 +25,7 @@
 
 // Libraries
 #include "DefaultCollisionDispatch.h"
+#include "collision/shapes/CollisionShape.h"
 
 using namespace reactphysics3d;
 
@@ -39,23 +40,35 @@ DefaultCollisionDispatch::~DefaultCollisionDispatch() {
 }
 
 /// Initialize the collision dispatch configuration
-void DefaultCollisionDispatch::init(MemoryAllocator* memoryAllocator) {
+void DefaultCollisionDispatch::init(const CollisionDetection* collisionDetection,
+                                    MemoryAllocator* memoryAllocator) {
 
     // Initialize the collision algorithms
-    mSphereVsSphereAlgorithm.init(memoryAllocator);
+    mSphereVsSphereAlgorithm.init(collisionDetection, memoryAllocator);
-    mGJKAlgorithm.init(memoryAllocator);
+    mGJKAlgorithm.init(collisionDetection, memoryAllocator);
 }
 
 // Select and return the narrow-phase collision detection algorithm to
 // use between two types of collision shapes.
-NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int shape1Type,
+NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int type2) {
-                                                                int shape2Type) {
 
+    CollisionShapeType shape1Type = static_cast<CollisionShapeType>(type1);
+    CollisionShapeType shape2Type = static_cast<CollisionShapeType>(type2);
 
-    if (shape1Type == SPHERE && shape2Type == SPHERE) { // Sphere vs Sphere algorithm
+    // Sphere vs Sphere algorithm
+    if (shape1Type == SPHERE && shape2Type == SPHERE) {
         return &mSphereVsSphereAlgorithm;
     }
-    else {   // GJK algorithm
+    // Concave vs Convex algorithm
+    else if ((!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) ||
+             (!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) {
+        return &mConcaveVsConvexAlgorithm;
+    }
+    // Convex vs Convex algorithm (GJK algorithm)
+    else if (CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) {
         return &mGJKAlgorithm;
     }
+    else {
+        return NULL;
+    }
 }

src/collision/narrowphase/DefaultCollisionDispatch.h

@@ -28,6 +28,7 @@
 
 // Libraries
 #include "CollisionDispatch.h"
+#include "ConcaveVsConvexAlgorithm.h"
 #include "SphereVsSphereAlgorithm.h"
 #include "GJK/GJKAlgorithm.h"
 
@@ -46,6 +47,9 @@ class DefaultCollisionDispatch : public CollisionDispatch {
         /// Sphere vs Sphere collision algorithm
         SphereVsSphereAlgorithm mSphereVsSphereAlgorithm;
 
+        /// Concave vs Convex collision algorithm
+        ConcaveVsConvexAlgorithm mConcaveVsConvexAlgorithm;
+
         /// GJK Algorithm
         GJKAlgorithm mGJKAlgorithm;
 
@@ -58,12 +62,12 @@ class DefaultCollisionDispatch : public CollisionDispatch {
         virtual ~DefaultCollisionDispatch();
 
         /// Initialize the collision dispatch configuration
-        virtual void init(MemoryAllocator* memoryAllocator);
+        virtual void init(const CollisionDetection* collisionDetection,
+                          MemoryAllocator* memoryAllocator);
 
         /// Select and return the narrow-phase collision detection algorithm to
         /// use between two types of collision shapes.
-        virtual NarrowPhaseAlgorithm* selectAlgorithm(int shape1Type,
+        virtual NarrowPhaseAlgorithm* selectAlgorithm(int type1, int type2);
-                                                      int shape2Type);
 };
 
 }

src/collision/narrowphase/EPA/EPAAlgorithm.cpp

@@ -82,20 +82,20 @@ int EPAAlgorithm::isOriginInTetrahedron(const Vector3& p1, const Vector3& p2,
 /// GJK algorithm. The EPA Algorithm will extend this simplex polytope to find
 /// the correct penetration depth
 bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simplex,
-                                                           ProxyShape* proxyShape1,
+                                                           CollisionShapeInfo shape1Info,
                                                            const Transform& transform1,
-                                                           ProxyShape* proxyShape2,
+                                                           CollisionShapeInfo shape2Info,
                                                            const Transform& transform2,
                                                            Vector3& v, ContactPointInfo*& contactInfo) {
 
-    assert(proxyShape1->getCollisionShape()->isConvex());
+    assert(shape1Info.collisionShape->isConvex());
-    assert(proxyShape2->getCollisionShape()->isConvex());
+    assert(shape2Info.collisionShape->isConvex());
 
-    const ConvexShape* shape1 = static_cast<const ConvexShape*>(proxyShape1->getCollisionShape());
+    const ConvexShape* shape1 = static_cast<const ConvexShape*>(shape1Info.collisionShape);
-    const ConvexShape* shape2 = static_cast<const ConvexShape*>(proxyShape2->getCollisionShape());
+    const ConvexShape* shape2 = static_cast<const ConvexShape*>(shape2Info.collisionShape);
 
-    void** shape1CachedCollisionData = proxyShape1->getCachedCollisionData();
+    void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
-    void** shape2CachedCollisionData = proxyShape2->getCachedCollisionData();
+    void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
 
     Vector3 suppPointsA[MAX_SUPPORT_POINTS];  // Support points of object A in local coordinates
     Vector3 suppPointsB[MAX_SUPPORT_POINTS];  // Support points of object B in local coordinates
@@ -427,7 +427,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
     
     // Create the contact info object
     contactInfo = new (mMemoryAllocator->allocate(sizeof(ContactPointInfo)))
-                          ContactPointInfo(proxyShape1, proxyShape2, normal,
+                          ContactPointInfo(shape1Info.proxyShape, shape2Info.proxyShape, normal,
                                            penetrationDepth, pALocal, pBLocal);
     
     return true;

src/collision/narrowphase/EPA/EPAAlgorithm.h

@@ -29,6 +29,7 @@
 // Libraries
 #include "collision/narrowphase/GJK/Simplex.h"
 #include "collision/shapes/CollisionShape.h"
+#include "collision/CollisionShapeInfo.h"
 #include "constraint/ContactPoint.h"
 #include "mathematics/mathematics.h"
 #include "TriangleEPA.h"
@@ -122,9 +123,9 @@ class EPAAlgorithm {
 
         /// Compute the penetration depth with EPA algorithm.
         bool computePenetrationDepthAndContactPoints(const Simplex& simplex,
-                                                     ProxyShape* proxyShape1,
+                                                     CollisionShapeInfo shape1Info,
                                                      const Transform& transform1,
-                                                     ProxyShape* proxyShape2,
+                                                     CollisionShapeInfo shape2Info,
                                                      const Transform& transform2,
                                                      Vector3& v, ContactPointInfo*& contactInfo);
 };

src/collision/narrowphase/GJK/GJKAlgorithm.cpp

@@ -56,7 +56,8 @@ GJKAlgorithm::~GJKAlgorithm() {
 /// algorithm on the enlarged object to obtain a simplex polytope that contains the
 /// origin, they we give that simplex polytope to the EPA algorithm which will compute
 /// the correct penetration depth and contact points between the enlarged objects.
-bool GJKAlgorithm::testCollision(ProxyShape* proxyShape1, ProxyShape* proxyShape2,
+bool GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
+                                 const CollisionShapeInfo& shape2Info,
                                  ContactPointInfo*& contactInfo) {
     
     Vector3 suppA;             // Support point of object A
@@ -67,20 +68,18 @@ bool GJKAlgorithm::testCollision(ProxyShape* proxyShape1, ProxyShape* proxyShape
     decimal vDotw;
     decimal prevDistSquare;
 
-    assert(proxyShape1->getCollisionShape()->isConvex());
+    assert(shape1Info.collisionShape->isConvex());
-    assert(proxyShape2->getCollisionShape()->isConvex());
+    assert(shape2Info.collisionShape->isConvex());
 
-    const ConvexShape* shape1 = static_cast<const ConvexShape*>(proxyShape1->getCollisionShape());
+    const ConvexShape* shape1 = static_cast<const ConvexShape*>(shape1Info.collisionShape);
-    const ConvexShape* shape2 = static_cast<const ConvexShape*>(proxyShape2->getCollisionShape());
+    const ConvexShape* shape2 = static_cast<const ConvexShape*>(shape2Info.collisionShape);
 
-    void** shape1CachedCollisionData = proxyShape1->getCachedCollisionData();
+    void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
-    void** shape2CachedCollisionData = proxyShape2->getCachedCollisionData();
+    void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
 
     // Get the local-space to world-space transforms
-    const Transform transform1 = proxyShape1->getBody()->getTransform() *
+    const Transform transform1 = shape1Info.shapeToWorldTransform;
-                                 proxyShape1->getLocalToBodyTransform();
+    const Transform transform2 = shape2Info.shapeToWorldTransform;
-    const Transform transform2 = proxyShape2->getBody()->getTransform() *
-                                 proxyShape2->getLocalToBodyTransform();
 
     // Transform a point from local space of body 2 to local
     // space of body 1 (the GJK algorithm is done in local space of body 1)
@@ -149,7 +148,7 @@ bool GJKAlgorithm::testCollision(ProxyShape* proxyShape1, ProxyShape* proxyShape
 			
             // Create the contact info object
             contactInfo = new (mMemoryAllocator->allocate(sizeof(ContactPointInfo)))
-                                 ContactPointInfo(proxyShape1, proxyShape2, normal,
+                                 ContactPointInfo(shape1Info.proxyShape, shape2Info.proxyShape, normal,
                                                   penetrationDepth, pA, pB);
 
             // There is an intersection, therefore we return true
@@ -181,7 +180,7 @@ bool GJKAlgorithm::testCollision(ProxyShape* proxyShape1, ProxyShape* proxyShape
 			
             // Create the contact info object
             contactInfo = new (mMemoryAllocator->allocate(sizeof(ContactPointInfo)))
-                                   ContactPointInfo(proxyShape1, proxyShape2, normal,
+                                   ContactPointInfo(shape1Info.proxyShape, shape2Info.proxyShape, normal,
                                                     penetrationDepth, pA, pB);
 
             // There is an intersection, therefore we return true
@@ -211,7 +210,7 @@ bool GJKAlgorithm::testCollision(ProxyShape* proxyShape1, ProxyShape* proxyShape
 			
             // Create the contact info object
             contactInfo = new (mMemoryAllocator->allocate(sizeof(ContactPointInfo)))
-                                 ContactPointInfo(proxyShape1, proxyShape2, normal,
+                                 ContactPointInfo(shape1Info.proxyShape, shape2Info.proxyShape, normal,
                                                   penetrationDepth, pA, pB);
 
             // There is an intersection, therefore we return true
@@ -248,7 +247,7 @@ bool GJKAlgorithm::testCollision(ProxyShape* proxyShape1, ProxyShape* proxyShape
 			
             // Create the contact info object
             contactInfo = new (mMemoryAllocator->allocate(sizeof(ContactPointInfo)))
-                                   ContactPointInfo(proxyShape1, proxyShape2, normal,
+                                   ContactPointInfo(shape1Info.proxyShape, shape2Info.proxyShape, normal,
                                                     penetrationDepth, pA, pB);
 
             // There is an intersection, therefore we return true
@@ -261,7 +260,7 @@ bool GJKAlgorithm::testCollision(ProxyShape* proxyShape1, ProxyShape* proxyShape
     // 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(proxyShape1, transform1, proxyShape2,
+    return computePenetrationDepthForEnlargedObjects(shape1Info, transform1, shape2Info,
                                                      transform2, contactInfo, v);
 }
 
@@ -270,9 +269,9 @@ bool GJKAlgorithm::testCollision(ProxyShape* proxyShape1, ProxyShape* proxyShape
 /// assumed to intersect in the original objects (without margin). Therefore such
 /// a polytope must exist. Then, we give that polytope to the EPA algorithm to
 /// compute the correct penetration depth and contact points of the enlarged objects.
-bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(ProxyShape* proxyShape1,
+bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info,
                                                              const Transform& transform1,
-                                                             ProxyShape* proxyShape2,
+                                                             const CollisionShapeInfo& shape2Info,
                                                              const Transform& transform2,
                                                              ContactPointInfo*& contactInfo,
                                                              Vector3& v) {
@@ -284,14 +283,14 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(ProxyShape* proxySh
     decimal distSquare = DECIMAL_LARGEST;
     decimal prevDistSquare;
 
-    assert(proxyShape1->getCollisionShape()->isConvex());
+    assert(shape1Info.collisionShape->isConvex());
-    assert(proxyShape2->getCollisionShape()->isConvex());
+    assert(shape2Info.collisionShape->isConvex());
 
-    const ConvexShape* shape1 = static_cast<const ConvexShape*>(proxyShape1->getCollisionShape());
+    const ConvexShape* shape1 = static_cast<const ConvexShape*>(shape1Info.collisionShape);
-    const ConvexShape* shape2 = static_cast<const ConvexShape*>(proxyShape2->getCollisionShape());
+    const ConvexShape* shape2 = static_cast<const ConvexShape*>(shape2Info.collisionShape);
 
-    void** shape1CachedCollisionData = proxyShape1->getCachedCollisionData();
+    void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
-    void** shape2CachedCollisionData = proxyShape2->getCachedCollisionData();
+    void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
 
     // Transform a point from local space of body 2 to local space
     // of body 1 (the GJK algorithm is done in local space of body 1)
@@ -343,8 +342,8 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(ProxyShape* proxySh
     // Give the simplex computed with GJK algorithm to the EPA algorithm
     // which will compute the correct penetration depth and contact points
     // between the two enlarged objects
-    return mAlgoEPA.computePenetrationDepthAndContactPoints(simplex, proxyShape1,
+    return mAlgoEPA.computePenetrationDepthAndContactPoints(simplex, shape1Info,
-                                                            transform1, proxyShape2, transform2,
+                                                            transform1, shape2Info, transform2,
                                                             v, contactInfo);
 }
 

src/collision/narrowphase/GJK/GJKAlgorithm.h

@@ -75,9 +75,9 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
         GJKAlgorithm& operator=(const GJKAlgorithm& algorithm);
 
         /// Compute the penetration depth for enlarged objects.
-        bool computePenetrationDepthForEnlargedObjects(ProxyShape* proxyShape1,
+        bool computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info,
                                                        const Transform& transform1,
-                                                       ProxyShape* proxyShape2,
+                                                       const CollisionShapeInfo& shape2Info,
                                                        const Transform& transform2,
                                                        ContactPointInfo*& contactInfo, Vector3& v);
 
@@ -92,10 +92,12 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
         ~GJKAlgorithm();
 
         /// Initalize the algorithm
-        virtual void init(MemoryAllocator* memoryAllocator);
+        virtual void init(const CollisionDetection* collisionDetection,
+                          MemoryAllocator* memoryAllocator);
 
         /// Return true and compute a contact info if the two bounding volumes collide.
-        virtual bool testCollision(ProxyShape* proxyShape1, ProxyShape* proxyShape2,
+        virtual bool testCollision(const CollisionShapeInfo& shape1Info,
+                                   const CollisionShapeInfo& shape2Info,
                                    ContactPointInfo*& contactInfo);
 
         /// Use the GJK Algorithm to find if a point is inside a convex collision shape
@@ -106,8 +108,9 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
 };
 
 // Initalize the algorithm
-inline void GJKAlgorithm::init(MemoryAllocator* memoryAllocator) {
+inline void GJKAlgorithm::init(const CollisionDetection* collisionDetection,
-    NarrowPhaseAlgorithm::init(memoryAllocator);
+                               MemoryAllocator* memoryAllocator) {
+    NarrowPhaseAlgorithm::init(collisionDetection, memoryAllocator);
     mAlgoEPA.init(memoryAllocator);
 }
 

src/collision/narrowphase/NarrowPhaseAlgorithm.cpp

@@ -41,6 +41,7 @@ NarrowPhaseAlgorithm::~NarrowPhaseAlgorithm() {
 }
 
 // Initalize the algorithm
-void NarrowPhaseAlgorithm::init(MemoryAllocator* memoryAllocator) {
+void NarrowPhaseAlgorithm::init(const CollisionDetection* collisionDetection, MemoryAllocator* memoryAllocator) {
+    mCollisionDetection = collisionDetection;
     mMemoryAllocator = memoryAllocator;
 }

src/collision/narrowphase/NarrowPhaseAlgorithm.h

@@ -31,11 +31,13 @@
 #include "constraint/ContactPoint.h"
 #include "memory/MemoryAllocator.h"
 #include "engine/OverlappingPair.h"
-
+#include "collision/CollisionShapeInfo.h"
 
 /// Namespace ReactPhysics3D
 namespace reactphysics3d {
 
+class CollisionDetection;
+
 // Class NarrowPhaseAlgorithm
 /**
  * This abstract class is the base class for a  narrow-phase collision
@@ -48,6 +50,9 @@ class NarrowPhaseAlgorithm {
 
         // -------------------- Attributes -------------------- //
 
+        /// Pointer to the collision detection object
+        const CollisionDetection* mCollisionDetection;
+
         /// Pointer to the memory allocator
         MemoryAllocator* mMemoryAllocator;
 
@@ -73,13 +78,14 @@ class NarrowPhaseAlgorithm {
         virtual ~NarrowPhaseAlgorithm();
 
         /// Initalize the algorithm
-        virtual void init(MemoryAllocator* memoryAllocator);
+        virtual void init(const CollisionDetection* collisionDetection, MemoryAllocator* memoryAllocator);
         
         /// Set the current overlapping pair of bodies
         void setCurrentOverlappingPair(OverlappingPair* overlappingPair);
 
         /// Return true and compute a contact info if the two bounding volume collide
-        virtual bool testCollision(ProxyShape* collisionShape1, ProxyShape* collisionShape2,
+        virtual bool testCollision(const CollisionShapeInfo& shape1Info,
+                                   const CollisionShapeInfo& shape2Info,
                                    ContactPointInfo*& contactInfo)=0;
 };
 

src/collision/narrowphase/SphereVsSphereAlgorithm.cpp

@@ -40,21 +40,17 @@ SphereVsSphereAlgorithm::~SphereVsSphereAlgorithm() {
     
 }   
 
-bool SphereVsSphereAlgorithm::testCollision(ProxyShape* collisionShape1,
+bool SphereVsSphereAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
-                                            ProxyShape* collisionShape2,
+                                            const CollisionShapeInfo& shape2Info,
                                             ContactPointInfo*& contactInfo) {
     
     // Get the sphere collision shapes
-    const CollisionShape* shape1 = collisionShape1->getCollisionShape();
+    const SphereShape* sphereShape1 = static_cast<const SphereShape*>(shape1Info.collisionShape);
-    const CollisionShape* shape2 = collisionShape2->getCollisionShape();
+    const SphereShape* sphereShape2 = static_cast<const SphereShape*>(shape2Info.collisionShape);
-    const SphereShape* sphereShape1 = dynamic_cast<const SphereShape*>(shape1);
-    const SphereShape* sphereShape2 = dynamic_cast<const SphereShape*>(shape2);
 
     // Get the local-space to world-space transforms
-    const Transform transform1 = collisionShape1->getBody()->getTransform() *
+    const Transform& transform1 = shape1Info.shapeToWorldTransform;
-                                 collisionShape1->getLocalToBodyTransform();
+    const Transform& transform2 = shape2Info.shapeToWorldTransform;
-    const Transform transform2 = collisionShape2->getBody()->getTransform() *
-                                 collisionShape2->getLocalToBodyTransform();
 
     // Compute the distance between the centers
     Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition();
@@ -75,7 +71,7 @@ bool SphereVsSphereAlgorithm::testCollision(ProxyShape* collisionShape1,
         
         // Create the contact info object
         contactInfo = new (mMemoryAllocator->allocate(sizeof(ContactPointInfo)))
-                         ContactPointInfo(collisionShape1, collisionShape2,
+                         ContactPointInfo(shape1Info.proxyShape, shape2Info.proxyShape,
                                           vectorBetweenCenters.getUnit(), penetrationDepth,
                                           intersectionOnBody1, intersectionOnBody2);
     

src/collision/narrowphase/SphereVsSphereAlgorithm.h

@@ -63,7 +63,8 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
         virtual ~SphereVsSphereAlgorithm();
 
         /// Return true and compute a contact info if the two bounding volume collide
-        virtual bool testCollision(ProxyShape* collisionShape1, ProxyShape* collisionShape2,
+        virtual bool testCollision(const CollisionShapeInfo& shape1Info,
+                                   const CollisionShapeInfo& shape2Info,
                                    ContactPointInfo*& contactInfo);
 };
 

src/collision/shapes/AABB.h

@@ -97,6 +97,9 @@ class AABB {
         /// Return true if the current AABB contains the AABB given in parameter
         bool contains(const AABB& aabb);
 
+        /// Return true if the AABB of a triangle intersects the AABB
+        bool testCollisionTriangleAABB(const Vector3* trianglePoints) const;
+
         /// Assignment operator
         AABB& operator=(const AABB& aabb);
 
@@ -148,6 +151,20 @@ inline decimal AABB::getVolume() const {
     return (diff.x * diff.y * diff.z);
 }
 
+// Return true if the AABB of a triangle intersects the AABB
+inline bool AABB::testCollisionTriangleAABB(const Vector3* trianglePoints) const {
+
+    if (min3(trianglePoints[0].x, trianglePoints[1].x, trianglePoints[2].x) > mMaxCoordinates.x) return false;
+    if (min3(trianglePoints[0].y, trianglePoints[1].y, trianglePoints[2].y) > mMaxCoordinates.y) return false;
+    if (min3(trianglePoints[0].z, trianglePoints[1].z, trianglePoints[2].z) > mMaxCoordinates.z) return false;
+
+    if (max3(trianglePoints[0].x, trianglePoints[1].x, trianglePoints[2].x) < mMinCoordinates.x) return false;
+    if (max3(trianglePoints[0].y, trianglePoints[1].y, trianglePoints[2].y) < mMinCoordinates.y) return false;
+    if (max3(trianglePoints[0].z, trianglePoints[1].z, trianglePoints[2].z) < mMinCoordinates.z) return false;
+
+    return true;
+}
+
 // Assignment operator
 inline AABB& AABB::operator=(const AABB& aabb) {
     if (this != &aabb) {

src/collision/shapes/CollisionShape.cpp

@@ -32,14 +32,13 @@
 using namespace reactphysics3d;
 
 // Constructor
-CollisionShape::CollisionShape(CollisionShapeType type)
+CollisionShape::CollisionShape(CollisionShapeType type) : mType(type){
-               : mType(type), mNbSimilarCreatedShapes(0) {
     
 }
 
 // Destructor
 CollisionShape::~CollisionShape() {
-    assert(mNbSimilarCreatedShapes == 0);
+
 }
 
 // Compute the world-space AABB of the collision shape given a transform

src/collision/shapes/CollisionShape.h

@@ -60,9 +60,6 @@ class CollisionShape {
 
         /// Type of the collision shape
         CollisionShapeType mType;
-
-        /// Current number of similar created shapes
-        uint mNbSimilarCreatedShapes;
         
         // -------------------- Methods -------------------- //
 
@@ -106,6 +103,9 @@ class CollisionShape {
         /// Compute the world-space AABB of the collision shape given a transform
         virtual void computeAABB(AABB& aabb, const Transform& transform) const;
 
+        /// Return true if the collision shape type is a convex shape
+        static bool isConvex(CollisionShapeType shapeType);
+
         // -------------------- Friendship -------------------- //
 
         friend class ProxyShape;
@@ -120,6 +120,11 @@ inline CollisionShapeType CollisionShape::getType() const {
     return mType;
 }
 
+// Return true if the collision shape type is a convex shape
+inline bool CollisionShape::isConvex(CollisionShapeType shapeType) {
+    return shapeType != CONCAVE_MESH;
+}
+
 }
 
 #endif

src/collision/shapes/ConcaveMeshShape.cpp

@@ -30,7 +30,7 @@ using namespace reactphysics3d;
 
 // Constructor
 ConcaveMeshShape::ConcaveMeshShape(TriangleMesh* triangleMesh) : ConcaveShape(CONCAVE_MESH) {
-
+    mTriangleMesh = triangleMesh;
 }
 
 // Destructor
@@ -38,6 +38,64 @@ ConcaveMeshShape::~ConcaveMeshShape() {
 
 }
 
+// Use a callback method on all triangles of the concave shape inside a given AABB
+void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const {
+
+    // For each sub-part of the mesh
+    for (int i=0; i<mTriangleMesh->getNbSubparts(); i++) {
+
+        // Get the triangle vertex array of the current sub-part
+        TriangleVertexArray* triangleVertexArray = mTriangleMesh->getSubpart(i);
+
+        TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType();
+        TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType();
+        unsigned char* verticesStart = triangleVertexArray->getVerticesStart();
+        unsigned char* indicesStart = triangleVertexArray->getIndicesStart();
+        int vertexStride = triangleVertexArray->getVerticesStride();
+        int indexStride = triangleVertexArray->getIndicesStride();
+
+        // For each triangle of the concave mesh
+        for (int j=0; j<triangleVertexArray->getNbTriangles(); j++) {
+
+            Vector3 trianglePoints[3];
+
+            // For each vertex of the triangle
+            for (int k=0; k < 3; k++) {
+
+                // Get the index of the current vertex in the triangle
+                int vertexIndex;
+                if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) {
+                    vertexIndex = ((unsigned int*)(indicesStart + j * indexStride))[k];
+                }
+                else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) {
+                    vertexIndex = ((unsigned short*)(indicesStart + j * indexStride))[k];
+                }
+
+                // Get the vertices components of the triangle
+                if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) {
+                    const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride);
+                    trianglePoints[k][0] = decimal(vertices[0]);
+                    trianglePoints[k][1] = decimal(vertices[1]);
+                    trianglePoints[k][2] = decimal(vertices[2]);
+                }
+                else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) {
+                    const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride);
+                    trianglePoints[k][0] = decimal(vertices[0]);
+                    trianglePoints[k][1] = decimal(vertices[1]);
+                    trianglePoints[k][2] = decimal(vertices[2]);
+                }
+            }
+
+            // If the triangle AABB intersects with the convex shape AABB
+            if (localAABB.testCollisionTriangleAABB(trianglePoints)) {
+
+                // Call the callback to report this triangle
+                callback.reportTriangle(trianglePoints);
+            }
+        }
+    }
+}
+
 // Raycast method with feedback information
 bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
 

src/collision/shapes/ConcaveMeshShape.h

@@ -34,6 +34,8 @@ namespace reactphysics3d {
 
 // TODO : Implement raycasting with this collision shape
 
+// TODO : Make possible for the user to have a scaling factor on the mesh
+
 // Class ConcaveMeshShape
 /**
  * This class represents a concave mesh shape. Note that collision detection
@@ -44,6 +46,11 @@ class ConcaveMeshShape : public ConcaveShape {
 
     protected:
 
+        // -------------------- Attributes -------------------- //
+
+        /// Triangle mesh
+        TriangleMesh* mTriangleMesh;
+
         // -------------------- Methods -------------------- //
 
         /// Private copy-constructor
@@ -86,8 +93,8 @@ class ConcaveMeshShape : public ConcaveShape {
         /// Update the AABB of a body using its collision shape
         virtual void computeAABB(AABB& aabb, const Transform& transform);
 
-        /// Test equality between two sphere shapes
+        /// Use a callback method on all triangles of the concave shape inside a given AABB
-        virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const;
+        virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const=0;
 };
 
 // Return the number of bytes used by the collision shape

src/collision/shapes/ConcaveShape.h

@@ -29,9 +29,24 @@
 // Libraries
 #include "CollisionShape.h"
 
-/// ReactPhysics3D namespace
+// ReactPhysics3D namespace
 namespace reactphysics3d {
 
+// Class TriangleCallback
+/**
+ * This class is used to encapsulate a callback method for
+ * a single triangle of a ConcaveMesh.
+ */
+class TriangleCallback {
+
+    public:
+
+        /// Report a triangle
+        virtual void reportTriangle(const Vector3* trianglePoints)=0;
+
+};
+
+
 // Class ConcaveShape
 /**
  * This abstract class represents a concave collision shape associated with a
@@ -65,6 +80,9 @@ class ConcaveShape : public CollisionShape {
 
         /// Return true if the collision shape is convex, false if it is concave
         virtual bool isConvex() const;
+
+        /// Use a callback method on all triangles of the concave shape inside a given AABB
+        virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const=0;
 };
 
 /// Return true if the collision shape is convex, false if it is concave

src/collision/shapes/ConvexMeshShape.h

@@ -44,6 +44,8 @@ class CollisionWorld;
 // TODO : Make possible to create a ConvexMeshShape using a TriangleMesh as for
 // the ConcaveMeshShape
 
+// TODO : Make possible for the user to have a scaling factor on the mesh
+
 // Class ConvexMeshShape
 /**
  * This class represents a convex mesh shape. In order to create a convex mesh shape, you

src/constraint/ContactPoint.h

@@ -28,6 +28,7 @@
 
 // Libraries
 #include "body/CollisionBody.h"
+#include "collision/CollisionShapeInfo.h"
 #include "configuration.h"
 #include "mathematics/mathematics.h"
 #include "configuration.h"
@@ -58,10 +59,10 @@ struct ContactPointInfo {
 
         // -------------------- Attributes -------------------- //
 
-        /// First proxy collision shape of the contact
+        /// First proxy shape of the contact
         ProxyShape* shape1;
 
-        /// Second proxy collision shape of the contact
+        /// Second proxy shape of the contact
         ProxyShape* shape2;
 
         /// Normal vector the the collision contact in world space
@@ -79,8 +80,8 @@ struct ContactPointInfo {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ContactPointInfo(ProxyShape* proxyShape1, ProxyShape* proxyShape2, const Vector3& normal,
+        ContactPointInfo(ProxyShape* proxyShape1, ProxyShape* proxyShape2,
-                         decimal penetrationDepth, const Vector3& localPoint1,
+                         const Vector3& normal, decimal penetrationDepth, const Vector3& localPoint1,
                          const Vector3& localPoint2)
             : shape1(proxyShape1), shape2(proxyShape2), normal(normal),
               penetrationDepth(penetrationDepth), localPoint1(localPoint1),