Modifications for smooth concave mesh contacts

CMakeLists.txt

@@ -1,5 +1,5 @@
 # Minimum cmake version required
-CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
+CMAKE_MINIMUM_REQUIRED(VERSION 3.1.0 FATAL_ERROR)
 
 # Project configuration
 PROJECT(REACTPHYSICS3D)
@@ -147,6 +147,7 @@ SET (REACTPHYSICS3D_SOURCES
     "src/engine/Timer.cpp"
     "src/mathematics/mathematics.h"
     "src/mathematics/mathematics_functions.h"
+    "src/mathematics/mathematics_functions.cpp"
     "src/mathematics/Matrix2x2.h"
     "src/mathematics/Matrix2x2.cpp"
     "src/mathematics/Matrix3x3.h"
@@ -168,6 +169,10 @@ SET (REACTPHYSICS3D_SOURCES
 # Create the library
 ADD_LIBRARY(reactphysics3d STATIC ${REACTPHYSICS3D_SOURCES})
 
+# Enable C++11 features
+set_property(TARGET reactphysics3d PROPERTY CXX_STANDARD 11)
+set_property(TARGET reactphysics3d PROPERTY CXX_STANDARD_REQUIRED ON)
+
 # If we need to compile the testbed application
 IF(COMPILE_TESTBED)
    add_subdirectory(testbed/)

src/collision/CollisionDetection.cpp

@@ -132,6 +132,8 @@ void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callbac
 
                 // Create the contact info object for the contact
                 ContactPointInfo contactInfo(manifold->getShape1(), manifold->getShape2(),
+                                             manifold->getShape1()->getCollisionShape(),
+                                             manifold->getShape2()->getCollisionShape(),
                                              contactPoint->getNormal(),
                                              contactPoint->getPenetrationDepth(),
                                              contactPoint->getLocalPointOnBody1(),

src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp

@@ -29,6 +29,7 @@
 #include "ConcaveVsConvexAlgorithm.h"
 #include "collision/CollisionDetection.h"
 #include "engine/CollisionWorld.h"
+#include <algorithm>
 
 using namespace reactphysics3d;
 
@@ -67,7 +68,6 @@ void ConcaveVsConvexAlgorithm::testCollision(const CollisionShapeInfo& shape1Inf
     }
 
     // Set the parameters of the callback object
-    mConvexVsTriangleCallback.setNarrowPhaseCallback(narrowPhaseCallback);
     mConvexVsTriangleCallback.setCollisionDetection(mCollisionDetection);
     mConvexVsTriangleCallback.setConvexShape(convexShape);
     mConvexVsTriangleCallback.setProxyShapes(convexProxyShape, concaveProxyShape);
@@ -77,8 +77,28 @@ void ConcaveVsConvexAlgorithm::testCollision(const CollisionShapeInfo& shape1Inf
     AABB aabb;
     convexShape->computeAABB(aabb, convexProxyShape->getLocalToWorldTransform());
 
+    // If smooth mesh collision is enabled for the concave mesh
+    if (concaveShape->getIsSmoothMeshCollisionEnabled()) {
+
+        std::vector<SmoothMeshContactInfo> contactPoints;
+
+        SmoothCollisionNarrowPhaseCallback smoothNarrowPhaseCallback(contactPoints);
+
+        mConvexVsTriangleCallback.setNarrowPhaseCallback(&smoothNarrowPhaseCallback);
+
         // Call the convex vs triangle callback for each triangle of the concave shape
         concaveShape->testAllTriangles(mConvexVsTriangleCallback, aabb);
+
+        // Run the smooth mesh collision algorithm
+        processSmoothMeshCollision(shape1Info.overlappingPair, contactPoints, narrowPhaseCallback);
+    }
+    else {
+
+        mConvexVsTriangleCallback.setNarrowPhaseCallback(narrowPhaseCallback);
+
+        // 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
@@ -107,3 +127,162 @@ void ConvexVsTriangleCallback::testTriangle(const Vector3* trianglePoints) {
     // Use the collision algorithm to test collision between the triangle and the other convex shape
     algo->testCollision(shapeConvexInfo, shapeConcaveInfo, mNarrowPhaseCallback);
 }
+
+// Process the concave triangle mesh collision using the smooth mesh collision algorithm described
+// by Pierre Terdiman (http://www.codercorner.com/MeshContacts.pdf). This is used to avoid the collision
+// issue with some internal edges.
+void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overlappingPair,
+                                                          std::vector<SmoothMeshContactInfo> contactPoints,
+                                                          NarrowPhaseCallback* narrowPhaseCallback) {
+
+    // Set with the triangle vertices already processed to void further contacts with same triangle
+    std::unordered_multimap<int, Vector3> processTriangleVertices;
+
+    // Sort the list of narrow-phase contacts according to their penetration depth
+    std::sort(contactPoints.begin(), contactPoints.end(), ContactsDepthCompare());
+
+    // For each contact point (from smaller penetration depth to larger)
+    std::vector<SmoothMeshContactInfo>::const_iterator it;
+    for (it = contactPoints.begin(); it != contactPoints.end(); ++it) {
+
+        const SmoothMeshContactInfo info = *it;
+        const Vector3& contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
+
+        // Compute the barycentric coordinates of the point in the triangle
+        decimal u, v, w;
+        computeBarycentricCoordinatesInTriangle(info.triangleVertices[0],
+                                                info.triangleVertices[1],
+                                                info.triangleVertices[2],
+                                                contactPoint, u, v, w);
+        int nbZeros = 0;
+        bool isUZero = approxEqual(u, 0, 0.0001);
+        bool isVZero = approxEqual(v, 0, 0.0001);
+        bool isWZero = approxEqual(w, 0, 0.0001);
+        if (isUZero) nbZeros++;
+        if (isVZero) nbZeros++;
+        if (isWZero) nbZeros++;
+
+        // If it is a vertex contact
+        if (nbZeros == 2) {
+
+            Vector3 contactVertex = !isUZero ? info.triangleVertices[0] : (!isVZero ? info.triangleVertices[1] : info.triangleVertices[2]);
+
+            // Check that this triangle vertex has not been processed yet
+            if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) {
+
+                // Keep the contact as it is and report it
+                narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo);
+            }
+        }
+        else if (nbZeros == 1) {  // If it is an edge contact
+
+            Vector3 contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]);
+            Vector3 contactVertex2 = isUZero ? info.triangleVertices[2] : (isVZero ? info.triangleVertices[2] : info.triangleVertices[1]);
+
+            // Check that this triangle edge has not been processed yet
+            if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) &&
+                !hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) {
+
+                // Keep the contact as it is and report it
+                narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo);
+            }
+
+        }
+        else {  // If it is a face contact
+
+            ContactPointInfo newContactInfo(info.contactInfo);
+
+            ProxyShape* firstShape;
+            ProxyShape* secondShape;
+            if (info.isFirstShapeTriangle) {
+                firstShape = overlappingPair->getShape1();
+                secondShape = overlappingPair->getShape2();
+            }
+            else {
+                firstShape = overlappingPair->getShape2();
+                secondShape = overlappingPair->getShape1();
+            }
+
+            // We use the triangle normal as the contact normal
+            Vector3 a = info.triangleVertices[1] - info.triangleVertices[0];
+            Vector3 b = info.triangleVertices[2] - info.triangleVertices[0];
+            Vector3 localNormal = a.cross(b);
+            newContactInfo.normal = firstShape->getLocalToWorldTransform().getOrientation() * localNormal;
+            Vector3 firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
+            Vector3 firstWorldPoint = firstShape->getLocalToWorldTransform() * firstLocalPoint;
+            newContactInfo.normal.normalize();
+            if (newContactInfo.normal.dot(info.contactInfo.normal) < 0) {
+                newContactInfo.normal = -newContactInfo.normal;
+            }
+
+            // We recompute the contact point on the second body with the new normal as described in
+            // the Smooth Mesh Contacts with GJK of the Game Physics Pearls book (from Gino van Den Bergen and
+            // Dirk Gregorius) to avoid adding torque
+            Transform worldToLocalSecondPoint = secondShape->getLocalToWorldTransform().getInverse();
+            if (info.isFirstShapeTriangle) {
+                Vector3 newSecondWorldPoint = firstWorldPoint + newContactInfo.normal;
+                newContactInfo.localPoint2 = worldToLocalSecondPoint * newSecondWorldPoint;
+            }
+            else {
+                Vector3 newSecondWorldPoint = firstWorldPoint - newContactInfo.normal;
+                newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint;
+            }
+
+            // Report the contact
+            narrowPhaseCallback->notifyContact(overlappingPair, newContactInfo);
+        }
+
+        // Add the three vertices of the triangle to the set of processed
+        // triangle vertices
+        addProcessedVertex(processTriangleVertices, info.triangleVertices[0]);
+        addProcessedVertex(processTriangleVertices, info.triangleVertices[1]);
+        addProcessedVertex(processTriangleVertices, info.triangleVertices[2]);
+    }
+}
+
+// Return true if the vertex is in the set of already processed vertices
+bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const std::unordered_multimap<int, Vector3>& processTriangleVertices, const Vector3& vertex) const {
+
+    int key = int(vertex.x * vertex.y * vertex.z);
+
+    auto range = processTriangleVertices.equal_range(key);
+    for (auto it = range.first; it != range.second; ++it) {
+        if (vertex.x == it->second.x && vertex.y == it->second.y && vertex.z == it->second.z) return true;
+    }
+
+    return false;
+}
+
+// Called by a narrow-phase collision algorithm when a new contact has been found
+void SmoothCollisionNarrowPhaseCallback::notifyContact(OverlappingPair* overlappingPair,
+                                                       const ContactPointInfo& contactInfo) {
+    Vector3 triangleVertices[3];
+    bool isFirstShapeTriangle;
+
+    // If the collision shape 1 is the triangle
+    if (contactInfo.collisionShape1->getType() == TRIANGLE) {
+        assert(contactInfo.collisionShape2->getType() != TRIANGLE);
+
+        const TriangleShape* triangleShape = static_cast<const TriangleShape*>(contactInfo.collisionShape1);
+        triangleVertices[0] = triangleShape->getVertex(0);
+        triangleVertices[1] = triangleShape->getVertex(1);
+        triangleVertices[2] = triangleShape->getVertex(2);
+
+        isFirstShapeTriangle = true;
+    }
+    else {  // If the collision shape 2 is the triangle
+        assert(contactInfo.collisionShape2->getType() == TRIANGLE);
+
+        const TriangleShape* triangleShape = static_cast<const TriangleShape*>(contactInfo.collisionShape2);
+        triangleVertices[0] = triangleShape->getVertex(0);
+        triangleVertices[1] = triangleShape->getVertex(1);
+        triangleVertices[2] = triangleShape->getVertex(2);
+
+        isFirstShapeTriangle = false;
+    }
+    SmoothMeshContactInfo smoothContactInfo(contactInfo, isFirstShapeTriangle, triangleVertices[0], triangleVertices[1], triangleVertices[2]);
+
+    // Add the narrow-phase contact into the list of contact to process for
+    // smooth mesh collision
+    mContactPoints.push_back(smoothContactInfo);
+}

src/collision/narrowphase/ConcaveVsConvexAlgorithm.h

@@ -30,6 +30,7 @@
 #include "NarrowPhaseAlgorithm.h"
 #include "collision/shapes/ConvexShape.h"
 #include "collision/shapes/ConcaveShape.h"
+#include <unordered_map>
 
 /// Namespace ReactPhysics3D
 namespace reactphysics3d {
@@ -62,6 +63,10 @@ class ConvexVsTriangleCallback : public TriangleCallback {
         /// Broadphase overlapping pair
         OverlappingPair* mOverlappingPair;
 
+        /// Used to sort ContactPointInfos according to their penetration depth
+        static bool contactsDepthCompare(const ContactPointInfo& contact1,
+                                         const ContactPointInfo& contact2);
+
     public:
 
         /// Set the collision detection pointer
@@ -95,6 +100,72 @@ class ConvexVsTriangleCallback : public TriangleCallback {
 
 };
 
+// Class SmoothMeshContactInfo
+/**
+ * This class is used to store data about a contact with a triangle for the smooth
+ * mesh algorithm.
+ */
+class SmoothMeshContactInfo {
+
+    public:
+
+        ContactPointInfo contactInfo;
+        bool isFirstShapeTriangle;
+        Vector3 triangleVertices[3];
+
+        /// Constructor
+        SmoothMeshContactInfo(const ContactPointInfo& contact, bool firstShapeTriangle, const Vector3& trianglePoint1,
+                              const Vector3& trianglePoint2, const Vector3& trianglePoint3)
+            : contactInfo(contact) {
+            isFirstShapeTriangle = firstShapeTriangle;
+            triangleVertices[0] = trianglePoint1;
+            triangleVertices[1] = trianglePoint2;
+            triangleVertices[2] = trianglePoint3;
+        }
+
+};
+
+struct ContactsDepthCompare {
+    bool operator()(const SmoothMeshContactInfo& contact1, const SmoothMeshContactInfo& contact2)
+    {
+        return contact1.contactInfo.penetrationDepth < contact2.contactInfo.penetrationDepth;
+    }
+};
+
+/// Method used to compare two smooth mesh contact info to sort them
+//inline static bool contactsDepthCompare(const SmoothMeshContactInfo& contact1,
+//                                        const SmoothMeshContactInfo& contact2) {
+//    return contact1.contactInfo.penetrationDepth < contact2.contactInfo.penetrationDepth;
+//}
+
+// 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 : public NarrowPhaseCallback {
+
+    private:
+
+        std::vector<SmoothMeshContactInfo>& mContactPoints;
+
+
+    public:
+
+        // Constructor
+        SmoothCollisionNarrowPhaseCallback(std::vector<SmoothMeshContactInfo>& contactPoints)
+          : mContactPoints(contactPoints) {
+
+        }
+
+
+        /// Called by a narrow-phase collision algorithm when a new contact has been found
+        virtual void notifyContact(OverlappingPair* overlappingPair,
+                                   const ContactPointInfo& contactInfo);
+
+};
+
 // Class ConcaveVsConvexAlgorithm
 /**
  * This class is used to compute the narrow-phase collision detection
@@ -119,6 +190,19 @@ class ConcaveVsConvexAlgorithm : public NarrowPhaseAlgorithm {
         /// Private assignment operator
         ConcaveVsConvexAlgorithm& operator=(const ConcaveVsConvexAlgorithm& algorithm);
 
+        /// Process the concave triangle mesh collision using the smooth mesh collision algorithm
+        void processSmoothMeshCollision(OverlappingPair* overlappingPair,
+                                        std::vector<SmoothMeshContactInfo> contactPoints,
+                                        NarrowPhaseCallback* narrowPhaseCallback);
+
+        /// Add a triangle vertex into the set of processed triangles
+        void addProcessedVertex(std::unordered_multimap<int, Vector3>& processTriangleVertices,
+                                const Vector3& vertex);
+
+        /// Return true if the vertex is in the set of already processed vertices
+        bool hasVertexBeenProcessed(const std::unordered_multimap<int, Vector3>& processTriangleVertices,
+                                    const Vector3& vertex) const;
+
     public :
 
         // -------------------- Methods -------------------- //
@@ -135,6 +219,11 @@ class ConcaveVsConvexAlgorithm : public NarrowPhaseAlgorithm {
                                    NarrowPhaseCallback* narrowPhaseCallback);
 };
 
+// Add a triangle vertex into the set of processed triangles
+inline void ConcaveVsConvexAlgorithm::addProcessedVertex(std::unordered_multimap<int, Vector3>& processTriangleVertices, const Vector3& vertex) {
+    processTriangleVertices.insert(std::make_pair(int(vertex.x * vertex.y * vertex.z), vertex));
+}
+
 }
 
 #endif

src/collision/narrowphase/EPA/EPAAlgorithm.cpp

@@ -25,6 +25,7 @@
 
 // Libraries
 #include "EPAAlgorithm.h"
+#include "engine/Profiler.h"
 #include "collision/narrowphase//GJK/GJKAlgorithm.h"
 #include "TrianglesStore.h"
 
@@ -89,6 +90,8 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
                                                            Vector3& v,
                                                            NarrowPhaseCallback* narrowPhaseCallback) {
 
+    PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()");
+
     assert(shape1Info.collisionShape->isConvex());
     assert(shape2Info.collisionShape->isConvex());
 
@@ -426,9 +429,11 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
     decimal penetrationDepth = v.length();
     assert(penetrationDepth > 0.0);
 
+    if (normal.lengthSquare() < MACHINE_EPSILON) return;
+    
     // Create the contact info object
-    ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape,
+    ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
-                                 normal, penetrationDepth, pALocal, pBLocal);
+                                 shape2Info.collisionShape, normal, penetrationDepth, pALocal, pBLocal);
 
     narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
 }

src/collision/narrowphase/GJK/GJKAlgorithm.cpp

@@ -28,6 +28,7 @@
 #include "Simplex.h"
 #include "constraint/ContactPoint.h"
 #include "configuration.h"
+#include "engine/Profiler.h"
 #include <algorithm>
 #include <cmath>
 #include <cfloat>
@@ -59,6 +60,8 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
                                  const CollisionShapeInfo& shape2Info,
                                  NarrowPhaseCallback* narrowPhaseCallback) {
 
+    PROFILE("GJKAlgorithm::testCollision()");
+    
     Vector3 suppA;             // Support point of object A
     Vector3 suppB;             // Support point of object B
     Vector3 w;                 // Support point of Minkowski difference A-B
@@ -146,8 +149,8 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
             if (penetrationDepth <= 0.0) return;
 			
             // Create the contact info object
-            ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape,
+            ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
-                                         normal, penetrationDepth, pA, pB);
+                                         shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
 
             narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
 
@@ -179,8 +182,8 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
             if (penetrationDepth <= 0.0) return;
 			
             // Create the contact info object
-            ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape,
+            ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
-                                         normal, penetrationDepth, pA, pB);
+                                         shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
 
             narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
 
@@ -210,8 +213,8 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
             if (penetrationDepth <= 0.0) return;
 			
             // Create the contact info object
-            ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape,
+            ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
-                                         normal, penetrationDepth, pA, pB);
+                                         shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
 
             narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
 
@@ -248,8 +251,8 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
             if (penetrationDepth <= 0.0) return;
 			
             // Create the contact info object
-            ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape,
+            ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
-                                         normal, penetrationDepth, pA, pB);
+                                         shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
 
             narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
 
@@ -278,6 +281,8 @@ void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap
                                                              const Transform& transform2,
                                                              NarrowPhaseCallback* narrowPhaseCallback,
                                                              Vector3& v) {
+    PROFILE("GJKAlgorithm::computePenetrationDepthForEnlargedObjects()");
+
     Simplex simplex;
     Vector3 suppA;
     Vector3 suppB;

src/collision/narrowphase/SphereVsSphereAlgorithm.cpp

@@ -70,8 +70,8 @@ void SphereVsSphereAlgorithm::testCollision(const CollisionShapeInfo& shape1Info
         decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
         
         // Create the contact info object
-        ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape,
+        ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
-                                     vectorBetweenCenters.getUnit(), penetrationDepth,
+                                     shape2Info.collisionShape, vectorBetweenCenters.getUnit(), penetrationDepth,
                                      intersectionOnBody1, intersectionOnBody2);
 
         // Notify about the new contact

src/collision/shapes/ConcaveShape.cpp

@@ -32,7 +32,7 @@ using namespace reactphysics3d;
 
 // Constructor
 ConcaveShape::ConcaveShape(CollisionShapeType type) : CollisionShape(type) {
-
+    mIsSmoothMeshCollisionEnabled = false;
 }
 
 // Destructor

src/collision/shapes/ConcaveShape.h

@@ -58,7 +58,8 @@ class ConcaveShape : public CollisionShape {
 
         // -------------------- Attributes -------------------- //
 
-
+        /// True if the smooth mesh collision algorithm is enabled
+        bool mIsSmoothMeshCollisionEnabled;
 
         // -------------------- Methods -------------------- //
 
@@ -86,6 +87,12 @@ class ConcaveShape : public CollisionShape {
 
         /// 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 smooth mesh collision is enabled
+        bool getIsSmoothMeshCollisionEnabled() const;
+
+        /// Enable/disable the smooth mesh collision algorithm
+        void setIsSmoothMeshCollisionEnabled(bool isEnabled);
 };
 
 /// Return true if the collision shape is convex, false if it is concave
@@ -98,6 +105,19 @@ inline bool ConcaveShape::testPointInside(const Vector3& localPoint, ProxyShape*
     return false;
 }
 
+// Return true if the smooth mesh collision is enabled
+inline bool ConcaveShape::getIsSmoothMeshCollisionEnabled() const {
+    return mIsSmoothMeshCollisionEnabled;
+}
+
+// Enable/disable the smooth mesh collision algorithm
+/// Smooth mesh collision is used to avoid collisions against some internal edges
+/// of the triangle mesh. If it is enabled, collsions with the mesh will be smoother
+/// but collisions computation is a bit more expensive.
+inline void ConcaveShape::setIsSmoothMeshCollisionEnabled(bool isEnabled) {
+    mIsSmoothMeshCollisionEnabled = isEnabled;
+}
+
 }
 
 #endif

src/collision/shapes/TriangleShape.h

@@ -117,6 +117,9 @@ class TriangleShape : public ConvexShape {
         // Set the raycast test type (front, back, front-back)
         void setRaycastTestType(TriangleRaycastSide testType);
 
+        /// Return the coordinates of a given vertex of the triangle
+        Vector3 getVertex(int index) const;
+
         // ---------- Friendship ---------- //
 
         friend class ConcaveMeshRaycastCallback;
@@ -213,6 +216,15 @@ inline void TriangleShape::setRaycastTestType(TriangleRaycastSide testType) {
     mRaycastTestType = testType;
 }
 
+// Return the coordinates of a given vertex of the triangle
+/**
+ * @param index Index (0 to 2) of a vertex of the triangle
+ */
+inline Vector3 TriangleShape::getVertex(int index) const {
+    assert(index >= 0 && index < 3);
+    return mPoints[index];
+}
+
 }
 
 #endif

src/constraint/ContactPoint.h

@@ -49,12 +49,6 @@ struct ContactPointInfo {
 
         // -------------------- Methods -------------------- //
 
-        /// Private copy-constructor
-        ContactPointInfo(const ContactPointInfo& contactInfo);
-
-        /// Private assignment operator
-        ContactPointInfo& operator=(const ContactPointInfo& contactInfo);
-
     public:
 
         // -------------------- Attributes -------------------- //
@@ -65,26 +59,32 @@ struct ContactPointInfo {
         /// Second proxy shape of the contact
         ProxyShape* shape2;
 
-        /// Normalized normal vector the the collision contact in world space
+        /// First collision shape
-        const Vector3 normal;
+        const CollisionShape* collisionShape1;
+
+        /// Second collision shape
+        const CollisionShape* collisionShape2;
+
+        /// Normalized normal vector of the collision contact in world space
+        Vector3 normal;
 
         /// Penetration depth of the contact
-        const decimal penetrationDepth;
+        decimal penetrationDepth;
 
         /// Contact point of body 1 in local space of body 1
-        const Vector3 localPoint1;
+        Vector3 localPoint1;
 
         /// Contact point of body 2 in local space of body 2
-        const Vector3 localPoint2;
+        Vector3 localPoint2;
 
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ContactPointInfo(ProxyShape* proxyShape1, ProxyShape* proxyShape2,
+        ContactPointInfo(ProxyShape* proxyShape1, ProxyShape* proxyShape2, const CollisionShape* collShape1,
-                         const Vector3& normal, decimal penetrationDepth, const Vector3& localPoint1,
+                         const CollisionShape* collShape2, const Vector3& normal, decimal penetrationDepth,
-                         const Vector3& localPoint2)
+                         const Vector3& localPoint1, const Vector3& localPoint2)
-            : shape1(proxyShape1), shape2(proxyShape2), normal(normal),
+            : shape1(proxyShape1), shape2(proxyShape2), collisionShape1(collShape1), collisionShape2(collShape2),
-              penetrationDepth(penetrationDepth), localPoint1(localPoint1),
+              normal(normal), penetrationDepth(penetrationDepth), localPoint1(localPoint1),
               localPoint2(localPoint2) {
 
         }
@@ -107,7 +107,7 @@ class ContactPoint {
         /// Second rigid body of the contact
         CollisionBody* mBody2;
 
-        /// Normalized normal vector of the contact (From body1 toward body2) in world space
+        /// Normalized normal vector of the contact (from body1 toward body2) in world space
         const Vector3 mNormal;
 
         /// Penetration depth

src/mathematics/mathematics_functions.cpp

@@ -0,0 +1,51 @@
+/********************************************************************************
+* 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.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "mathematics_functions.h"
+#include "Vector3.h"
+
+using namespace reactphysics3d;
+
+/// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c)
+/// This method uses the technique described in the book Real-Time collision detection by
+/// Christer Ericson.
+void reactphysics3d::computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, const Vector3& c,
+                                             const Vector3& p, decimal& u, decimal& v, decimal& w) {
+    const Vector3 v0 = b - a;
+    const Vector3 v1 = c - a;
+    const Vector3 v2 = p - a;
+
+    decimal d00 = v0.dot(v0);
+    decimal d01 = v0.dot(v1);
+    decimal d11 = v1.dot(v1);
+    decimal d20 = v2.dot(v0);
+    decimal d21 = v2.dot(v1);
+
+    decimal denom = d00 * d11 - d01 * d01;
+    v = (d11 * d20 - d01 * d21) / denom;
+    w = (d00 * d21 - d01 * d20) / denom;
+    u = decimal(1.0) - u - w;
+}

src/mathematics/mathematics_functions.h

@@ -30,6 +30,8 @@
 #include "configuration.h"
 #include "decimal.h"
 #include <algorithm>
+#include <cassert>
+#include <cmath>
 
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
@@ -41,7 +43,7 @@ struct Vector3;
 /// Function to test if two real numbers are (almost) equal
 /// We test if two numbers a and b are such that (a-b) are in [-EPSILON; EPSILON]
 inline bool approxEqual(decimal a, decimal b, decimal epsilon = MACHINE_EPSILON) {
-    return (fabs(a - b) < epsilon);
+    return (std::fabs(a - b) < epsilon);
 }
 
 /// Function that returns the result of the "value" clamped by
@@ -66,6 +68,10 @@ inline bool sameSign(decimal a, decimal b) {
     return a * b >= decimal(0.0);
 }
 
+/// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c)
+void computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, const Vector3& c,
+                                             const Vector3& p, decimal& u, decimal& v, decimal& w);
+
 }
 
 #endif