Fix issues in collision detection

src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp

@@ -29,6 +29,7 @@
 #include "GJK/GJKAlgorithm.h"
 #include "collision/shapes/CapsuleShape.h"
 #include "collision/shapes/ConvexPolyhedronShape.h"
+#include <cassert>
 
 // We want to use the ReactPhysics3D namespace
 using namespace reactphysics3d;
@@ -47,40 +48,52 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na
     narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true;
     narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false;
 
+	assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
+		   narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
+	assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE ||
+		   narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE);
+
     // 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)
+        // GJK has found a shallow contact. If the face of the polyhedron mesh is orthogonal to the
+		// capsule inner segment and parallel to the contact point normal, 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();
+		assert(contactPoint != nullptr);
 
         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;
+			const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform;
 
             // 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)) {
+			const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0);
+			const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0);
+			const Vector3 capsuleInnerSegmentWorld = capsuleToWorld.getOrientation() * (capsuleSegB - capsuleSegA);
+
+			bool isFaceNormalInDirectionOfContactNormal = faceNormalWorld.dot(contactPoint->normal) > decimal(0.0);
+			bool isFaceNormalInContactDirection = (isCapsuleShape1 && !isFaceNormalInDirectionOfContactNormal) || (!isCapsuleShape1 && isFaceNormalInDirectionOfContactNormal);
+	
+            // If the polyhedron face normal is orthogonal to the capsule inner segment and parallel to the contact point normal and the face normal
+			// is in direction of the contact normal (from the polyhedron point of view).
+            if (isFaceNormalInContactDirection && areOrthogonalVectors(faceNormalWorld, capsuleInnerSegmentWorld)
+				&& areParallelVectors(faceNormalWorld, contactPoint->normal)) {
 
                 // Remove the previous contact point computed by GJK
                 contactManifoldInfo.reset();

src/collision/narrowphase/SAT/SATAlgorithm.cpp

@@ -296,7 +296,7 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo*
         }
     }
 
-    // We the shapes are still overlapping in the same axis as in
+    // If the shapes are still overlapping in the same axis as in the previous frame
     // the previous frame, we skip the whole SAT algorithm
     if (!isTemporalCoherenceValid) {
 
@@ -470,7 +470,7 @@ decimal SATAlgorithm::computePolyhedronFaceVsCapsulePenetrationDepth(uint polyhe
     const Vector3 capsuleSupportPoint = capsule->getLocalSupportPointWithMargin(-outFaceNormalCapsuleSpace, nullptr);
     const Vector3 pointOnPolyhedronFace = polyhedronToCapsuleTransform * polyhedron->getVertexPosition(face.faceVertices[0]);
     const Vector3 capsuleSupportPointToFacePoint =  pointOnPolyhedronFace - capsuleSupportPoint;
-    const decimal penetrationDepth = capsuleSupportPointToFacePoint.dot(faceNormal);
+    const decimal penetrationDepth = capsuleSupportPointToFacePoint.dot(outFaceNormalCapsuleSpace);
 
     return penetrationDepth;
 }

src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp

@@ -41,6 +41,11 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* nar
     GJKAlgorithm gjkAlgorithm;
     GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo);
 
+	assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
+		narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
+	assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE ||
+		narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE);
+
     narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true;
     narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false;
 

src/collision/shapes/BoxShape.cpp

@@ -57,15 +57,15 @@ BoxShape::BoxShape(const Vector3& extent, decimal margin)
     std::vector<uint> face0;
     face0.push_back(0); face0.push_back(1); face0.push_back(2); face0.push_back(3);
     std::vector<uint> face1;
-    face1.push_back(1); face0.push_back(5); face0.push_back(6); face0.push_back(2);
+    face1.push_back(1); face1.push_back(5); face1.push_back(6); face1.push_back(2);
     std::vector<uint> face2;
-    face2.push_back(4); face0.push_back(7); face0.push_back(6); face0.push_back(5);
+    face2.push_back(4); face2.push_back(7); face2.push_back(6); face2.push_back(5);
     std::vector<uint> face3;
-    face3.push_back(4); face0.push_back(0); face0.push_back(3); face0.push_back(7);
+    face3.push_back(4); face3.push_back(0); face3.push_back(3); face3.push_back(7);
     std::vector<uint> face4;
-    face4.push_back(4); face0.push_back(5); face0.push_back(1); face0.push_back(0);
+    face4.push_back(4); face4.push_back(5); face4.push_back(1); face4.push_back(0);
     std::vector<uint> face5;
-    face5.push_back(2); face0.push_back(6); face0.push_back(7); face0.push_back(3);
+    face5.push_back(2); face5.push_back(6); face5.push_back(7); face5.push_back(3);
 
     mHalfEdgeStructure.addFace(face0);
     mHalfEdgeStructure.addFace(face1);
@@ -73,6 +73,8 @@ BoxShape::BoxShape(const Vector3& extent, decimal margin)
     mHalfEdgeStructure.addFace(face3);
     mHalfEdgeStructure.addFace(face4);
     mHalfEdgeStructure.addFace(face5);
+
+	mHalfEdgeStructure.init();
 }
 
 // Return the local inertia tensor of the collision shape

src/mathematics/mathematics_functions.cpp

@@ -65,6 +65,11 @@ bool reactphysics3d::areParallelVectors(const Vector3& vector1, const Vector3& v
     return vector1.cross(vector2).lengthSquare() < decimal(0.00001);
 }
 
+/// Return true if two vectors are orthogonal
+bool reactphysics3d::areOrthogonalVectors(const Vector3& vector1, const Vector3& vector2) {
+	return std::abs(vector1.dot(vector2)) < 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

@@ -79,6 +79,9 @@ inline bool sameSign(decimal a, decimal b) {
 /// Return true if two vectors are parallel
 bool areParallelVectors(const Vector3& vector1, const Vector3& vector2);
 
+/// Return true if two vectors are orthogonal
+bool areOrthogonalVectors(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);
 

testbed/scenes/collisiondetection/CollisionDetectionScene.cpp

@@ -61,7 +61,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
     mSphere1->setSleepingColor(mRedColorDemo);
 
     // ---------- Sphere 2 ---------- //
-    openglframework::Vector3 position2(0, 0, 0);
+    openglframework::Vector3 position2(12, 8, 0);
 
     // Create a sphere and a corresponding collision body in the dynamics world
     mSphere2 = new Sphere(2, position2, mCollisionWorld, mMeshFolderPath);
@@ -72,7 +72,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
     mSphere2->setSleepingColor(mRedColorDemo);
 
 	// ---------- Capsule 1 ---------- //
-	openglframework::Vector3 position3(8, 0, 0);
+	openglframework::Vector3 position3(0, -12, 0);
 
 	// Create a cylinder and a corresponding collision body in the dynamics world
 	mCapsule1 = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position3, mCollisionWorld, mMeshFolderPath);
@@ -94,7 +94,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
     mCapsule2->setSleepingColor(mRedColorDemo);
 
 	// ---------- Box 1 ---------- //
-	openglframework::Vector3 position5(0, -12, 0);
+	openglframework::Vector3 position5(0, -0, 0);
 
 	// Create a cylinder and a corresponding collision body in the dynamics world
 	mBox1 = new Box(BOX_SIZE, position5, mCollisionWorld, mMeshFolderPath);
@@ -105,7 +105,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
 	mBox1->setSleepingColor(mRedColorDemo);
 
 	// ---------- Box 2 ---------- //
-	openglframework::Vector3 position6(0, 12, 0);
+	openglframework::Vector3 position6(0, 8, 0);
 
 	// Create a cylinder and a corresponding collision body in the dynamics world
 	mBox2 = new Box(openglframework::Vector3(3, 2, 5), position6, mCollisionWorld, mMeshFolderPath);