From 6eec956eb0afcdf6cf0194da3c520158fd3aa0d4 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Fri, 21 Jul 2017 08:09:43 +0200
Subject: [PATCH] Fix issues in SAT algorithm between two convex polyhedra

---
 src/collision/narrowphase/SAT/SATAlgorithm.cpp | 15 ++++++++-------
 src/mathematics/mathematics_functions.cpp      |  5 +++++
 src/mathematics/mathematics_functions.h        |  3 +++
 3 files changed, 16 insertions(+), 7 deletions(-)

diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index 4e15eb15..c838d8ab 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -854,18 +854,19 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
         assert(clipPolygonVertices.size() > 0);
 
         // We only keep the clipped points that are below the reference face
-        const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(firstEdgeIndex);
+        const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(referencePolyhedron->getHalfEdge(firstEdgeIndex).vertexIndex);
         std::vector<Vector3>::const_iterator itPoints;
         for (itPoints = clipPolygonVertices.begin(); itPoints != clipPolygonVertices.end(); ++itPoints) {
 
             // If the clip point is bellow the reference face
-            if (((*itPoints) - referenceFaceVertex).dot(axisReferenceSpace) < decimal(0.0)) {
+            if (((*itPoints) - referenceFaceVertex).dot(axisReferenceSpace) < decimal(0.0))
+			{
 
                 // Convert the clip incident polyhedron vertex into the incident polyhedron local-space
                 const Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * (*itPoints);
 
                 // Project the contact point onto the reference face
-                Vector3 contactPointReferencePolyhedron = (*itPoints) + axisReferenceSpace * minPenetrationDepth;
+				Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(*itPoints, axisReferenceSpace, referenceFaceVertex);
 
                 // Create a new contact point
                 contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth,
@@ -1011,8 +1012,8 @@ bool SATAlgorithm::testEdgesBuildMinkowskiFace(const ConvexPolyhedronShape* poly
                                                const ConvexPolyhedronShape* polyhedron2, const HalfEdgeStructure::Edge& edge2,
                                                const Transform& polyhedron1ToPolyhedron2) const {
 
-    const Vector3 a = polyhedron1ToPolyhedron2 * polyhedron1->getFaceNormal(edge1.faceIndex);
-    const Vector3 b = polyhedron1ToPolyhedron2 * polyhedron1->getFaceNormal(polyhedron1->getHalfEdge(edge1.twinEdgeIndex).faceIndex);
+    const Vector3 a = polyhedron1ToPolyhedron2.getOrientation() * polyhedron1->getFaceNormal(edge1.faceIndex);
+    const Vector3 b = polyhedron1ToPolyhedron2.getOrientation() * polyhedron1->getFaceNormal(polyhedron1->getHalfEdge(edge1.twinEdgeIndex).faceIndex);
 
     const Vector3 c = polyhedron2->getFaceNormal(edge2.faceIndex);
     const Vector3 d = polyhedron2->getFaceNormal(polyhedron2->getHalfEdge(edge2.twinEdgeIndex).faceIndex);
@@ -1020,12 +1021,12 @@ bool SATAlgorithm::testEdgesBuildMinkowskiFace(const ConvexPolyhedronShape* poly
     // Compute b.cross(a) using the edge direction
     const Vector3 edge1Vertex1 = polyhedron1->getVertexPosition(edge1.vertexIndex);
     const Vector3 edge1Vertex2 = polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.twinEdgeIndex).vertexIndex);
-    const Vector3 bCrossA = polyhedron1ToPolyhedron2.getOrientation() * (edge1Vertex2 - edge1Vertex1);
+    const Vector3 bCrossA = polyhedron1ToPolyhedron2.getOrientation() * (edge1Vertex1 - edge1Vertex2);
 
     // Compute d.cross(c) using the edge direction
     const Vector3 edge2Vertex1 = polyhedron2->getVertexPosition(edge2.vertexIndex);
     const Vector3 edge2Vertex2 = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.twinEdgeIndex).vertexIndex);
-    const Vector3 dCrossC = edge2Vertex2 - edge2Vertex1;
+    const Vector3 dCrossC = edge2Vertex1 - edge2Vertex2;
 
     // Test if the two arcs of the Gauss Map intersect (therefore forming a minkowski face)
     // Note that we negate the normals of the second polyhedron because we are looking at the
diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp
index 24329732..d627ed7b 100755
--- a/src/mathematics/mathematics_functions.cpp
+++ b/src/mathematics/mathematics_functions.cpp
@@ -354,4 +354,9 @@ std::vector<Vector3> reactphysics3d::clipPolygonWithPlanes(const std::vector<Vec
     return outputVertices;
 }
 
+// Project a point onto a plane that is given by a point and its unit length normal
+Vector3 reactphysics3d::projectPointOntoPlane(const Vector3& point, const Vector3& unitPlaneNormal, const Vector3& planePoint) {
+	return point - unitPlaneNormal.dot(point - planePoint) * unitPlaneNormal;
+}
+
 
diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h
index 5ee03a19..78b5ba67 100755
--- a/src/mathematics/mathematics_functions.h
+++ b/src/mathematics/mathematics_functions.h
@@ -112,6 +112,9 @@ std::vector<Vector3> clipSegmentWithPlanes(const Vector3& segA, const Vector3& s
 std::vector<Vector3> clipPolygonWithPlanes(const std::vector<Vector3>& polygonVertices, const std::vector<Vector3>& planesPoints,
                                            const std::vector<Vector3>& planesNormals);
 
+/// Project a point onto a plane that is given by a point and its unit length normal
+Vector3 projectPointOntoPlane(const Vector3& point, const Vector3& planeNormal, const Vector3& planePoint);
+
 }