Working on capsule vs polyhedron narrow-phase algorithm

src/collision/ContactManifoldInfo.h

@@ -64,15 +64,8 @@ class ContactManifoldInfo {
         /// Destructor
         ~ContactManifoldInfo() {
 
-            // Delete all the contact points in the linked list
+            // Remove all the contact points
-            ContactPointInfo* element = mContactPointsList;
+            reset();
-            while(element != nullptr) {
-                ContactPointInfo* elementToDelete = element;
-                element = element->next;
-
-                // Delete the current element
-                mAllocator.release(elementToDelete, sizeof(ContactPointInfo));
-            }
         }
 
         /// Deleted copy-constructor
@@ -96,6 +89,22 @@ class ContactManifoldInfo {
             mContactPointsList = contactPointInfo;
         }
 
+        /// Remove all the contact points
+        void reset() {
+
+            // Delete all the contact points in the linked list
+            ContactPointInfo* element = mContactPointsList;
+            while(element != nullptr) {
+                ContactPointInfo* elementToDelete = element;
+                element = element->next;
+
+                // Delete the current element
+                mAllocator.release(elementToDelete, sizeof(ContactPointInfo));
+            }
+
+            mContactPointsList = nullptr;
+        }
+
         /// Get the first contact point info of the linked list of contact points
         ContactPointInfo* getFirstContactPointInfo() const {
             return mContactPointsList;

src/collision/PolyhedronMesh.cpp

@@ -42,6 +42,9 @@ PolyhedronMesh::PolyhedronMesh(PolygonVertexArray* polygonVertexArray) {
 
    // Compute the faces normals
    computeFacesNormals();
+
+   // Compute the centroid
+   computeCentroid();
 }
 
 // Destructor
@@ -126,3 +129,15 @@ void PolyhedronMesh::computeFacesNormals() {
         mFacesNormals[f].normalize();
     }
 }
+
+// Compute the centroid of the polyhedron
+void PolyhedronMesh::computeCentroid() {
+
+    mCentroid.setToZero();
+
+    for (uint v=0; v < getNbVertices(); v++) {
+        mCentroid += getVertex(v);
+    }
+
+    mCentroid /= getNbVertices();
+}

src/collision/PolyhedronMesh.h

@@ -55,6 +55,9 @@ class PolyhedronMesh {
         /// Array with the face normals
         Vector3* mFacesNormals;
 
+        /// Centroid of the polyhedron
+        Vector3 mCentroid;
+
         // -------------------- Methods -------------------- //
 
         /// Create the half-edge structure of the mesh
@@ -63,6 +66,9 @@ class PolyhedronMesh {
         /// Compute the faces normals
         void computeFacesNormals();
 
+        /// Compute the centroid of the polyhedron
+        void computeCentroid() ;
+
     public:
 
         // -------------------- Methods -------------------- //
@@ -84,6 +90,9 @@ class PolyhedronMesh {
 
         /// Return the half-edge structure of the mesh
         const HalfEdgeStructure& getHalfEdgeStructure() const;
+
+        /// Return the centroid of the polyhedron
+        Vector3 getCentroid() const;
 };
 
 // Return the number of vertices
@@ -102,6 +111,11 @@ inline const HalfEdgeStructure& PolyhedronMesh::getHalfEdgeStructure() const {
     return mHalfEdgeStructure;
 }
 
+// Return the centroid of the polyhedron
+inline Vector3 PolyhedronMesh::getCentroid() const {
+    return mCentroid;
+}
+
 }
 
 #endif

src/collision/ProxyShape.h

@@ -175,7 +175,6 @@ class ProxyShape {
         friend class CollisionDetection;
         friend class CollisionWorld;
         friend class DynamicsWorld;
-        friend class EPAAlgorithm;
         friend class GJKAlgorithm;
         friend class ConvexMeshShape;
 

src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp

@@ -30,13 +30,14 @@
 // We want to use the ReactPhysics3D namespace
 using namespace reactphysics3d;  
 
+// Compute the narrow-phase collision detection between two capsules
+// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
+// by Dirk Gregorius.
 bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) {
     
     assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE);
     assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE);
 
-	const decimal parallelEpsilon = decimal(0.001);
-
     // Get the capsule collision shapes
     const CapsuleShape* capsuleShape1 = static_cast<const CapsuleShape*>(narrowPhaseInfo->collisionShape1);
     const CapsuleShape* capsuleShape2 = static_cast<const CapsuleShape*>(narrowPhaseInfo->collisionShape2);
@@ -62,7 +63,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase
 	decimal sumRadius = capsuleShape2->getRadius() + capsuleShape1->getRadius();
 
 	// If the two capsules are parallel (we create two contact points)
-	if (seg1.cross(seg2).lengthSquare() < parallelEpsilon * parallelEpsilon) {
+    if (areParallelVectors(seg1, seg2)) {
 
 		// If the distance between the two segments is larger than the sum of the capsules radius (we do not have overlapping)
 		const decimal segmentsDistance = computeDistancePointToLineDistance(capsule1SegA, capsule1SegB, capsule2SegA);

src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h

@@ -60,7 +60,7 @@ class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
         /// Deleted assignment operator
 		CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete;
 
-        /// Compute a contact info if the two bounding volume collide
+        /// Compute the narrow-phase collision detection between two capsules
         virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
                                    ContactManifoldInfo& contactManifoldInfo) override;
 };

src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp

@@ -27,24 +27,86 @@
 #include "CapsuleVsConvexPolyhedronAlgorithm.h"
 #include "SAT/SATAlgorithm.h"
 #include "GJK/GJKAlgorithm.h"
+#include "collision/shapes/CapsuleShape.h"
+#include "collision/shapes/ConvexPolyhedronShape.h"
 
 // We want to use the ReactPhysics3D namespace
 using namespace reactphysics3d;
 
+// Compute the narrow-phase collision detection between a capsule and a polyhedron
+// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
+// by Dirk Gregorius.
 bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
                                                        ContactManifoldInfo& contactManifoldInfo) {
 
-    // Get the local-space to world-space transforms
-    const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform;
-    const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform;
-
     // First, we run the GJK algorithm
     GJKAlgorithm gjkAlgorithm;
+    SATAlgorithm satAlgorithm;
     GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo);
 
     // If we have found a contact point inside the margins (shallow penetration)
     if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) {
 
+        // GJK has found a shallow contact. If the contact normal is parallel to a face of the
+        // polyhedron mesh, we would like to create two contact points instead of a single one
+        // (as in the deep contact case with SAT algorithm)
+
+        // Get the contact point created by GJK
+        ContactPointInfo* contactPoint = contactManifoldInfo.getFirstContactPointInfo();
+
+        bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE;
+        assert(isCapsuleShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
+        assert(!isCapsuleShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
+        assert(!isCapsuleShape1 || narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE);
+
+        // Get the collision shapes
+        const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(isCapsuleShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2);
+        const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(isCapsuleShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1);
+
+        // For each face of the polyhedron
+        // For each face of the convex mesh
+        for (uint f = 0; f < polyhedron->getNbFaces(); f++) {
+
+            // Get the face
+            HalfEdgeStructure::Face face = polyhedron->getFace(f);
+
+            const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform;
+
+            // Get the face normal
+            const Vector3 faceNormal = polyhedron->getFaceNormal(f);
+            const Vector3 faceNormalWorld = polyhedronToWorld.getOrientation() * faceNormal;
+
+            // If the polyhedron face normal is parallel to the computed GJK contact normal
+            if (areParallelVectors(faceNormalWorld, contactPoint->normal)) {
+
+                // Remove the previous contact point computed by GJK
+                contactManifoldInfo.reset();
+
+                const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform;
+                const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld;
+
+                // Compute the end-points of the inner segment of the capsule
+                const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0);
+                const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0);
+
+                // Convert the inner capsule segment points into the polyhedron local-space
+                const Transform capsuleToPolyhedronTransform = polyhedronToCapsuleTransform.getInverse();
+                const Vector3 capsuleSegAPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegA;
+                const Vector3 capsuleSegBPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegB;
+
+                const Vector3 separatingAxisCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal;
+
+                // Compute and create two contact points
+                satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth,
+                                                          polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace,
+                                                          capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
+                                                          contactManifoldInfo, isCapsuleShape1);
+
+                break;
+            }
+
+        }
+
         // Return true
         return true;
     }
@@ -53,7 +115,6 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na
     if (result == GJKAlgorithm::GJKResult::INTERPENETRATE) {
 
         // Run the SAT algorithm to find the separating axis and compute contact point
-        SATAlgorithm satAlgorithm;
         return satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo);
     }
 

src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h

@@ -60,7 +60,7 @@ class CapsuleVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
         /// Deleted assignment operator
         CapsuleVsConvexPolyhedronAlgorithm& operator=(const CapsuleVsConvexPolyhedronAlgorithm& algorithm) = delete;
 
-        /// Compute a contact info if the two bounding volume collide
+        /// Compute the narrow-phase collision detection between a capsule and a polyhedron
         virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
                                    ContactManifoldInfo& contactManifoldInfo) override;
 };

src/collision/narrowphase/SAT/SATAlgorithm.cpp

@@ -27,7 +27,7 @@
 #include "SATAlgorithm.h"
 #include "constraint/ContactPoint.h"
 #include "collision/PolyhedronMesh.h"
-#include "collision/shapes/ConvexPolyhedronShape.h"
+#include "collision/shapes/CapsuleShape.h"
 #include "collision/shapes/SphereShape.h"
 #include "configuration.h"
 #include "engine/Profiler.h"
@@ -110,8 +110,220 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo*
 // Test collision between a capsule and a convex mesh
 bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
 
-    assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE);
+    bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE;
-    assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
+
+    assert(isCapsuleShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
+    assert(!isCapsuleShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
+    assert(!isCapsuleShape1 || narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE);
+
+    // Get the collision shapes
+    const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(isCapsuleShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2);
+    const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(isCapsuleShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1);
+
+    const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform;
+    const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform;
+
+    const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld;
+
+    // Minimum penetration depth
+    decimal minPenetrationDepth = DECIMAL_LARGEST;
+    uint minFaceIndex = 0;
+    uint minEdgeIndex = 0;
+    bool isMinPenetrationFaceNormal = false;
+    Vector3 separatingAxisCapsuleSpace;
+    Vector3 separatingPolyhedronEdgeVertex1;
+    Vector3 separatingPolyhedronEdgeVertex2;
+
+    // For each face of the convex mesh
+    for (uint f = 0; f < polyhedron->getNbFaces(); f++) {
+
+        // Get the face
+        HalfEdgeStructure::Face face = polyhedron->getFace(f);
+
+        // Get the face normal
+        const Vector3 faceNormal = polyhedron->getFaceNormal(f);
+
+        // Compute the penetration depth (using the capsule support in the direction opposite to the face normal)
+        const Vector3 faceNormalCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal;
+        const Vector3 capsuleSupportPoint = capsuleShape->getLocalSupportPointWithMargin(-faceNormalCapsuleSpace, nullptr);
+        const Vector3 pointOnPolyhedronFace = polyhedronToCapsuleTransform * polyhedron->getVertexPosition(face.faceVertices[0]);
+        const Vector3 capsuleSupportPointToFacePoint =  pointOnPolyhedronFace - capsuleSupportPoint;
+        const decimal penetrationDepth = capsuleSupportPointToFacePoint.dot(faceNormal);
+
+        // If the penetration depth is negative, we have found a separating axis
+        if (penetrationDepth <= decimal(0.0)) {
+            return false;
+        }
+
+        // Check if we have found a new minimum penetration axis
+        if (penetrationDepth < minPenetrationDepth) {
+            minPenetrationDepth = penetrationDepth;
+            minFaceIndex = f;
+            isMinPenetrationFaceNormal = true;
+            separatingAxisCapsuleSpace = faceNormalCapsuleSpace;
+        }
+    }
+
+    // Compute the end-points of the inner segment of the capsule
+    const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0);
+    const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0);
+    const Vector3 capsuleSegmentAxis = capsuleSegB - capsuleSegA;
+
+    // For each direction that is the cross product of the capsule inner segment and
+    // an edge of the polyhedron
+    for (uint e = 0; e < polyhedron->getNbHalfEdges(); e += 2) {
+
+        // Get an edge from the polyhedron (convert it into the capsule local-space)
+        HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(e);
+        const Vector3 edgeVertex1 = polyhedron->getVertexPosition(edge.vertexIndex);
+        const Vector3 edgeVertex2 = polyhedron->getVertexPosition(polyhedron->getHalfEdge(edge.nextEdgeIndex).vertexIndex);
+        const Vector3 edgeDirectionCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * (edgeVertex2 - edgeVertex1);
+
+        HalfEdgeStructure::Edge twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex);
+        const Vector3 adjacentFace1Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(edge.faceIndex);
+        const Vector3 adjacentFace2Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(twinEdge.faceIndex);
+
+        // Check using the Gauss Map if this edge cross product can be as separating axis
+        if (isMinkowskiFaceCapsuleVsEdge(capsuleSegmentAxis, adjacentFace1Normal, adjacentFace2Normal)) {
+
+            // Compute the axis to test (cross product between capsule inner segment and polyhedron edge)
+            Vector3 axis = capsuleSegmentAxis.cross(edgeDirectionCapsuleSpace);
+
+            // Skip separating axis test if polyhedron edge is parallel to the capsule inner segment
+            if (axis.lengthSquare() >= decimal(0.00001)) {
+
+                const Vector3 polyhedronCentroid = polyhedronToCapsuleTransform * polyhedron->getCentroid();
+                const Vector3 pointOnPolyhedronEdge = polyhedronToCapsuleTransform * edgeVertex1;
+
+                // Swap axis direction if necessary such that it points out of the polyhedron
+                if (axis.dot(pointOnPolyhedronEdge - polyhedronCentroid) < 0) {
+                    axis = -axis;
+                }
+
+                axis.normalize();
+
+                // Compute the penetration depth
+                const Vector3 capsuleSupportPoint = capsuleShape->getLocalSupportPointWithMargin(-axis, nullptr);
+                const Vector3 capsuleSupportPointToEdgePoint = pointOnPolyhedronEdge - capsuleSupportPoint;
+                const decimal penetrationDepth = capsuleSupportPointToEdgePoint.dot(axis);
+
+                // If the penetration depth is negative, we have found a separating axis
+                if (penetrationDepth <= decimal(0.0)) {
+                    return false;
+                }
+
+                // Check if we have found a new minimum penetration axis
+                if (penetrationDepth < minPenetrationDepth) {
+                    minPenetrationDepth = penetrationDepth;
+                    minEdgeIndex = e;
+                    isMinPenetrationFaceNormal = false;
+                    separatingAxisCapsuleSpace = axis;
+                    separatingPolyhedronEdgeVertex1 = edgeVertex1;
+                    separatingPolyhedronEdgeVertex2 = edgeVertex2;
+                }
+            }
+        }
+    }
+
+    // Convert the inner capsule segment points into the polyhedron local-space
+    const Transform capsuleToPolyhedronTransform = polyhedronToCapsuleTransform.getInverse();
+    const Vector3 capsuleSegAPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegA;
+    const Vector3 capsuleSegBPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegB;
+
+    const Vector3 normalWorld = capsuleToWorld.getOrientation() * separatingAxisCapsuleSpace;
+    const decimal capsuleRadius = capsuleShape->getRadius();
+
+    // If the separating axis is a face normal
+    // We need to clip the inner capsule segment with the adjacent faces of the separating face
+    if (isMinPenetrationFaceNormal) {
+
+        computeCapsulePolyhedronFaceContactPoints(minFaceIndex, capsuleRadius, polyhedron, minPenetrationDepth,
+                                                  polyhedronToCapsuleTransform, normalWorld, separatingAxisCapsuleSpace,
+                                                  capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
+                                                  contactManifoldInfo, isCapsuleShape1);
+    }
+    else {   // The separating axis is the cross product of a polyhedron edge and the inner capsule segment
+
+        // Compute the closest points between the inner capsule segment and the
+        // edge of the polyhedron in polyhedron local-space
+        Vector3 closestPointPolyhedronEdge, closestPointCapsuleInnerSegment;
+        computeClosestPointBetweenTwoSegments(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
+                                              separatingPolyhedronEdgeVertex1, separatingPolyhedronEdgeVertex2,
+                                              closestPointCapsuleInnerSegment, closestPointPolyhedronEdge);
+
+
+        // Project closest capsule inner segment point into the capsule bounds
+        const Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * closestPointCapsuleInnerSegment) - separatingAxisCapsuleSpace * capsuleRadius;
+
+        // Create the contact point
+        contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth,
+                                            isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge,
+                                            isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule);
+    }
+
+    return true;
+}
+
+// Compute the two contact points between a polyhedron and a capsule when the separating
+// 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,
+                                                             const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace,
+                                                             ContactManifoldInfo& contactManifoldInfo, bool isCapsuleShape1) const {
+
+    HalfEdgeStructure::Face face = polyhedron->getFace(referenceFaceIndex);
+    uint firstEdgeIndex = face.edgeIndex;
+    uint edgeIndex = firstEdgeIndex;
+
+    std::vector<Vector3> planesPoints;
+    std::vector<Vector3> planesNormals;
+
+    // For each adjacent edge of the separating face of the polyhedron
+    do {
+        HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(edgeIndex);
+        HalfEdgeStructure::Edge twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex);
+
+        // Construct a clippling plane for each adjacent edge of the separting face of the polyhedron
+        planesPoints.push_back(polyhedron->getVertexPosition(edge.vertexIndex));
+        planesNormals.push_back(polyhedron->getFaceNormal(twinEdge.faceIndex));
+
+        edgeIndex = edge.nextEdgeIndex;
+
+    } while(edgeIndex != firstEdgeIndex);
+
+    // First we clip the inner segment of the capsule with the four planes of the adjacent faces
+    std::vector<Vector3> clipSegment = clipSegmentWithPlanes(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
+                                                             planesPoints, planesNormals);
+
+    // Project the two clipped points into the polyhedron face
+    const Vector3 faceNormal = polyhedron->getFaceNormal(referenceFaceIndex);
+    const Vector3 contactPoint1Polyhedron = clipSegment[0] + faceNormal * (penetrationDepth - capsuleRadius);
+    const Vector3 contactPoint2Polyhedron = clipSegment[1] + faceNormal * (penetrationDepth - capsuleRadius);
+
+    // Project the two clipped points into the capsule bounds
+    const Vector3 contactPoint1Capsule = (polyhedronToCapsuleTransform * clipSegment[0]) - separatingAxisCapsuleSpace * capsuleRadius;
+    const Vector3 contactPoint2Capsule = (polyhedronToCapsuleTransform * clipSegment[1]) - separatingAxisCapsuleSpace * capsuleRadius;
+
+    // Create the contact points
+    contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth,
+                                        isCapsuleShape1 ? contactPoint1Capsule : contactPoint1Polyhedron,
+                                        isCapsuleShape1 ? contactPoint1Polyhedron : contactPoint1Capsule);
+    contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth,
+                                        isCapsuleShape1 ? contactPoint2Capsule : contactPoint2Polyhedron,
+                                        isCapsuleShape1 ? contactPoint2Polyhedron : contactPoint2Capsule);
+}
+
+// This method returns true if an edge of a polyhedron and a capsule forms a
+// face of the Minkowski Difference. This test is used to know if two edges
+// (one edge of the polyhedron vs the inner segment of the capsule in this case)
+// have to be test as a possible separating axis
+bool SATAlgorithm::isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, const Vector3& edgeAdjacentFace1Normal,
+                                                const Vector3& edgeAdjacentFace2Normal) const {
+
+    // Return true if the arc on the Gauss Map corresponding to the polyhedron edge
+    // intersect the unit circle plane corresponding to capsule Gauss Map
+    return capsuleSegment.dot(edgeAdjacentFace1Normal) * capsuleSegment.dot(edgeAdjacentFace2Normal) < decimal(0.0);
 }
 
 // Test collision between a triangle and a convex mesh

src/collision/narrowphase/SAT/SATAlgorithm.h

@@ -29,6 +29,7 @@
 // Libraries
 #include "collision/ContactManifoldInfo.h"
 #include "collision/NarrowPhaseInfo.h"
+#include "collision/shapes/ConvexPolyhedronShape.h"
 
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
@@ -68,6 +69,17 @@ class SATAlgorithm {
         /// Test collision between a capsule and a convex mesh
         bool testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
 
+        /// 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,
+                                                       const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace,
+                                                       ContactManifoldInfo& contactManifoldInfo, bool isCapsuleShape1) const;
+
+        // This method returns true if an edge of a polyhedron and a capsule forms a face of the Minkowski Difference
+        bool isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, const Vector3& edgeAdjacentFace1Normal,
+                                          const Vector3& edgeAdjacentFace2Normal) const;
+
         /// Test collision between a triangle and a convex mesh
         bool testCollisionTriangleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
 

src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp

@@ -31,6 +31,9 @@
 // We want to use the ReactPhysics3D namespace
 using namespace reactphysics3d;  
 
+// Compute the narrow-phase collision detection between a sphere and a capsule
+// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
+// by Dirk Gregorius.
 bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) {
 
     bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE;

src/collision/narrowphase/SphereVsCapsuleAlgorithm.h

@@ -60,7 +60,7 @@ class SphereVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
         /// Deleted assignment operator
 		SphereVsCapsuleAlgorithm& operator=(const SphereVsCapsuleAlgorithm& algorithm) = delete;
 
-        /// Compute a contact info if the two bounding volume collide
+        /// Compute the narrow-phase collision detection between a sphere and a capsule
         virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
                                    ContactManifoldInfo& contactManifoldInfo) override;
 };

src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp

@@ -31,13 +31,12 @@
 // We want to use the ReactPhysics3D namespace
 using namespace reactphysics3d;
 
+// Compute the narrow-phase collision detection a sphere and a convex polyhedron
+// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
+// by Dirk Gregorius.
 bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
                                                       ContactManifoldInfo& contactManifoldInfo) {
 
-    // Get the local-space to world-space transforms
-    const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform;
-    const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform;
-
     // First, we run the GJK algorithm
     GJKAlgorithm gjkAlgorithm;
     GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo);

src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h

@@ -60,7 +60,7 @@ class SphereVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
         /// Deleted assignment operator
         SphereVsConvexPolyhedronAlgorithm& operator=(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete;
 
-        /// Compute a contact info if the two bounding volume collide
+        /// Compute the narrow-phase collision detection a sphere and a convex polyhedron
         virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
                                    ContactManifoldInfo& contactManifoldInfo) override;
 };

src/collision/shapes/BoxShape.h

@@ -128,6 +128,9 @@ class BoxShape : public ConvexPolyhedronShape {
 
         /// Return the normal vector of a given face of the polyhedron
         virtual Vector3 getFaceNormal(uint faceIndex) const override;
+
+        /// Return the centroid of the polyhedron
+        virtual Vector3 getCentroid() const override;
 };
 
 // Return the extents of the box
@@ -236,6 +239,11 @@ inline Vector3 BoxShape::getFaceNormal(uint faceIndex) const {
     }
 }
 
+// Return the centroid of the box
+inline Vector3 BoxShape::getCentroid() const {
+    return Vector3::zero();
+}
+
 // Return the number of half-edges of the polyhedron
 inline uint BoxShape::getNbHalfEdges() const {
     return 24;

src/collision/shapes/ConvexMeshShape.h

@@ -142,6 +142,9 @@ class ConvexMeshShape : public ConvexPolyhedronShape {
 
         /// Return the normal vector of a given face of the polyhedron
         virtual Vector3 getFaceNormal(uint faceIndex) const override;
+
+        /// Return the centroid of the polyhedron
+        virtual Vector3 getCentroid() const override;
 };
 
 /// Set the scaling vector of the collision shape
@@ -239,6 +242,11 @@ inline Vector3 ConvexMeshShape::getFaceNormal(uint faceIndex) const {
     return mPolyhedronMesh->getFaceNormal(faceIndex);
 }
 
+// Return the centroid of the polyhedron
+inline Vector3 ConvexMeshShape::getCentroid() const {
+    return mPolyhedronMesh->getCentroid();
+}
+
 }
 
 #endif

src/collision/shapes/ConvexPolyhedronShape.h

@@ -88,6 +88,9 @@ class ConvexPolyhedronShape : public ConvexShape {
 
         /// Return true if the collision shape is a polyhedron
         virtual bool isPolyhedron() const override;
+
+        /// Return the centroid of the polyhedron
+        virtual Vector3 getCentroid() const=0;
 };
 
 // Return true if the collision shape is a polyhedron

src/collision/shapes/ConvexShape.h

@@ -81,7 +81,7 @@ class ConvexShape : public CollisionShape {
         // -------------------- Friendship -------------------- //
 
         friend class GJKAlgorithm;
-        friend class EPAAlgorithm;
+        friend class SATAlgorithm;
 };
 
 // Return true if the collision shape is convex, false if it is concave

src/mathematics/mathematics_functions.cpp

@@ -60,6 +60,11 @@ Vector3 reactphysics3d::clamp(const Vector3& vector, decimal maxLength) {
     return vector;
 }
 
+/// Return true if two vectors are parallel
+bool reactphysics3d::areParallelVectors(const Vector3& vector1, const Vector3& vector2) {
+    return vector1.cross(vector2).lengthSquare() < decimal(0.00001);
+}
+
 /// Compute and return a point on segment from "segPointA" and "segPointB" that is closest to point "pointC"
 Vector3 reactphysics3d::computeClosestPointOnSegment(const Vector3& segPointA, const Vector3& segPointB, const Vector3& pointC) {
 

src/mathematics/mathematics_functions.h

@@ -76,6 +76,9 @@ inline bool sameSign(decimal a, decimal b) {
     return a * b >= decimal(0.0);
 }
 
+/// Return true if two vectors are parallel
+bool areParallelVectors(const Vector3& vector1, const Vector3& vector2);
+
 /// Clamp a vector such that it is no longer than a given maximum length
 Vector3 clamp(const Vector3& vector, decimal maxLength);