diff --git a/src/body/CollisionBody.h b/src/body/CollisionBody.h
index 2c88d155..f69be6c5 100644
--- a/src/body/CollisionBody.h
+++ b/src/body/CollisionBody.h
@@ -303,4 +303,4 @@ inline Vector3 CollisionBody::getLocalVector(const Vector3& worldVector) const {
 
 }
 
- #endif
+#endif
diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h
index 7e81a25f..d5c585e6 100644
--- a/src/collision/CollisionDetection.h
+++ b/src/collision/CollisionDetection.h
@@ -35,9 +35,9 @@
 #include "memory/MemoryAllocator.h"
 #include "constraint/ContactPoint.h"
 #include <vector>
-#include <map>
 #include <set>
 #include <utility>
+#include <map>
 
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp
index 513ca367..6c94ed5a 100644
--- a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp
+++ b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp
@@ -81,8 +81,9 @@ int EPAAlgorithm::isOriginInTetrahedron(const Vector3& p1, const Vector3& p2,
 /// enlarged objects (with margin) where the original objects (without margin)
 /// intersect. An initial simplex that contains origin has been computed with
 /// GJK algorithm. The EPA Algorithm will extend this simplex polytope to find
-/// the correct penetration depth
-void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simplex,
+/// the correct penetration depth. This method returns true if the EPA penetration
+/// depth computation has succeeded and false it has failed.
+bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const VoronoiSimplex& simplex,
                                                            CollisionShapeInfo shape1Info,
                                                            const Transform& transform1,
                                                            CollisionShapeInfo shape2Info,
@@ -92,6 +93,8 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
 
     PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()");
 
+    decimal gjkPenDepthSquare = v.lengthSquare();
+
     assert(shape1Info.collisionShape->isConvex());
     assert(shape2Info.collisionShape->isConvex());
 
@@ -118,7 +121,7 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
                               transform1.getOrientation();
 
     // Get the simplex computed previously by the GJK algorithm
-    unsigned int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points);
+    int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points);
 
     // Compute the tolerance
     decimal tolerance = MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint();
@@ -137,7 +140,7 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
             // Only one point in the simplex (which should be the origin).
             // We have a touching contact with zero penetration depth.
             // We drop that kind of contact. Therefore, we return false
-            return;
+            return true;
 
         case 2: {
             // The simplex returned by GJK is a line segment d containing the origin.
@@ -204,7 +207,7 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
             }
             else {
                 // The origin is not in the initial polytope
-                return;
+                return false;
             }
 
             // The polytope contains now 4 vertices
@@ -234,7 +237,7 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
                 if (!((face0 != NULL) && (face1 != NULL) && (face2 != NULL) && (face3 != NULL)
                    && face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0
                    && face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) {
-                    return;
+                    return false;
                 }
 
                 // Associate the edges of neighbouring triangle faces
@@ -319,14 +322,14 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
                 face3 = triangleStore.newTriangle(points, 1, 4, 2);
             }
             else {
-                return;
+                return false;
             }
 
             // If the constructed tetrahedron is not correct
             if (!((face0 != NULL) && (face1 != NULL) && (face2 != NULL) && (face3 != NULL)
                && face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0
                && face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) {
-                return;
+                return false;
             }
 
             // Associate the edges of neighbouring triangle faces
@@ -353,7 +356,7 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
     // can run the EPA algorithm.
 
     if (nbTriangles == 0) {
-        return;
+        return false;
     }
 
     TriangleEPA* triangle = 0;
@@ -368,6 +371,7 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
 
         // If the candidate face in the heap is not obsolete
         if (!triangle->getIsObsolete()) {
+
             // If we have reached the maximum number of support points
             if (nbVertices == MAX_SUPPORT_POINTS) {
                 assert(false);
@@ -388,7 +392,7 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
 
             // Update the upper bound of the penetration depth
             decimal wDotv = points[indexNewVertex].dot(triangle->getClosestPoint());
-            assert(wDotv > 0.0);
+
             decimal wDotVSquare = wDotv * wDotv / triangle->getDistSquare();
             if (wDotVSquare < upperBoundSquarePenDepth) {
                 upperBoundSquarePenDepth = wDotVSquare;
@@ -427,13 +431,22 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
     Vector3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB);
     Vector3 normal = v.getUnit();
     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, shape1Info.collisionShape,
-                                 shape2Info.collisionShape, normal, penetrationDepth, pALocal, pBLocal);
+    // If the length of the normal vector is too small, skip this contact point
+    if (normal.lengthSquare() < MACHINE_EPSILON) {
+        return false;
+    }
 
-    narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
+    if (penetrationDepth * penetrationDepth > gjkPenDepthSquare && penetrationDepth > 0) {
+
+        // Create the contact info object
+        ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
+                                     shape2Info.collisionShape, normal, penetrationDepth, pALocal, pBLocal);
+
+        narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
+
+        return true;
+    }
+
+    return false;
 }
diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.h b/src/collision/narrowphase/EPA/EPAAlgorithm.h
index 1d4b527a..377ccc11 100644
--- a/src/collision/narrowphase/EPA/EPAAlgorithm.h
+++ b/src/collision/narrowphase/EPA/EPAAlgorithm.h
@@ -27,7 +27,7 @@
 #define REACTPHYSICS3D_EPA_ALGORITHM_H
 
 // Libraries
-#include "collision/narrowphase/GJK/Simplex.h"
+#include "collision/narrowphase/GJK/VoronoiSimplex.h"
 #include "collision/shapes/CollisionShape.h"
 #include "collision/CollisionShapeInfo.h"
 #include "constraint/ContactPoint.h"
@@ -123,13 +123,13 @@ class EPAAlgorithm {
         void init(MemoryAllocator* memoryAllocator);
 
         /// Compute the penetration depth with EPA algorithm.
-        void computePenetrationDepthAndContactPoints(const Simplex& simplex,
+        bool computePenetrationDepthAndContactPoints(const VoronoiSimplex& simplex,
                                                      CollisionShapeInfo shape1Info,
                                                      const Transform& transform1,
                                                      CollisionShapeInfo shape2Info,
                                                      const Transform& transform2,
                                                      Vector3& v,
-                                                    NarrowPhaseCallback* narrowPhaseCallback);
+                                                     NarrowPhaseCallback* narrowPhaseCallback);
 };
 
 // Add a triangle face in the candidate triangle heap in the EPA algorithm
diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
index 49db5968..a9419bb5 100644
--- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
+++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
@@ -69,6 +69,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
     Vector3 pB;                // Closest point of object B
     decimal vDotw;
     decimal prevDistSquare;
+    bool contactFound = false;
 
     assert(shape1Info.collisionShape->isConvex());
     assert(shape2Info.collisionShape->isConvex());
@@ -79,6 +80,8 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
     void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
     void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
 
+    bool isPolytopeShape = shape1->isPolyhedron() && shape2->isPolyhedron();
+
     // Get the local-space to world-space transforms
     const Transform transform1 = shape1Info.shapeToWorldTransform;
     const Transform transform2 = shape2Info.shapeToWorldTransform;
@@ -98,7 +101,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
     assert(margin > 0.0);
 
     // Create a simplex set
-    Simplex simplex;
+    VoronoiSimplex simplex;
 
     // Get the previous point V (last cached separating axis)
     Vector3 v = mCurrentOverlappingPair->getCachedSeparatingAxis();
@@ -131,31 +134,9 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
         // If the objects intersect only in the margins
         if (simplex.isPointInSimplex(w) || distSquare - vDotw <= distSquare * REL_ERROR_SQUARE) {
 
-            // Compute the closet points of both objects (without the margins)
-            simplex.computeClosestPointsOfAandB(pA, pB);
-
-            // Project those two points on the margins to have the closest points of both
-            // object with the margins
-            decimal dist = sqrt(distSquare);
-            assert(dist > 0.0);
-            pA = (pA - (shape1->getMargin() / dist) * v);
-            pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
-
-            // Compute the contact info
-            Vector3 normal = transform1.getOrientation() * (-v.getUnit());
-            decimal penetrationDepth = margin - dist;
-			
-			// Reject the contact if the penetration depth is negative (due too numerical errors)
-            if (penetrationDepth <= 0.0) return;
-			
-            // Create the contact info object
-            ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
-                                         shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
-
-            narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
-
-            // There is an intersection, therefore we return
-            return;
+            // Contact point has been found
+            contactFound = true;
+            break;
         }
 
         // Add the new support point to the simplex
@@ -164,62 +145,24 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
         // If the simplex is affinely dependent
         if (simplex.isAffinelyDependent()) {
 
-            // Compute the closet points of both objects (without the margins)
-            simplex.computeClosestPointsOfAandB(pA, pB);
-
-            // Project those two points on the margins to have the closest points of both
-            // object with the margins
-            decimal dist = sqrt(distSquare);
-            assert(dist > 0.0);
-            pA = (pA - (shape1->getMargin() / dist) * v);
-            pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
-
-            // Compute the contact info
-            Vector3 normal = transform1.getOrientation() * (-v.getUnit());
-            decimal penetrationDepth = margin - dist;
-			
-			// Reject the contact if the penetration depth is negative (due too numerical errors)
-            if (penetrationDepth <= 0.0) return;
-			
-            // Create the contact info object
-            ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
-                                         shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
-
-            narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
-
-            // There is an intersection, therefore we return
-            return;
+            // Contact point has been found
+            contactFound = true;
+            break;
         }
 
         // Compute the point of the simplex closest to the origin
-        // If the computation of the closest point fail
+        // If the computation of the closest point fails
         if (!simplex.computeClosestPoint(v)) {
 
-            // Compute the closet points of both objects (without the margins)
-            simplex.computeClosestPointsOfAandB(pA, pB);
+            // Contact point has been found
+            contactFound = true;
+            break;
+        }
 
-            // Project those two points on the margins to have the closest points of both
-            // object with the margins
-            decimal dist = sqrt(distSquare);
-            assert(dist > 0.0);
-            pA = (pA - (shape1->getMargin() / dist) * v);
-            pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
-
-            // Compute the contact info
-            Vector3 normal = transform1.getOrientation() * (-v.getUnit());
-            decimal penetrationDepth = margin - dist;
-			
-			// Reject the contact if the penetration depth is negative (due too numerical errors)
-            if (penetrationDepth <= 0.0) return;
-			
-            // Create the contact info object
-            ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
-                                         shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
-
-            narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
-
-            // There is an intersection, therefore we return
-            return;
+        // Closest point is almost the origin, go to EPA algorithm
+        // Vector v to small to continue computing support points
+        if (v.lengthSquare() < MACHINE_EPSILON) {
+            break;
         }
 
         // Store and update the squared distance of the closest point
@@ -228,46 +171,65 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
 
         // If the distance to the closest point doesn't improve a lot
         if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
+
             simplex.backupClosestPointInSimplex(v);
             
             // Get the new squared distance
             distSquare = v.lengthSquare();
 
-            // Compute the closet points of both objects (without the margins)
-            simplex.computeClosestPointsOfAandB(pA, pB);
+            // Contact point has been found
+            contactFound = true;
+            break;
+        }
 
-            // Project those two points on the margins to have the closest points of both
-            // object with the margins
-            decimal dist = sqrt(distSquare);
-            assert(dist > 0.0);
-            pA = (pA - (shape1->getMargin() / dist) * v);
-            pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
+    } while((isPolytopeShape && !simplex.isFull()) || (!isPolytopeShape && !simplex.isFull() &&
+            distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint()));
 
-            // Compute the contact info
-            Vector3 normal = transform1.getOrientation() * (-v.getUnit());
-            decimal penetrationDepth = margin - dist;
-			
-			// Reject the contact if the penetration depth is negative (due too numerical errors)
-            if (penetrationDepth <= 0.0) return;
-			
-            // Create the contact info object
-            ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
-                                         shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
+    bool isEPAResultValid = false;
 
-            narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
+    // If no contact has been found (penetration case)
+    if (!contactFound) {
 
-            // There is an intersection, therefore we return
+        // The objects (without margins) intersect. Therefore, we run the GJK algorithm
+        // again but on the enlarged objects to compute a simplex polytope that contains
+        // the origin. Then, we give that simplex polytope to the EPA algorithm to compute
+        // the correct penetration depth and contact points between the enlarged objects.
+        isEPAResultValid = computePenetrationDepthForEnlargedObjects(shape1Info, transform1, shape2Info,
+                                                                     transform2, narrowPhaseCallback, v);
+    }
+
+    if ((contactFound || !isEPAResultValid) && distSquare > MACHINE_EPSILON) {
+
+        // Compute the closet points of both objects (without the margins)
+        simplex.computeClosestPointsOfAandB(pA, pB);
+
+        // Project those two points on the margins to have the closest points of both
+        // object with the margins
+        decimal dist = std::sqrt(distSquare);
+        assert(dist > 0.0);
+        pA = (pA - (shape1->getMargin() / dist) * v);
+        pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
+
+        // Compute the contact info
+        Vector3 normal = transform1.getOrientation() * (-v.getUnit());
+        decimal penetrationDepth = margin - dist;
+
+        // If the penetration depth is negative (due too numerical errors), there is no contact
+        if (penetrationDepth <= 0.0) {
             return;
         }
-    } while(!simplex.isFull() && distSquare > MACHINE_EPSILON *
-                                 simplex.getMaxLengthSquareOfAPoint());
 
-    // The objects (without margins) intersect. Therefore, we run the GJK algorithm
-    // again but on the enlarged objects to compute a simplex polytope that contains
-    // the origin. Then, we give that simplex polytope to the EPA algorithm to compute
-    // the correct penetration depth and contact points between the enlarged objects.
-    return computePenetrationDepthForEnlargedObjects(shape1Info, transform1, shape2Info,
-                                                     transform2, narrowPhaseCallback, v);
+        // Do not generate a contact point with zero normal length
+        if (normal.lengthSquare() < MACHINE_EPSILON) {
+            return;
+        }
+
+        // Create the contact info object
+        ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
+                                     shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
+
+        narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
+    }
 }
 
 /// This method runs the GJK algorithm on the two enlarged objects (with margin)
@@ -275,7 +237,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
 /// assumed to intersect in the original objects (without margin). Therefore such
 /// a polytope must exist. Then, we give that polytope to the EPA algorithm to
 /// compute the correct penetration depth and contact points of the enlarged objects.
-void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info,
+bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info,
                                                              const Transform& transform1,
                                                              const CollisionShapeInfo& shape2Info,
                                                              const Transform& transform2,
@@ -283,7 +245,7 @@ void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap
                                                              Vector3& v) {
     PROFILE("GJKAlgorithm::computePenetrationDepthForEnlargedObjects()");
 
-    Simplex simplex;
+    VoronoiSimplex simplex;
     Vector3 suppA;
     Vector3 suppB;
     Vector3 w;
@@ -297,6 +259,8 @@ void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap
     const ConvexShape* shape1 = static_cast<const ConvexShape*>(shape1Info.collisionShape);
     const ConvexShape* shape2 = static_cast<const ConvexShape*>(shape2Info.collisionShape);
 
+    bool isPolytopeShape = shape1->isPolyhedron() && shape2->isPolyhedron();
+
     void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
     void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
 
@@ -322,18 +286,18 @@ void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap
         if (vDotw > 0.0) {
 
             // No intersection, we return
-            return;
+            return false;
         }
 
         // Add the new support point to the simplex
         simplex.addPoint(w, suppA, suppB);
 
         if (simplex.isAffinelyDependent()) {
-            return;
+            return false;
         }
 
         if (!simplex.computeClosestPoint(v)) {
-            return;
+            return false;
         }
 
         // Store and update the square distance
@@ -341,11 +305,11 @@ void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap
         distSquare = v.lengthSquare();
 
         if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
-            return;
+            return false;
         }
 
-    } while(!simplex.isFull() && distSquare > MACHINE_EPSILON *
-                                 simplex.getMaxLengthSquareOfAPoint());
+    } while((!simplex.isFull() && isPolytopeShape) || (!isPolytopeShape && !simplex.isFull() &&
+            distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint()));
 
     // Give the simplex computed with GJK algorithm to the EPA algorithm
     // which will compute the correct penetration depth and contact points
@@ -372,7 +336,7 @@ bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyS
     const Vector3 suppB(localPoint);
 
     // Create a simplex set
-    Simplex simplex;
+    VoronoiSimplex simplex;
 
     // Initial supporting direction
     Vector3 v(1, 1, 1);
@@ -446,7 +410,7 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo&
     Vector3 w;
 
     // Create a simplex set
-    Simplex simplex;
+    VoronoiSimplex simplex;
 
     Vector3 n(decimal(0.0), decimal(0.0), decimal(0.0));
     decimal lambda = decimal(0.0);
diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.h b/src/collision/narrowphase/GJK/GJKAlgorithm.h
index 23ebc789..8617a002 100644
--- a/src/collision/narrowphase/GJK/GJKAlgorithm.h
+++ b/src/collision/narrowphase/GJK/GJKAlgorithm.h
@@ -75,7 +75,7 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
         GJKAlgorithm& operator=(const GJKAlgorithm& algorithm);
 
         /// Compute the penetration depth for enlarged objects.
-        void computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info,
+        bool computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info,
                                                        const Transform& transform1,
                                                        const CollisionShapeInfo& shape2Info,
                                                        const Transform& transform2,
diff --git a/src/collision/narrowphase/GJK/VoronoiSimplex.cpp b/src/collision/narrowphase/GJK/VoronoiSimplex.cpp
index a30f52ff..382ccc9b 100644
--- a/src/collision/narrowphase/GJK/VoronoiSimplex.cpp
+++ b/src/collision/narrowphase/GJK/VoronoiSimplex.cpp
@@ -150,10 +150,7 @@ bool VoronoiSimplex::isAffinelyDependent() const {
 ///      pA = sum(lambda_i * a_i)    where "a_i" are the support points of object A
 ///      pB = sum(lambda_i * b_i)    where "b_i" are the support points of object B
 ///      with lambda_i = deltaX_i / deltaX
-void VoronoiSimplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) {
-
-    // Recompute the closest point (if necessary)
-    //recomputeClosestPoint();
+void VoronoiSimplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const {
 
     pA = mClosestSuppPointA;
     pB = mClosestSuppPointB;
@@ -380,6 +377,7 @@ void VoronoiSimplex::computeClosestPointOnTriangle(const Vector3& a, const Vecto
 
         // The origin is in the Voronoi region of edge AB
         // We return the projection of the origin on the edge AB
+        assert(std::abs(d1 - d3) > MACHINE_EPSILON);
         decimal v = d1 / (d1 - d3);
 
         // Set the barycentric coords of the closest point on the triangle
@@ -410,6 +408,7 @@ void VoronoiSimplex::computeClosestPointOnTriangle(const Vector3& a, const Vecto
 
         // The origin is in the Voronoi region of edge AC
         // We return the projection of the origin on the edge AC
+        assert(std::abs(d2 - d6) > MACHINE_EPSILON);
         decimal w = d2 / (d2 - d6);
 
         // Set the barycentric coords of the closest point on the triangle
@@ -425,6 +424,7 @@ void VoronoiSimplex::computeClosestPointOnTriangle(const Vector3& a, const Vecto
 
         // The origin is in the Voronoi region of edge BC
         // We return the projection of the origin on the edge BC
+        assert(std::abs((d4 - d3) + (d5 - d6)) > MACHINE_EPSILON);
         decimal w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
 
         // Set the barycentric coords of the closest point on the triangle
diff --git a/src/collision/narrowphase/GJK/VoronoiSimplex.h b/src/collision/narrowphase/GJK/VoronoiSimplex.h
index 7b6c6614..9cbee47e 100644
--- a/src/collision/narrowphase/GJK/VoronoiSimplex.h
+++ b/src/collision/narrowphase/GJK/VoronoiSimplex.h
@@ -165,7 +165,7 @@ class VoronoiSimplex {
         void backupClosestPointInSimplex(Vector3& point);
 
         /// Compute the closest points "pA" and "pB" of object A and B.
-        void computeClosestPointsOfAandB(Vector3& pA, Vector3& pB);
+        void computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const;
 
         /// Compute the closest point to the origin of the current simplex.
         bool computeClosestPoint(Vector3& v);
diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h
index f581baa0..bda4bf3f 100644
--- a/src/collision/shapes/BoxShape.h
+++ b/src/collision/shapes/BoxShape.h
@@ -99,6 +99,9 @@ class BoxShape : public ConvexShape {
         /// Return the local bounds of the shape in x, y and z directions
         virtual void getLocalBounds(Vector3& min, Vector3& max) const;
 
+        /// Return true if the collision shape is a polyhedron
+        virtual bool isPolyhedron() const;
+
         /// Return the local inertia tensor of the collision shape
         virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
 };
@@ -134,6 +137,11 @@ inline void BoxShape::getLocalBounds(Vector3& min, Vector3& max) const {
     min = -max;
 }
 
+// Return true if the collision shape is a polyhedron
+inline bool BoxShape::isPolyhedron() const {
+    return true;
+}
+
 // Return the number of bytes used by the collision shape
 inline size_t BoxShape::getSizeInBytes() const {
     return sizeof(BoxShape);
diff --git a/src/collision/shapes/CapsuleShape.h b/src/collision/shapes/CapsuleShape.h
index 56269d04..8e77f050 100644
--- a/src/collision/shapes/CapsuleShape.h
+++ b/src/collision/shapes/CapsuleShape.h
@@ -101,6 +101,9 @@ class CapsuleShape : public ConvexShape {
         /// Return the local bounds of the shape in x, y and z directions
         virtual void getLocalBounds(Vector3& min, Vector3& max) const;
 
+        /// Return true if the collision shape is a polyhedron
+        virtual bool isPolyhedron() const;
+
         /// Return the local inertia tensor of the collision shape
         virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
 };
@@ -154,6 +157,11 @@ inline void CapsuleShape::getLocalBounds(Vector3& min, Vector3& max) const {
     min.z = min.x;
 }
 
+// Return true if the collision shape is a polyhedron
+inline bool CapsuleShape::isPolyhedron() const {
+    return false;
+}
+
 // Return a local support point in a given direction without the object margin.
 /// A capsule is the convex hull of two spheres S1 and S2. The support point in the direction "d"
 /// of the convex hull of a set of convex objects is the support point "p" in the set of all
diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h
index cd89255d..80419c03 100644
--- a/src/collision/shapes/CollisionShape.h
+++ b/src/collision/shapes/CollisionShape.h
@@ -98,6 +98,9 @@ class CollisionShape {
         /// Return true if the collision shape is convex, false if it is concave
         virtual bool isConvex() const=0;
 
+        /// Return true if the collision shape is a polyhedron
+        virtual bool isPolyhedron() const=0;
+
         /// Return the local bounds of the shape in x, y and z directions
         virtual void getLocalBounds(Vector3& min, Vector3& max) const=0;
 
diff --git a/src/collision/shapes/ConcaveShape.h b/src/collision/shapes/ConcaveShape.h
index 9c1418df..9cee92ab 100644
--- a/src/collision/shapes/ConcaveShape.h
+++ b/src/collision/shapes/ConcaveShape.h
@@ -101,6 +101,9 @@ class ConcaveShape : public CollisionShape {
         /// Return true if the collision shape is convex, false if it is concave
         virtual bool isConvex() const;
 
+        /// Return true if the collision shape is a polyhedron
+        virtual bool isPolyhedron() const;
+
         /// Use a callback method on all triangles of the concave shape inside a given AABB
         virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const=0;
 
@@ -116,11 +119,16 @@ inline decimal ConcaveShape::getTriangleMargin() const {
     return mTriangleMargin;
 }
 
-/// Return true if the collision shape is convex, false if it is concave
+// Return true if the collision shape is convex, false if it is concave
 inline bool ConcaveShape::isConvex() const {
     return false;
 }
 
+// Return true if the collision shape is a polyhedron
+inline bool ConcaveShape::isPolyhedron() const {
+    return true;
+}
+
 // Return true if a point is inside the collision shape
 inline bool ConcaveShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
     return false;
diff --git a/src/collision/shapes/ConeShape.h b/src/collision/shapes/ConeShape.h
index b693f386..53630817 100644
--- a/src/collision/shapes/ConeShape.h
+++ b/src/collision/shapes/ConeShape.h
@@ -107,6 +107,9 @@ class ConeShape : public ConvexShape {
         /// Return the local bounds of the shape in x, y and z directions
         virtual void getLocalBounds(Vector3& min, Vector3& max) const;
 
+        /// Return true if the collision shape is a polyhedron
+        virtual bool isPolyhedron() const;
+
         /// Return the local inertia tensor of the collision shape
         virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
 };
@@ -159,6 +162,11 @@ inline void ConeShape::getLocalBounds(Vector3& min, Vector3& max) const {
     min.z = min.x;
 }
 
+// Return true if the collision shape is a polyhedron
+inline bool ConeShape::isPolyhedron() const {
+    return false;
+}
+
 // Return the local inertia tensor of the collision shape
 /**
  * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h
index d23e91b0..94ec67f9 100644
--- a/src/collision/shapes/ConvexMeshShape.h
+++ b/src/collision/shapes/ConvexMeshShape.h
@@ -140,6 +140,9 @@ class ConvexMeshShape : public ConvexShape {
         /// Add an edge into the convex mesh by specifying the two vertex indices of the edge.
         void addEdge(uint v1, uint v2);
 
+        /// Return true if the collision shape is a polyhedron
+        virtual bool isPolyhedron() const;
+
         /// Return true if the edges information is used to speed up the collision detection
         bool isEdgesInformationUsed() const;
 
@@ -159,6 +162,11 @@ inline size_t ConvexMeshShape::getSizeInBytes() const {
     return sizeof(ConvexMeshShape);
 }
 
+// Return true if the collision shape is a polyhedron
+inline bool ConvexMeshShape::isPolyhedron() const {
+    return true;
+}
+
 // Return the local bounds of the shape in x, y and z directions
 /**
  * @param min The minimum bounds of the shape in local-space coordinates
diff --git a/src/collision/shapes/CylinderShape.h b/src/collision/shapes/CylinderShape.h
index 71e4f9ae..a732ecf6 100644
--- a/src/collision/shapes/CylinderShape.h
+++ b/src/collision/shapes/CylinderShape.h
@@ -98,6 +98,9 @@ class CylinderShape : public ConvexShape {
         /// Return the height
         decimal getHeight() const;
 
+        /// Return true if the collision shape is a polyhedron
+        virtual bool isPolyhedron() const;
+
         /// Set the scaling vector of the collision shape
         virtual void setLocalScaling(const Vector3& scaling);
 
@@ -124,6 +127,11 @@ inline decimal CylinderShape::getHeight() const {
     return mHalfHeight + mHalfHeight;
 }
 
+// Return true if the collision shape is a polyhedron
+inline bool CylinderShape::isPolyhedron() const {
+    return false;
+}
+
 // Set the scaling vector of the collision shape
 inline void CylinderShape::setLocalScaling(const Vector3& scaling) {
 
diff --git a/src/collision/shapes/SphereShape.h b/src/collision/shapes/SphereShape.h
index a1316ae0..bfbe46b5 100644
--- a/src/collision/shapes/SphereShape.h
+++ b/src/collision/shapes/SphereShape.h
@@ -83,6 +83,9 @@ class SphereShape : public ConvexShape {
         /// Return the radius of the sphere
         decimal getRadius() const;
 
+        /// Return true if the collision shape is a polyhedron
+        virtual bool isPolyhedron() const;
+
         /// Set the scaling vector of the collision shape
         virtual void setLocalScaling(const Vector3& scaling);
 
@@ -104,6 +107,11 @@ inline decimal SphereShape::getRadius() const {
     return mMargin;
 }
 
+// Return true if the collision shape is a polyhedron
+inline bool SphereShape::isPolyhedron() const {
+        return false;
+}
+
 // Set the scaling vector of the collision shape
 inline void SphereShape::setLocalScaling(const Vector3& scaling) {
 
diff --git a/src/collision/shapes/TriangleShape.h b/src/collision/shapes/TriangleShape.h
index 1b9f3681..71af0f89 100644
--- a/src/collision/shapes/TriangleShape.h
+++ b/src/collision/shapes/TriangleShape.h
@@ -116,6 +116,9 @@ class TriangleShape : public ConvexShape {
         /// Return the coordinates of a given vertex of the triangle
         Vector3 getVertex(int index) const;
 
+        /// Return true if the collision shape is a polyhedron
+        virtual bool isPolyhedron() const;
+
         // ---------- Friendship ---------- //
 
         friend class ConcaveMeshRaycastCallback;
@@ -218,6 +221,11 @@ inline Vector3 TriangleShape::getVertex(int index) const {
     return mPoints[index];
 }
 
+// Return true if the collision shape is a polyhedron
+inline bool TriangleShape::isPolyhedron() const {
+    return true;
+}
+
 }
 
 #endif