Working on ConcaveMeshShape and HeightFieldShape collision detection

CMakeLists.txt

@@ -137,6 +137,8 @@ SET (REACTPHYSICS3D_SOURCES
     "src/collision/ContactManifold.cpp"
     "src/collision/ContactManifoldSet.h"
     "src/collision/ContactManifoldSet.cpp"
+    "src/collision/MiddlePhaseTriangleCallback.h"
+    "src/collision/MiddlePhaseTriangleCallback.cpp"
     "src/constraint/BallAndSocketJoint.h"
     "src/constraint/BallAndSocketJoint.cpp"
     "src/constraint/ContactPoint.h"

src/collision/CollisionDetection.cpp

@@ -29,9 +29,11 @@
 #include "collision/OverlapCallback.h"
 #include "body/Body.h"
 #include "collision/shapes/BoxShape.h"
+#include "collision/shapes/ConcaveShape.h"
 #include "body/RigidBody.h"
 #include "configuration.h"
 #include "collision/CollisionCallback.h"
+#include "collision/MiddlePhaseTriangleCallback.h"
 #include "collision/OverlapCallback.h"
 #include <cassert>
 #include <complex>

src/collision/ContactManifold.h

@@ -80,9 +80,14 @@ struct ContactManifoldListElement {
 
 // Class ContactManifold
 /**
- * This class represents the set of contact points between two bodies.
+ * This class represents a set of contact points between two bodies that
+ * all have a similar contact normal direction. Usually, there is a single
+ * contact manifold when two convex shapes are in contact. However, when
+ * a convex shape collides with a concave shape, there can be several
+ * contact manifolds with different normal directions.
  * The contact manifold is implemented in a way to cache the contact
- * points among the frames for better stability.
+ * points among the frames for better stability (warm starting of the
+ * contact solver)
  */
 class ContactManifold {
 

src/collision/MiddlePhaseTriangleCallback.cpp

@@ -0,0 +1,49 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 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.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "collision/MiddlePhaseTriangleCallback.h"
+
+using namespace reactphysics3d;
+
+// Report collision between a triangle of a concave shape and the convex mesh shape (for middle-phase)
+void MiddlePhaseTriangleCallback::testTriangle(uint meshSubPart, uint triangleIndex, const Vector3* trianglePoints,
+                                               const Vector3* verticesNormals) {
+
+    // Create a triangle collision shape
+    decimal margin = mConcaveShape->getTriangleMargin();
+    TriangleShape* triangleShape = new (mAllocator.allocate(sizeof(TriangleShape)))
+                                   TriangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2],
+                                                 verticesNormals, meshSubPart, triangleIndex, margin);
+
+    // Create a narrow phase info for the narrow-phase collision detection
+    NarrowPhaseInfo* firstNarrowPhaseInfo = narrowPhaseInfoList;
+    narrowPhaseInfoList = new (mAllocator.allocate(sizeof(NarrowPhaseInfo)))
+                           NarrowPhaseInfo(mOverlappingPair, mConvexProxyShape->getCollisionShape(),
+                           triangleShape, mConvexProxyShape->getLocalToWorldTransform(),
+                           mConcaveProxyShape->getLocalToWorldTransform(), mConvexProxyShape->getCachedCollisionData(),
+                           mConcaveProxyShape->getCachedCollisionData());
+    narrowPhaseInfoList->next = firstNarrowPhaseInfo;
+}

src/collision/MiddlePhaseTriangleCallback.h

@@ -0,0 +1,84 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 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_MIDDLE_PHASE_TRIANGLE_CALLBACK_H
+#define REACTPHYSICS3D_MIDDLE_PHASE_TRIANGLE_CALLBACK_H
+
+// Libraries
+#include "collision/shapes/ConcaveShape.h"
+#include "collision/narrowphase/NarrowPhaseAlgorithm.h"
+
+/// Namespace ReactPhysics3D
+namespace reactphysics3d {
+
+// Class ConvexVsTriangleCallback
+/**
+ * This class is used to report a collision between the triangle
+ * of a concave mesh shape and a convex shape during the
+ * middle-phase algorithm.
+ */
+class MiddlePhaseTriangleCallback : public TriangleCallback {
+
+    protected:
+
+        /// Broadphase overlapping pair
+        OverlappingPair* mOverlappingPair;
+
+        /// Pointer to the concave proxy shape
+        ProxyShape* mConcaveProxyShape;
+
+        /// Pointer to the convex proxy shape
+        ProxyShape* mConvexProxyShape;
+
+        /// Pointer to the concave collision shape
+        const ConcaveShape* mConcaveShape;
+
+        /// Reference to the single-frame memory allocator
+        Allocator& mAllocator;
+
+    public:
+
+        /// Pointer to the first element of the linked-list of narrow-phase info
+        NarrowPhaseInfo* narrowPhaseInfoList;
+
+        /// Constructor
+        MiddlePhaseTriangleCallback(OverlappingPair* overlappingPair,
+                                    ProxyShape* concaveProxyShape,
+                                    ProxyShape* convexProxyShape, const ConcaveShape* concaveShape,
+                                    Allocator& allocator)
+            :mOverlappingPair(overlappingPair), mConcaveProxyShape(concaveProxyShape),
+             mConvexProxyShape(convexProxyShape), mConcaveShape(concaveShape),
+             mAllocator(allocator), narrowPhaseInfoList(nullptr) {
+
+        }
+
+        /// Test collision between a triangle and the convex mesh shape
+        virtual void testTriangle(uint meshSubpart, uint triangleIndex, const Vector3* trianglePoints,
+                                  const Vector3* verticesNormals) override;
+};
+
+}
+
+#endif

src/collision/PolyhedronMesh.cpp

@@ -30,6 +30,10 @@ using namespace reactphysics3d;
 
 
 // Constructor
+/*
+ * Create a polyhedron mesh given an array of polygons.
+ * @param polygonVertexArray Pointer to the array of polygons and their vertices
+ */
 PolyhedronMesh::PolyhedronMesh(PolygonVertexArray* polygonVertexArray) {
 
    mPolygonVertexArray = polygonVertexArray;

src/collision/TriangleVertexArray.cpp

@@ -25,13 +25,19 @@
 
 // Libraries
 #include "TriangleVertexArray.h"
+#include "mathematics/Vector3.h"
+#include <cassert>
 
 using namespace reactphysics3d;
 
-// Constructor
+// Constructor without vertices normals
 /// Note that your data will not be copied into the TriangleVertexArray and
 /// therefore, you need to make sure that those data are always valid during
-/// the lifetime of the TriangleVertexArray.
+/// the lifetime of the TriangleVertexArray. With this constructor, you do not
+/// need to provide vertices normals for smooth mesh collision. Therefore, the
+/// vertices normals will be computed automatically. The vertices normals are
+/// computed with weighted average of the associated triangle face normal. The
+/// weights are the angle between the associated edges of neighbor triangle face.
 /**
  * @param nbVertices Number of vertices in the array
  * @param verticesStart Pointer to the first vertices of the array
@@ -48,9 +54,237 @@ TriangleVertexArray::TriangleVertexArray(uint nbVertices, void* verticesStart, i
     mNbVertices = nbVertices;
     mVerticesStart = reinterpret_cast<unsigned char*>(verticesStart);
     mVerticesStride = verticesStride;
+    mVerticesNormalsStart = nullptr;
+    mVerticesNormalsStride = 0;
     mNbTriangles = nbTriangles;
     mIndicesStart = reinterpret_cast<unsigned char*>(indexesStart);
     mIndicesStride = indexesStride;
     mVertexDataType = vertexDataType;
+    mVertexNormaldDataType = NormalDataType::NORMAL_FLOAT_TYPE;
     mIndexDataType = indexDataType;
+    mAreVerticesNormalsProvidedByUser = false;
+
+    // Compute the vertices normals because they are not provided by the user
+    computeVerticesNormals();
+}
+
+// Constructor with vertices normals
+/// Note that your data will not be copied into the TriangleVertexArray and
+/// therefore, you need to make sure that those data are always valid during
+/// the lifetime of the TriangleVertexArray. With this constructor, you need
+/// to provide the vertices normals that will be used for smooth mesh collision.
+/**
+ * @param nbVertices Number of vertices in the array
+ * @param verticesStart Pointer to the first vertices of the array
+ * @param verticesStride Number of bytes between the beginning of two consecutive vertices
+ * @param verticesNormalsStart Pointer to the first vertex normal of the array
+ * @param verticesNormalsStride Number of bytes between the beginning of two consecutive vertex normals
+ * @param nbTriangles Number of triangles in the array
+ * @param indexesStart Pointer to the first triangle index
+ * @param indexesStride Number of bytes between the beginning of two consecutive triangle indices
+ * @param vertexDataType Type of data for the vertices (float, double)
+ * @param indexDataType Type of data for the indices (short, int)
+ */
+TriangleVertexArray::TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride,
+                                         void* verticesNormalsStart, int verticesNormalsStride,
+                                         uint nbTriangles, void* indexesStart, int indexesStride,
+                                         VertexDataType vertexDataType, NormalDataType normalDataType,
+                                         IndexDataType indexDataType) {
+
+    mNbVertices = nbVertices;
+    mVerticesStart = reinterpret_cast<unsigned char*>(verticesStart);
+    mVerticesStride = verticesStride;
+    mVerticesNormalsStart = reinterpret_cast<unsigned char*>(verticesNormalsStart);
+    mVerticesNormalsStride = verticesNormalsStride;
+    mNbTriangles = nbTriangles;
+    mIndicesStart = reinterpret_cast<unsigned char*>(indexesStart);
+    mIndicesStride = indexesStride;
+    mVertexDataType = vertexDataType;
+    mVertexNormaldDataType = normalDataType;
+    mIndexDataType = indexDataType;
+    mAreVerticesNormalsProvidedByUser = true;
+
+    assert(mVerticesNormalsStart != nullptr);
+}
+
+// Destructor
+TriangleVertexArray::~TriangleVertexArray() {
+
+    // If the vertices normals have not been provided by the user
+    if (!mAreVerticesNormalsProvidedByUser) {
+
+        // Release the allocated memory
+        float* verticesNormals = reinterpret_cast<float*>(mVerticesNormalsStart);
+        delete[] verticesNormals;
+    }
+}
+
+// Compute the vertices normals when they are not provided by the user
+/// The vertices normals are computed with weighted average of the associated
+/// triangle face normal. The weights are the angle between the associated edges
+/// of neighbor triangle face.
+void TriangleVertexArray::computeVerticesNormals() {
+
+    // Allocate memory for the vertices normals
+    float* verticesNormals = new float[mNbVertices * 3];
+
+    // Init vertices normals to zero
+    for (uint i=0; i<mNbVertices * 3; i++) {
+        verticesNormals[i] = 0.0f;
+    }
+
+    // For each triangle face in the array
+    for (uint f=0; f < mNbTriangles; f++) {
+
+        // Get the indices of the three vertices of the triangle in the array
+        uint verticesIndices[3];
+        getTriangleVerticesIndices(f, verticesIndices);
+
+        // Get the triangle vertices
+        Vector3 triangleVertices[3];
+        getTriangleVertices(f, triangleVertices);
+
+        // Edges lengths
+        decimal edgesLengths[3];
+        edgesLengths[0] = (triangleVertices[1] - triangleVertices[0]).length();
+        edgesLengths[1] = (triangleVertices[2] - triangleVertices[1]).length();
+        edgesLengths[2] = (triangleVertices[0] - triangleVertices[2]).length();
+
+        // For each vertex of the face
+        for (uint v=0; v < 3; v++) {
+
+            uint previousVertex = (v == 0) ? 2 : v-1;
+            uint nextVertex = (v == 2) ? 0 : v+1;
+            Vector3 a = triangleVertices[nextVertex] - triangleVertices[v];
+            Vector3 b = triangleVertices[previousVertex] - triangleVertices[v];
+
+            Vector3 crossProduct = a.cross(b);
+            decimal sinA = crossProduct.length() / (edgesLengths[previousVertex] * edgesLengths[v]);
+            Vector3 normalComponent = std::asin(sinA) * crossProduct;
+
+            // Add the normal component of this vertex into the normals array
+            verticesNormals[verticesIndices[v]] = normalComponent.x;
+            verticesNormals[verticesIndices[v] + 1] = normalComponent.y;
+            verticesNormals[verticesIndices[v] + 2] = normalComponent.z;
+        }
+    }
+
+    // Normalize the computed vertices normals
+    for (uint v=0; v<mNbVertices * 3; v += 3) {
+
+        // Normalize the normal
+        Vector3 normal(verticesNormals[v], verticesNormals[v + 1], verticesNormals[v + 2]);
+        normal.normalize();
+
+        verticesNormals[v] = normal.x;
+        verticesNormals[v + 1] = normal.y;
+        verticesNormals[v + 2] = normal.z;
+    }
+
+    mVerticesNormalsStart = reinterpret_cast<unsigned char*>(verticesNormals);
+}
+
+// Return the indices of the three vertices of a given triangle in the array
+void TriangleVertexArray::getTriangleVerticesIndices(uint triangleIndex, uint* outVerticesIndices) const {
+
+    assert(triangleIndex >= 0 && triangleIndex < mNbTriangles);
+
+    void* vertexIndexPointer = (mIndicesStart + triangleIndex * 3 * mIndicesStride);
+
+    // For each vertex of the triangle
+    for (int i=0; i < 3; i++) {
+
+        // Get the index of the current vertex in the triangle
+        if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE) {
+            outVerticesIndices[i] = ((uint*)vertexIndexPointer)[i];
+        }
+        else if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE) {
+            outVerticesIndices[i] = ((unsigned short*)vertexIndexPointer)[i];
+        }
+        else {
+            assert(false);
+        }
+    }
+}
+
+// Return the vertices coordinates of a triangle
+void TriangleVertexArray::getTriangleVertices(uint triangleIndex, Vector3* outTriangleVertices) const {
+
+    assert(triangleIndex >= 0 && triangleIndex < mNbTriangles);
+
+    void* vertexIndexPointer = (mIndicesStart + triangleIndex * 3 * mIndicesStride);
+
+    // 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 = 0;
+        if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE) {
+            vertexIndex = ((uint*)vertexIndexPointer)[k];
+        }
+        else if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE) {
+            vertexIndex = ((unsigned short*)vertexIndexPointer)[k];
+        }
+        else {
+            assert(false);
+        }
+
+        // Get the vertices components of the triangle
+        if (mVertexDataType == TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE) {
+            const float* vertices = (float*)(mVerticesStart + vertexIndex * mVerticesStride);
+            outTriangleVertices[k][0] = decimal(vertices[0]);
+            outTriangleVertices[k][1] = decimal(vertices[1]);
+            outTriangleVertices[k][2] = decimal(vertices[2]);
+        }
+        else if (mVertexDataType == TriangleVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE) {
+            const double* vertices = (double*)(mVerticesStart + vertexIndex * mVerticesStride);
+            outTriangleVertices[k][0] = decimal(vertices[0]);
+            outTriangleVertices[k][1] = decimal(vertices[1]);
+            outTriangleVertices[k][2] = decimal(vertices[2]);
+        }
+        else {
+            assert(false);
+        }
+    }
+}
+
+// Return the three vertices normals of a triangle
+void TriangleVertexArray::getTriangleVerticesNormals(uint triangleIndex, Vector3* outTriangleVerticesNormals) const {
+
+    assert(triangleIndex >= 0 && triangleIndex < mNbTriangles);
+
+    void* vertexIndexPointer = (mIndicesStart + triangleIndex * 3 * mIndicesStride);
+
+    // 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 = 0;
+        if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE) {
+            vertexIndex = ((uint*)vertexIndexPointer)[k];
+        }
+        else if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE) {
+            vertexIndex = ((unsigned short*)vertexIndexPointer)[k];
+        }
+        else {
+            assert(false);
+        }
+
+        // Get the normals from the array
+        if (mVertexNormaldDataType == TriangleVertexArray::NormalDataType::NORMAL_FLOAT_TYPE) {
+            const float* normal = (float*)(mVerticesNormalsStart + vertexIndex * mVerticesNormalsStride);
+            outTriangleVerticesNormals[k][0] = decimal(normal[0]);
+            outTriangleVerticesNormals[k][1] = decimal(normal[1]);
+            outTriangleVerticesNormals[k][2] = decimal(normal[2]);
+        }
+        else if (mVertexNormaldDataType == TriangleVertexArray::NormalDataType::NORMAL_DOUBLE_TYPE) {
+            const double* normal = (double*)(mVerticesNormalsStart + vertexIndex * mVerticesNormalsStride);
+            outTriangleVerticesNormals[k][0] = decimal(normal[0]);
+            outTriangleVerticesNormals[k][1] = decimal(normal[1]);
+            outTriangleVerticesNormals[k][2] = decimal(normal[2]);
+        }
+        else {
+            assert(false);
+        }
+    }
 }

src/collision/TriangleVertexArray.h

@@ -31,6 +31,8 @@
 
 namespace reactphysics3d {
 
+struct Vector3;
+
 // Class TriangleVertexArray
 /**
  * This class is used to describe the vertices and faces of a triangular mesh.
@@ -48,11 +50,16 @@ class TriangleVertexArray {
         /// Data type for the vertices in the array
         enum class VertexDataType {VERTEX_FLOAT_TYPE, VERTEX_DOUBLE_TYPE};
 
+        /// Data type for the vertex normals in the array
+        enum class NormalDataType {NORMAL_FLOAT_TYPE, NORMAL_DOUBLE_TYPE};
+
         /// Data type for the indices in the array
         enum class IndexDataType {INDEX_INTEGER_TYPE, INDEX_SHORT_TYPE};
 
     protected:
 
+        // -------------------- Attributes -------------------- //
+
         /// Number of vertices in the array
         uint mNbVertices;
 
@@ -63,6 +70,13 @@ class TriangleVertexArray {
         /// values in the array
         int mVerticesStride;
 
+        /// Pointer to the first vertex normal value in the array
+        unsigned char* mVerticesNormalsStart;
+
+        /// Stride (number of bytes) between the beginning of two vertex normals
+        /// values in the array
+        int mVerticesNormalsStride;
+
         /// Number of triangles in the array
         uint mNbTriangles;
 
@@ -76,22 +90,45 @@ class TriangleVertexArray {
         /// Data type of the vertices in the array
         VertexDataType mVertexDataType;
 
+        /// Data type of the vertex normals in the array
+        NormalDataType mVertexNormaldDataType;
+
         /// Data type of the indices in the array
         IndexDataType mIndexDataType;
 
+        /// True if the vertices normals are provided by the user
+        bool mAreVerticesNormalsProvidedByUser;
+
+        // -------------------- Methods -------------------- //
+
+        /// Compute the vertices normals when they are not provided by the user
+        void computeVerticesNormals();
+
     public:
 
-        /// Constructor
+        // -------------------- Methods -------------------- //
+
+        /// Constructor without vertices normals
         TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride,
                             uint nbTriangles, void* indexesStart, int indexesStride,
                             VertexDataType vertexDataType, IndexDataType indexDataType);
 
+        /// Constructor with vertices normals
+        TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride,
+                            void* verticesNormalsStart, int verticesNormalsStride,
+                            uint nbTriangles, void* indexesStart, int indexesStride,
+                            VertexDataType vertexDataType, NormalDataType normalDataType,
+                            IndexDataType indexDataType);
+
         /// Destructor
-        ~TriangleVertexArray() = default;
+        ~TriangleVertexArray();
 
         /// Return the vertex data type
         VertexDataType getVertexDataType() const;
 
+        /// Return the vertex normal data type
+        NormalDataType getVertexNormalDataType() const;
+
         /// Return the index data type
         IndexDataType getIndexDataType() const;
 
@@ -104,14 +141,29 @@ class TriangleVertexArray {
         /// Return the vertices stride (number of bytes)
         int getVerticesStride() const;
 
+        /// Return the vertex normals stride (number of bytes)
+        int getVerticesNormlasStride() 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 vertex normals array
+        unsigned char* getVerticesNormalsStart() const;
+
         /// Return the pointer to the start of the indices array
         unsigned char* getIndicesStart() const;
+
+        /// Return the vertices coordinates of a triangle
+        void getTriangleVertices(uint triangleIndex, Vector3* outTriangleVertices) const;
+
+        /// Return the three vertices normals of a triangle
+        void getTriangleVerticesNormals(uint triangleIndex, Vector3* outTriangleVerticesNormals) const;
+
+        /// Return the indices of the three vertices of a given triangle in the array
+        void getTriangleVerticesIndices(uint triangleIndex, uint* outVerticesIndices) const;
 };
 
 // Return the vertex data type
@@ -119,6 +171,11 @@ inline TriangleVertexArray::VertexDataType TriangleVertexArray::getVertexDataTyp
     return mVertexDataType;
 }
 
+// Return the vertex normal data type
+inline TriangleVertexArray::NormalDataType TriangleVertexArray::getVertexNormalDataType() const {
+    return mVertexNormaldDataType;
+}
+
 // Return the index data type
 inline TriangleVertexArray::IndexDataType TriangleVertexArray::getIndexDataType() const {
    return mIndexDataType;
@@ -139,6 +196,11 @@ inline int TriangleVertexArray::getVerticesStride() const {
     return mVerticesStride;
 }
 
+// Return the vertex normals stride (number of bytes)
+inline int TriangleVertexArray::getVerticesNormlasStride() const {
+    return mVerticesNormalsStride;
+}
+
 // Return the indices stride (number of bytes)
 inline int TriangleVertexArray::getIndicesStride() const {
     return mIndicesStride;
@@ -149,6 +211,11 @@ inline unsigned char* TriangleVertexArray::getVerticesStart() const {
     return mVerticesStart;
 }
 
+// Return the pointer to the start of the vertex normals array
+inline unsigned char* TriangleVertexArray::getVerticesNormalsStart() const {
+    return mVerticesNormalsStart;
+}
+
 // Return the pointer to the start of the indices array
 inline unsigned char* TriangleVertexArray::getIndicesStart() const {
     return mIndicesStart;

src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp

@@ -82,7 +82,7 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh
 
                 // Get the face normal
                 const Vector3 faceNormal = polyhedron->getFaceNormal(f);
-                const Vector3 faceNormalWorld = polyhedronToWorld.getOrientation() * faceNormal;
+                Vector3 faceNormalWorld = polyhedronToWorld.getOrientation() * faceNormal;
 
                 const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0);
                 const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0);

src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp

@@ -22,7 +22,7 @@
 * 3. This notice may not be removed or altered from any source distribution.    *
 *                                                                               *
 ********************************************************************************/
-
+/*
 // Libraries
 #include "collision/shapes/ConcaveShape.h"
 #include "collision/shapes/TriangleShape.h"
@@ -34,12 +34,14 @@
 using namespace reactphysics3d;
 
 // Report collision between a triangle of a concave shape and the convex mesh shape (for middle-phase)
-void MiddlePhaseTriangleCallback::testTriangle(const Vector3* trianglePoints) {
+void MiddlePhaseTriangleCallback::testTriangle(uint meshSubPart, uint triangleIndex, const Vector3* trianglePoints,
+                                               const Vector3* verticesNormals) {
 
     // Create a triangle collision shape
     decimal margin = mConcaveShape->getTriangleMargin();
     TriangleShape* triangleShape = new (mAllocator.allocate(sizeof(TriangleShape)))
-                                   TriangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
+                                   TriangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2],
+                                                 verticesNormals, meshSubPart, triangleIndex, margin);
 
     // Create a narrow phase info for the narrow-phase collision detection
     NarrowPhaseInfo* firstNarrowPhaseInfo = narrowPhaseInfoList;
@@ -297,3 +299,4 @@ bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const std::unordered_multi
 //    // smooth mesh collision
 //    mContactPoints.push_back(smoothContactInfo);
 //}
+*/

src/collision/narrowphase/ConcaveVsConvexAlgorithm.h

@@ -22,7 +22,7 @@
 * 3. This notice may not be removed or altered from any source distribution.    *
 *                                                                               *
 ********************************************************************************/
-
+/*
 #ifndef REACTPHYSICS3D_CONCAVE_VS_CONVEX_ALGORITHM_H
 #define	REACTPHYSICS3D_CONCAVE_VS_CONVEX_ALGORITHM_H
 
@@ -37,11 +37,6 @@
 namespace reactphysics3d {
 
 // Class ConvexVsTriangleCallback
-/**
- * This class is used to report a collision between the triangle
- * of a concave mesh shape and a convex shape during the
- * middle-phase algorithm
- */
 class MiddlePhaseTriangleCallback : public TriangleCallback {
 
     protected:
@@ -78,14 +73,11 @@ class MiddlePhaseTriangleCallback : public TriangleCallback {
         }
 
         /// Test collision between a triangle and the convex mesh shape
-        virtual void testTriangle(const Vector3* trianglePoints) override;
+        virtual void testTriangle(uint meshSubpart, uint triangleIndex, const Vector3* trianglePoints,
+                                  const Vector3* verticesNormals) override;
 };
 
 // Class SmoothMeshContactInfo
-/**
- * Contains data for of potential smooth contact during the smooth mesh
- * contacts computation.
- */
 struct SmoothMeshContactInfo {
 
     public:
@@ -131,11 +123,6 @@ struct ContactsDepthCompare {
 
 // TODO : Delete this
 // Class SmoothCollisionNarrowPhaseCallback
-/**
- * This class is used as a narrow-phase callback to get narrow-phase contacts
- * of the concave triangle mesh to temporary store them in order to be used in
- * the smooth mesh collision algorithm if this one is enabled.
- */
 class SmoothCollisionNarrowPhaseCallback {
 
     private:
@@ -155,12 +142,6 @@ class SmoothCollisionNarrowPhaseCallback {
 
 // TODO : Delete this
 // Class ConcaveVsConvexAlgorithm
-/**
- * This class is used to compute the narrow-phase collision detection
- * between a concave collision shape and a convex collision shape. The idea is
- * to use the GJK collision detection algorithm to compute the collision between
- * the convex shape and each of the triangles in the concave shape.
- */
 class ConcaveVsConvexAlgorithm {
 
     protected :
@@ -212,3 +193,4 @@ inline void ConcaveVsConvexAlgorithm::addProcessedVertex(std::unordered_multimap
 
 #endif
 
+*/

src/collision/narrowphase/GJK/GJKAlgorithm.cpp

@@ -27,6 +27,7 @@
 #include "GJKAlgorithm.h"
 #include "constraint/ContactPoint.h"
 #include "engine/OverlappingPair.h"
+#include "collision/shapes/TriangleShape.h"
 #include "configuration.h"
 #include "engine/Profiler.h"
 #include <algorithm>
@@ -76,7 +77,8 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
 
     // 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)
-    Transform body2Tobody1 = transform1.getInverse() * transform2;
+    Transform transform1Inverse = transform1.getInverse();
+    Transform body2Tobody1 = transform1Inverse * transform2;
 
     // Matrix that transform a direction from local
     // space of body 1 into local space of body 2
@@ -209,6 +211,10 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
 
         if (reportContacts) {
 
+            // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
+            TriangleShape::computeSmoothTriangleMeshContact(shape1, shape2, pA, pB, transform1, transform2,
+                                                            penetrationDepth, normal);
+
             // Add a new contact point
             narrowPhaseInfo->addContactPoint(normal, penetrationDepth, pA, pB);
         }
@@ -219,6 +225,7 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
     return GJKResult::SEPARATED;
 }
 
+
 // Use the GJK Algorithm to find if a point is inside a convex collision shape
 bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) {
 

src/collision/narrowphase/SAT/SATAlgorithm.cpp

@@ -30,6 +30,7 @@
 #include "collision/shapes/CapsuleShape.h"
 #include "collision/shapes/SphereShape.h"
 #include "engine/OverlappingPair.h"
+#include "collision/shapes/TriangleShape.h"
 #include "configuration.h"
 #include "engine/Profiler.h"
 #include <algorithm>
@@ -143,13 +144,21 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrow
 
         const Vector3 minFaceNormal = polyhedron->getFaceNormal(minFaceIndex);
         Vector3 normalWorld = -(polyhedronToWorldTransform.getOrientation() * minFaceNormal);
-        const Vector3 contactPointSphereLocal = sphereToWorldTransform.getInverse().getOrientation() * normalWorld * sphere->getRadius();
-        const Vector3 contactPointPolyhedronLocal = sphereCenter + minFaceNormal * (minPenetrationDepth - sphere->getRadius());
+        Vector3 contactPointSphereLocal = sphereToWorldTransform.getInverse().getOrientation() * normalWorld * sphere->getRadius();
+        Vector3 contactPointPolyhedronLocal = sphereCenter + minFaceNormal * (minPenetrationDepth - sphere->getRadius());
+
+        // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
+        TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2,
+                                                        isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal,
+                                                        isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal,
+                                                        narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform,
+                                                        minPenetrationDepth, normalWorld);
+
 
         // Create the contact info object
         narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
-                                            isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal,
-                                            isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal);
+                                         isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal,
+                                         isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal);
     }
 
     lastFrameInfo.satMinAxisFaceIndex = minFaceIndex;
@@ -380,7 +389,7 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro
     const Vector3 capsuleSegAPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegA;
     const Vector3 capsuleSegBPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegB;
 
-    const Vector3 normalWorld = capsuleToWorld.getOrientation() * separatingAxisCapsuleSpace;
+    Vector3 normalWorld = capsuleToWorld.getOrientation() * separatingAxisCapsuleSpace;
     const decimal capsuleRadius = capsuleShape->getRadius();
 
     // If the separating axis is a face normal
@@ -410,7 +419,14 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro
                                                   closestPointCapsuleInnerSegment, closestPointPolyhedronEdge);
 
             // Project closest capsule inner segment point into the capsule bounds
-            const Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * closestPointCapsuleInnerSegment) - separatingAxisCapsuleSpace * capsuleRadius;
+            Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * closestPointCapsuleInnerSegment) - separatingAxisCapsuleSpace * capsuleRadius;
+
+            // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
+            TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2,
+                                                        isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge,
+                                                        isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule,
+                                                        narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform,
+                                                        minPenetrationDepth, normalWorld);
 
             // Create the contact point
             narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
@@ -483,7 +499,7 @@ decimal SATAlgorithm::computePolyhedronFaceVsCapsulePenetrationDepth(uint polyhe
 // axis is a face normal of the polyhedron
 void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron,
                                                              decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform,
-                                                             const Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace,
+                                                             Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace,
                                                              const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace,
                                                              NarrowPhaseInfo* narrowPhaseInfo, bool isCapsuleShape1) const {
 
@@ -525,15 +541,27 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
 		// If the clipped point is one that produce this penetration depth, we keep it
 		if (clipPointPenDepth > penetrationDepth - capsuleRadius - decimal(0.001)) {
 
-			const Vector3 contactPointPolyhedron = clipSegment[i] + delta;
+            Vector3 contactPointPolyhedron = clipSegment[i] + delta;
 
 			// Project the clipped point into the capsule bounds
-			const Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * clipSegment[i]) - separatingAxisCapsuleSpace * capsuleRadius;
+            Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * clipSegment[i]) - separatingAxisCapsuleSpace * capsuleRadius;
+
+            if (isCapsuleShape1) {
+                normalWorld = -normalWorld;
+            }
+
+            // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
+            TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2,
+                                                        isCapsuleShape1 ? contactPointCapsule : contactPointPolyhedron,
+                                                        isCapsuleShape1 ? contactPointPolyhedron : contactPointCapsule,
+                                                        narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform,
+                                                        penetrationDepth, normalWorld);
+
 
 			// Create the contact point
-            narrowPhaseInfo->addContactPoint(isCapsuleShape1 ? -normalWorld : normalWorld, penetrationDepth,
-				isCapsuleShape1 ? contactPointCapsule : contactPointPolyhedron,
-				isCapsuleShape1 ? contactPointPolyhedron : contactPointCapsule);
+            narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth,
+                                             isCapsuleShape1 ? contactPointCapsule : contactPointPolyhedron,
+                                             isCapsuleShape1 ? contactPointPolyhedron : contactPointCapsule);
 		}
 	}
 }
@@ -807,7 +835,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
             const Vector3 axisIncidentSpace = referenceToIncidentTransform.getOrientation() * axisReferenceSpace;
 
             // Compute the world normal
-            const Vector3 normalWorld = isMinPenetrationFaceNormalPolyhedron1 ? narrowPhaseInfo->shape1ToWorldTransform.getOrientation() * axisReferenceSpace :
+            Vector3 normalWorld = isMinPenetrationFaceNormalPolyhedron1 ? narrowPhaseInfo->shape1ToWorldTransform.getOrientation() * axisReferenceSpace :
                                                                                 -(narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * axisReferenceSpace);
 
             // Get the reference face
@@ -874,11 +902,18 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
                 if (((*itPoints) - referenceFaceVertex).dot(axisReferenceSpace) < decimal(0.0)) {
 
                     // Convert the clip incident polyhedron vertex into the incident polyhedron local-space
-                    const Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * (*itPoints);
+                    Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * (*itPoints);
 
                     // Project the contact point onto the reference face
                     Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(*itPoints, axisReferenceSpace, referenceFaceVertex);
 
+                    // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
+                    TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2,
+                                                                    isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron,
+                                                                    isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron,
+                                                                    narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform,
+                                                                    minPenetrationDepth, normalWorld);
+
                     // Create a new contact point
                     narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
                                                      isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron,
@@ -901,14 +936,20 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
                                                   closestPointPolyhedron1Edge, closestPointPolyhedron2Edge);
 
             // Compute the contact point on polyhedron 1 edge in the local-space of polyhedron 1
-            const Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge;
+            Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge;
 
             // Compute the world normal
-            const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
+            Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
+
+            // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
+            TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2,
+                                                            closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge,
+                                                            narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform,
+                                                            minPenetrationDepth, normalWorld);
 
             // Create the contact point
             narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
-                                                closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge);
+                                             closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge);
         }
 
         lastFrameInfo.satIsAxisFacePolyhedron1 = false;

src/collision/narrowphase/SAT/SATAlgorithm.h

@@ -121,7 +121,7 @@ class SATAlgorithm {
         /// Compute the two contact points between a polyhedron and a capsule when the separating axis is a face normal of the polyhedron
         void computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron,
                                                        decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform,
-                                                       const Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace,
+                                                       Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace,
                                                        const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace,
                                                        NarrowPhaseInfo* narrowPhaseInfo, bool isCapsuleShape1) const;
 

src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp

@@ -36,15 +36,15 @@ using namespace reactphysics3d;
 // by Dirk Gregorius.
 bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
 
+    assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
+        narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
+    assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE ||
+        narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE);
+
     // First, we run the GJK algorithm
     GJKAlgorithm gjkAlgorithm;
     GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts);
 
-	assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
-		narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
-	assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE ||
-		narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE);
-
     narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true;
     narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false;
 

src/collision/shapes/ConcaveMeshShape.cpp

@@ -49,51 +49,13 @@ void ConcaveMeshShape::initBVHTree() {
         // Get the triangle vertex array of the current sub-part
         TriangleVertexArray* triangleVertexArray = mTriangleMesh->getSubpart(subPart);
 
-        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 (uint triangleIndex=0; triangleIndex<triangleVertexArray->getNbTriangles(); triangleIndex++) {
 
-            void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride);
             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 = 0;
-                if (indexType == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE) {
-                    vertexIndex = ((uint*)vertexIndexPointer)[k];
-                }
-                else if (indexType == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE) {
-                    vertexIndex = ((unsigned short*)vertexIndexPointer)[k];
-                }
-                else {
-                    assert(false);
-                }
-
-                // Get the vertices components of the triangle
-                if (vertexType == TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE) {
-                    const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride);
-                    trianglePoints[k][0] = decimal(vertices[0]) * mScaling.x;
-                    trianglePoints[k][1] = decimal(vertices[1]) * mScaling.y;
-                    trianglePoints[k][2] = decimal(vertices[2]) * mScaling.z;
-                }
-                else if (vertexType == TriangleVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE) {
-                    const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride);
-                    trianglePoints[k][0] = decimal(vertices[0]) * mScaling.x;
-                    trianglePoints[k][1] = decimal(vertices[1]) * mScaling.y;
-                    trianglePoints[k][2] = decimal(vertices[2]) * mScaling.z;
-                }
-                else {
-                    assert(false);
-                }
-            }
+            // Get the triangle vertices
+            triangleVertexArray->getTriangleVertices(triangleIndex, trianglePoints);
 
             // Create the AABB for the triangle
             AABB aabb = AABB::createAABBForTriangle(trianglePoints);
@@ -106,56 +68,32 @@ void ConcaveMeshShape::initBVHTree() {
 }
 
 // Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
-// given the start vertex index pointer of the triangle
-void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32 subPart, int32 triangleIndex,
-                                                           Vector3* outTriangleVertices) const {
+void ConcaveMeshShape::getTriangleVertices(uint subPart, uint triangleIndex,
+                                           Vector3* outTriangleVertices) const {
 
     // Get the triangle vertex array of the current sub-part
     TriangleVertexArray* triangleVertexArray = mTriangleMesh->getSubpart(subPart);
 
-    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();
+    // Get the vertices coordinates of the triangle
+    triangleVertexArray->getTriangleVertices(triangleIndex, outTriangleVertices);
 
-    void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride);
-
-    // 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 = 0;
-        if (indexType == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE) {
-            vertexIndex = ((uint*)vertexIndexPointer)[k];
-        }
-        else if (indexType == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE) {
-            vertexIndex = ((unsigned short*)vertexIndexPointer)[k];
-        }
-        else {
-            assert(false);
-        }
-
-        // Get the vertices components of the triangle
-        if (vertexType == TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE) {
-            const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride);
-            outTriangleVertices[k][0] = decimal(vertices[0]) * mScaling.x;
-            outTriangleVertices[k][1] = decimal(vertices[1]) * mScaling.y;
-            outTriangleVertices[k][2] = decimal(vertices[2]) * mScaling.z;
-        }
-        else if (vertexType == TriangleVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE) {
-            const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride);
-            outTriangleVertices[k][0] = decimal(vertices[0]) * mScaling.x;
-            outTriangleVertices[k][1] = decimal(vertices[1]) * mScaling.y;
-            outTriangleVertices[k][2] = decimal(vertices[2]) * mScaling.z;
-        }
-        else {
-            assert(false);
-        }
-    }
+    // Apply the scaling factor to the vertices
+    outTriangleVertices[0] *= mScaling.x;
+    outTriangleVertices[1] *= mScaling.x;
+    outTriangleVertices[2] *= mScaling.x;
 }
 
+// Return the three vertex normals (in the array outVerticesNormals) of a triangle
+void ConcaveMeshShape::getTriangleVerticesNormals(uint subPart, uint triangleIndex, Vector3* outVerticesNormals) const {
+
+    // Get the triangle vertex array of the current sub-part
+    TriangleVertexArray* triangleVertexArray = mTriangleMesh->getSubpart(subPart);
+
+    // Get the vertices normals of the triangle
+    triangleVertexArray->getTriangleVerticesNormals(triangleIndex, outVerticesNormals);
+}
+
+
 // Use a callback method on all triangles of the concave shape inside a given AABB
 void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const {
 
@@ -208,11 +146,15 @@ void ConcaveMeshRaycastCallback::raycastTriangles() {
 
         // Get the triangle vertices for this node from the concave mesh shape
         Vector3 trianglePoints[3];
-        mConcaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints);
+        mConcaveMeshShape.getTriangleVertices(data[0], data[1], trianglePoints);
 
+        // Get the vertices normals of the triangle
+        Vector3 verticesNormals[3];
+        mConcaveMeshShape.getTriangleVerticesNormals(data[0], data[1], verticesNormals);
         // Create a triangle collision shape
         decimal margin = mConcaveMeshShape.getTriangleMargin();
-        TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
+        TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2],
+                                    verticesNormals, data[0], data[1], margin);
         triangleShape.setRaycastTestType(mConcaveMeshShape.getRaycastTestType());
 
         // Ray casting test against the collision shape

src/collision/shapes/ConcaveMeshShape.h

@@ -118,6 +118,10 @@ class ConcaveMeshShape : public ConcaveShape {
         /// Dynamic AABB tree to accelerate collision with the triangles
         DynamicAABBTree mDynamicAABBTree;
 
+        /// Array with computed vertices normals for each TriangleVertexArray of the triangle mesh (only
+        /// if the user did not provide its own vertices normals)
+        Vector3** mComputedVerticesNormals;
+
         // -------------------- Methods -------------------- //
 
         /// Raycast method with feedback information
@@ -130,9 +134,13 @@ class ConcaveMeshShape : public ConcaveShape {
         void initBVHTree();
 
         /// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
-        /// given the start vertex index pointer of the triangle.
-        void getTriangleVerticesWithIndexPointer(int32 subPart, int32 triangleIndex,
-                                                 Vector3* outTriangleVertices) const;
+        void getTriangleVertices(uint subPart, uint triangleIndex, Vector3* outTriangleVertices) const;
+
+        /// Return the three vertex normals (in the array outVerticesNormals) of a triangle
+        void getTriangleVerticesNormals(uint subPart, uint triangleIndex, Vector3* outVerticesNormals) const;
+
+        /// Get a smooth contact normal for collision for a triangle of the mesh
+        Vector3 computeSmoothLocalContactNormalForTriangle(TriangleShape* triangleShape, const Vector3& localContactPoint) const;
 
     public:
 
@@ -224,10 +232,14 @@ inline void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int nodeId)
 
     // Get the triangle vertices for this node from the concave mesh shape
     Vector3 trianglePoints[3];
-    mConcaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints);
+    mConcaveMeshShape.getTriangleVertices(data[0], data[1], trianglePoints);
+
+    // Get the vertices normals of the triangle
+    Vector3 verticesNormals[3];
+    mConcaveMeshShape.getTriangleVerticesNormals(data[0], data[1], verticesNormals);
 
     // Call the callback to test narrow-phase collision with this triangle
-    mTriangleTestCallback.testTriangle(trianglePoints);
+    mTriangleTestCallback.testTriangle(data[0], data[1], trianglePoints, verticesNormals);
 }
 
 }

src/collision/shapes/ConcaveShape.h

@@ -46,7 +46,8 @@ class TriangleCallback {
         virtual ~TriangleCallback() = default;
 
         /// Report a triangle
-        virtual void testTriangle(const Vector3* trianglePoints)=0;
+        virtual void testTriangle(uint meshSubPart, uint triangleIndex,
+                                  const Vector3* trianglePoints, const Vector3* verticesNormals)=0;
 
 };
 

src/collision/shapes/ConvexShape.h

@@ -49,10 +49,12 @@ class ConvexShape : public CollisionShape {
         // -------------------- Methods -------------------- //
 
         // Return a local support point in a given direction with the object margin
+        // TODO : Try to remove cachedCollisionData parameter
         Vector3 getLocalSupportPointWithMargin(const Vector3& direction,
                                                void** cachedCollisionData) const;
 
         /// Return a local support point in a given direction without the object margin
+        // TODO : Try to remove cachedCollisionData parameter
         virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
                                                           void** cachedCollisionData) const=0;
 

src/collision/shapes/HeightFieldShape.cpp

@@ -133,24 +133,48 @@ void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB&
        for (int j = jMin; j < jMax; j++) {
 
            // Compute the four point of the current quad
-           Vector3 p1 = getVertexAt(i, j);
-           Vector3 p2 = getVertexAt(i, j + 1);
-           Vector3 p3 = getVertexAt(i + 1, j);
-           Vector3 p4 = getVertexAt(i + 1, j + 1);
+           const Vector3 p1 = getVertexAt(i, j);
+           const Vector3 p2 = getVertexAt(i, j + 1);
+           const Vector3 p3 = getVertexAt(i + 1, j);
+           const Vector3 p4 = getVertexAt(i + 1, j + 1);
 
            // Generate the first triangle for the current grid rectangle
            Vector3 trianglePoints[3] = {p1, p2, p3};
 
+           // Compute the triangle normal
+           Vector3 triangle1Normal = (p2 - p1).cross(p3 - p1).getUnit();
+
+           // Use the triangle face normal as vertices normals (this is an aproximation. The correct
+           // solution would be to compute all the normals of the neighbor triangles and use their
+           // weighted average (with incident angle as weight) at the vertices. However, this solution
+           // seems too expensive (it requires to compute the normal of all neighbor triangles instead
+           // and compute the angle of incident edges with asin(). Maybe we could also precompute the
+           // vertices normal at the HeightFieldShape constructor but it will require extra memory to
+           // store them.
+           Vector3 verticesNormals1[3] = {triangle1Normal, triangle1Normal, triangle1Normal};
+
            // Test collision against the first triangle
-           callback.testTriangle(trianglePoints);
+           callback.testTriangle(0, 0, trianglePoints, verticesNormals1);
 
            // Generate the second triangle for the current grid rectangle
            trianglePoints[0] = p3;
            trianglePoints[1] = p2;
            trianglePoints[2] = p4;
 
+           // Compute the triangle normal
+           Vector3 triangle2Normal = (p2 - p3).cross(p4 - p3).getUnit();
+
+           // Use the triangle face normal as vertices normals (this is an aproximation. The correct
+           // solution would be to compute all the normals of the neighbor triangles and use their
+           // weighted average (with incident angle as weight) at the vertices. However, this solution
+           // seems too expensive (it requires to compute the normal of all neighbor triangles instead
+           // and compute the angle of incident edges with asin(). Maybe we could also precompute the
+           // vertices normal at the HeightFieldShape constructor but it will require extra memory to
+           // store them.
+           Vector3 verticesNormals2[3] = {triangle2Normal, triangle2Normal, triangle2Normal};
+
            // Test collision against the second triangle
-           callback.testTriangle(trianglePoints);
+           callback.testTriangle(0, 0, trianglePoints, verticesNormals2);
        }
    }
 }
@@ -232,11 +256,13 @@ Vector3 HeightFieldShape::getVertexAt(int x, int y) const {
 }
 
 // Raycast test between a ray and a triangle of the heightfield
-void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints) {
+void TriangleOverlapCallback::testTriangle(uint meshSubPart, uint triangleIndex, const Vector3* trianglePoints,
+                                           const Vector3* verticesNormals) {
 
     // Create a triangle collision shape
     decimal margin = mHeightFieldShape.getTriangleMargin();
-    TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
+    TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2],
+                                verticesNormals, meshSubPart, triangleIndex, margin);
     triangleShape.setRaycastTestType(mHeightFieldShape.getRaycastTestType());
 
     // Ray casting test against the collision shape

src/collision/shapes/HeightFieldShape.h

@@ -64,7 +64,8 @@ class TriangleOverlapCallback : public TriangleCallback {
         bool getIsHit() const {return mIsHit;}
 
         /// Raycast test between a ray and a triangle of the heightfield
-        virtual void testTriangle(const Vector3* trianglePoints) override;
+        virtual void testTriangle(uint meshSubPart, uint triangleIndex,
+                                  const Vector3* trianglePoints, const Vector3* verticesNormals) override;
 };
 
 

src/collision/shapes/TriangleShape.cpp

@@ -26,6 +26,7 @@
 // Libraries
 #include "TriangleShape.h"
 #include "collision/ProxyShape.h"
+#include "mathematics/mathematics_functions.h"
 #include "engine/Profiler.h"
 #include "configuration.h"
 #include <cassert>
@@ -34,13 +35,17 @@ using namespace reactphysics3d;
 
 // Constructor
 /**
+ * Do not use this constructor. It is supposed to be used internally only.
+ * Use a ConcaveMeshShape instead.
  * @param point1 First point of the triangle
  * @param point2 Second point of the triangle
  * @param point3 Third point of the triangle
+ * @param verticesNormals The three vertices normals for smooth mesh collision
  * @param margin The collision margin (in meters) around the collision shape
  */
-TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, decimal margin)
-              : ConvexPolyhedronShape(margin) {
+TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3,
+                             const Vector3* verticesNormals, uint meshSubPart, uint triangleIndex, decimal margin)
+              : ConvexPolyhedronShape(margin), mMeshSubPart(meshSubPart), mTriangleIndex(triangleIndex) {
 
     mPoints[0] = point1;
     mPoints[1] = point2;
@@ -50,9 +55,104 @@ TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const
     mNormal = (point3 - point1).cross(point2 - point1);
     mNormal.normalize();
 
+    mVerticesNormals[0] = verticesNormals[0];
+    mVerticesNormals[1] = verticesNormals[1];
+    mVerticesNormals[2] = verticesNormals[2];
+
     mRaycastTestType = TriangleRaycastSide::FRONT;
 }
 
+// This method compute the smooth mesh contact with a triangle in case one of the two collision
+// shapes is a triangle. The idea in this case is to use a smooth vertex normal of the triangle mesh
+// at the contact point instead of the triangle normal to avoid the internal edge collision issue.
+// This method will return the new smooth world contact
+// normal of the triangle and the the local contact point on the other shape.
+void TriangleShape::computeSmoothTriangleMeshContact(const CollisionShape* shape1, const CollisionShape* shape2,
+                                                     Vector3& localContactPointShape1, Vector3 localContactPointShape2,
+                                                     const Transform& shape1ToWorld, const Transform& shape2ToWorld,
+                                                     decimal penetrationDepth, Vector3& outSmoothVertexNormal) {
+
+    assert(shape1->getType() != CollisionShapeType::TRIANGLE || shape2->getType() != CollisionShapeType::TRIANGLE);
+
+    // If one the shape is a triangle
+    bool isShape1Triangle = shape1->getType() == CollisionShapeType::TRIANGLE;
+    if (isShape1Triangle || shape2->getType() == CollisionShapeType::TRIANGLE) {
+
+        const TriangleShape* triangleShape = isShape1Triangle ? static_cast<const TriangleShape*>(shape1):
+                                                                static_cast<const TriangleShape*>(shape2);
+
+        // Compute the smooth triangle mesh contact normal and recompute the local contact point on the other shape
+        triangleShape->computeSmoothMeshContact(isShape1Triangle ? localContactPointShape1 : localContactPointShape2,
+                                                isShape1Triangle ? shape1ToWorld : shape2ToWorld,
+                                                isShape1Triangle ? shape2ToWorld.getInverse() : shape1ToWorld.getInverse(),
+                                                penetrationDepth,
+                                                isShape1Triangle ? localContactPointShape2 : localContactPointShape1,
+                                                outSmoothVertexNormal);
+
+        // Make sure the direction of the contact normal is from shape1 to shape2
+        if (!isShape1Triangle) {
+            outSmoothVertexNormal = -outSmoothVertexNormal;
+        }
+    }
+}
+
+
+// This method implements the technique described in Game Physics Pearl book
+// by Gino van der Bergen and Dirk Gregorius to get smooth triangle mesh collision. The idea is
+// to replace the contact normal of the triangle shape with the precomputed normal of the triangle
+// mesh at this point. Then, we need to recompute the contact point on the other shape in order to
+// stay aligned with the new contact normal. This method will return the new smooth world contact
+// normal of the triangle and the the local contact point on the other shape.
+void TriangleShape::computeSmoothMeshContact(Vector3 localContactPointTriangle, const Transform& triangleShapeToWorldTransform,
+                                             const Transform& worldToOtherShapeTransform, decimal penetrationDepth,
+                                             Vector3& outNewLocalContactPointOtherShape, Vector3& outSmoothWorldContactTriangleNormal) const {
+
+    // Get the smooth contact normal of the mesh at the contact point on the triangle
+    Vector3 localNormal = computeSmoothLocalContactNormalForTriangle(localContactPointTriangle);
+
+    // Convert the local contact normal into world-space
+    outSmoothWorldContactTriangleNormal = triangleShapeToWorldTransform.getOrientation() * localNormal;
+
+    // Convert the contact normal into the local-space of the other shape
+    Vector3 normalOtherShape = worldToOtherShapeTransform.getOrientation() * outSmoothWorldContactTriangleNormal;
+
+    // Convert the local contact point of the triangle into the local-space of the other shape
+    Vector3 trianglePointOtherShape = worldToOtherShapeTransform * triangleShapeToWorldTransform *
+                                      localContactPointTriangle;
+
+    // Re-align the local contact point on the other shape such that it is aligned along
+    // the new contact normal
+    Vector3 otherShapePoint = trianglePointOtherShape - normalOtherShape * penetrationDepth;
+    outNewLocalContactPointOtherShape.setAllValues(otherShapePoint.x, otherShapePoint.y, otherShapePoint.z);
+}
+
+// Get a smooth contact normal for collision for a triangle of the mesh
+/// This is used to avoid the internal edges issue that occurs when a shape is colliding with
+/// several triangles of a concave mesh. If the shape collide with an edge of the triangle for instance,
+/// the computed contact normal from this triangle edge is not necessarily in the direction of the surface
+/// normal of the mesh at this point. The idea to solve this problem is to use the real (smooth) surface
+/// normal of the mesh at this point as the contact normal. This technique is described in the chapter 5
+/// of the Game Physics Pearl book by Gino van der Bergen and Dirk Gregorius. The vertices normals of the
+/// mesh are either provided by the user or precomputed if the user did not provide them.
+Vector3 TriangleShape::computeSmoothLocalContactNormalForTriangle(const Vector3& localContactPoint) const {
+
+    // Compute the barycentric coordinates of the point in the triangle
+    decimal u, v, w;
+    computeBarycentricCoordinatesInTriangle(mPoints[0], mPoints[1], mPoints[2], localContactPoint, u, v, w);
+
+    int nbZeros = 0;
+    bool isUZero = approxEqual(u, decimal(0), decimal(0.0001));
+    bool isVZero = approxEqual(v, decimal(0), decimal(0.0001));
+    bool isWZero = approxEqual(w, decimal(0), decimal(0.0001));
+    if (isUZero) nbZeros++;
+    if (isVZero) nbZeros++;
+    if (isWZero) nbZeros++;
+
+    // We compute the contact normal as the barycentric interpolation of the three vertices normals
+    return (u * mVerticesNormals[0] + v * mVerticesNormals[1] + w * mVerticesNormals[2]).getUnit();
+}
+
+
 // Raycast method with feedback information
 /// This method use the line vs triangle raycasting technique described in
 /// Real-time Collision Detection by Christer Ericson.

src/collision/shapes/TriangleShape.h

@@ -49,7 +49,10 @@ enum class TriangleRaycastSide {
 // Class TriangleShape
 /**
  * This class represents a triangle collision shape that is centered
- * at the origin and defined three points.
+ * at the origin and defined three points. A user cannot instanciate
+ * an object of this class. This class is for internal use only. Instances
+ * of this class are created when the user creates an HeightFieldShape and
+ * a ConcaveMeshShape
  */
 class TriangleShape : public ConvexPolyhedronShape {
 
@@ -63,15 +66,27 @@ class TriangleShape : public ConvexPolyhedronShape {
         /// Normal of the triangle
         Vector3 mNormal;
 
+        /// Three vertices normals for smooth collision with triangle mesh
+        Vector3 mVerticesNormals[3];
+
         /// Raycast test type for the triangle (front, back, front-back)
         TriangleRaycastSide mRaycastTestType;
 
+        /// Index of the mesh sub part in the original mesh
+        uint mMeshSubPart;
+
+        /// Triangle index of the triangle in the sub mesh
+        uint mTriangleIndex;
+
         // -------------------- Methods -------------------- //
 
         /// Return a local support point in a given direction without the object margin
         virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
                                                           void** cachedCollisionData) const override;
 
+        /// Get a smooth contact normal for collision for a triangle of the mesh
+        Vector3 computeSmoothLocalContactNormalForTriangle(const Vector3& localContactPoint) const;
+
         /// Return true if a point is inside the collision shape
         virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override;
 
@@ -81,13 +96,20 @@ class TriangleShape : public ConvexPolyhedronShape {
         /// Return the number of bytes used by the collision shape
         virtual size_t getSizeInBytes() const override;
 
+        // -------------------- Methods -------------------- //
+
+        /// This method implements the technique described in Game Physics Pearl book
+        void computeSmoothMeshContact(Vector3 localContactPointTriangle, const Transform& triangleShapeToWorldTransform,
+                                      const Transform& worldToOtherShapeTransform, decimal penetrationDepth,
+                                      Vector3& outNewLocalContactPointOtherShape, Vector3& outSmoothWorldContactTriangleNormal) const;
+
     public:
 
         // -------------------- Methods -------------------- //
 
         /// Constructor
         TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3,
-                      decimal margin = OBJECT_MARGIN);
+                      const Vector3* verticesNormals, uint meshSubPart, uint triangleIndex, decimal margin = OBJECT_MARGIN);
 
         /// Destructor
         virtual ~TriangleShape() override = default;
@@ -143,10 +165,23 @@ class TriangleShape : public ConvexPolyhedronShape {
         /// Return the centroid of the polyhedron
         virtual Vector3 getCentroid() const override;
 
+        /// Return the index of the sub part mesh of the original mesh
+        uint getMeshSubPart() const;
+
+        /// Return the triangle index in the original mesh
+        uint getTriangleIndex() const;
+
+        /// This method compute the smooth mesh contact with a triangle in case one of the two collision shapes is a triangle. The idea in this case is to use a smooth vertex normal of the triangle mesh
+        static void computeSmoothTriangleMeshContact(const CollisionShape* shape1, const CollisionShape* shape2,
+                                                     Vector3& localContactPointShape1, Vector3 localContactPointShape2,
+                                                     const Transform& shape1ToWorld, const Transform& shape2ToWorld,
+                                                     decimal penetrationDepth, Vector3& outSmoothVertexNormal);
+
         // ---------- Friendship ---------- //
 
         friend class ConcaveMeshRaycastCallback;
         friend class TriangleOverlapCallback;
+        friend class MiddlePhaseTriangleCallback;
 };
 
 // Return the number of bytes used by the collision shape
@@ -155,8 +190,7 @@ inline size_t TriangleShape::getSizeInBytes() const {
 }
 
 // Return a local support point in a given direction without the object margin
-inline Vector3 TriangleShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
-                                                              void** cachedCollisionData) const {
+inline Vector3 TriangleShape::getLocalSupportPointWithoutMargin(const Vector3& direction, void** cachedCollisionData) const {
     Vector3 dotProducts(direction.dot(mPoints[0]), direction.dot(mPoints[1]), direction.dot(mPoints[2]));
     return mPoints[dotProducts.getMaxAxis()];
 }
@@ -286,6 +320,16 @@ inline Vector3 TriangleShape::getCentroid() const {
     return (mPoints[0] + mPoints[1] + mPoints[2]) / decimal(3.0);
 }
 
+// Return the index of the sub part mesh of the original mesh
+inline uint TriangleShape::getMeshSubPart() const {
+    return mMeshSubPart;
+}
+
+// Return the triangle index in the original mesh
+inline uint TriangleShape::getTriangleIndex() const {
+    return mTriangleIndex;
+}
+
 // Return the number of half-edges of the polyhedron
 inline uint TriangleShape::getNbHalfEdges() const {
     return 6;