Add sphere vs convex polyhedron test in SAT algorithm

CMakeLists.txt

@@ -84,14 +84,14 @@ SET (REACTPHYSICS3D_SOURCES
     "src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp"
     "src/collision/narrowphase/ConcaveVsConvexAlgorithm.h"
     "src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp"
-    "src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h"
+    "src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h"
-    "src/collision/narrowphase/SphereVsConvexMeshAlgorithm.cpp"
+    "src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp"
     "src/collision/shapes/AABB.h"
     "src/collision/shapes/AABB.cpp"
     "src/collision/shapes/ConvexShape.h"
     "src/collision/shapes/ConvexShape.cpp"
-    "src/collision/shapes/ConvexPolyhedron.h"
+    "src/collision/shapes/ConvexPolyhedronShape.h"
-    "src/collision/shapes/ConvexPolyhedron.cpp"
+    "src/collision/shapes/ConvexPolyhedronShape.cpp"
     "src/collision/shapes/ConcaveShape.h"
     "src/collision/shapes/ConcaveShape.cpp"
     "src/collision/shapes/BoxShape.h"

src/collision/PolyhedronMesh.cpp

@@ -111,6 +111,7 @@ Vector3 PolyhedronMesh::getVertex(uint index) const {
     return vertex;
 }
 
+// Compute the faces normals
 void PolyhedronMesh::computeFacesNormals() {
 
     // For each face
@@ -122,5 +123,6 @@ void PolyhedronMesh::computeFacesNormals() {
         const Vector3 vec1 = getVertex(face.faceVertices[1]) - getVertex(face.faceVertices[0]);
         const Vector3 vec2 = getVertex(face.faceVertices[2]) - getVertex(face.faceVertices[0]);
         mFacesNormals[f] = vec1.cross(vec2);
+        mFacesNormals[f].normalize();
     }
 }

src/collision/narrowphase/DefaultCollisionDispatch.cpp

@@ -52,6 +52,10 @@ NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int t
     if (shape1Type == CollisionShapeType::CAPSULE && shape2Type == CollisionShapeType::CAPSULE) {
         return &mCapsuleVsCapsuleAlgorithm;
     }
+    // Sphere vs Convex Polyhedron algorithm
+    if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::CONVEX_POLYHEDRON) {
+        return &mSphereVsConvexPolyhedronAlgorithm;
+    }
 
     return nullptr;
 }

src/collision/narrowphase/DefaultCollisionDispatch.h

@@ -30,7 +30,7 @@
 #include "CollisionDispatch.h"
 #include "ConcaveVsConvexAlgorithm.h"
 #include "SphereVsSphereAlgorithm.h"
-#include "SphereVsConvexMeshAlgorithm.h"
+#include "SphereVsConvexPolyhedronAlgorithm.h"
 #include "SphereVsCapsuleAlgorithm.h"
 #include "CapsuleVsCapsuleAlgorithm.h"
 #include "GJK/GJKAlgorithm.h"
@@ -51,7 +51,7 @@ class DefaultCollisionDispatch : public CollisionDispatch {
         SphereVsSphereAlgorithm mSphereVsSphereAlgorithm;
 
         /// Sphere vs Convex Mesh collision algorithm
-        SphereVsConvexMeshAlgorithm mSphereVsConvexMeshAlgorithm;
+        SphereVsConvexPolyhedronAlgorithm mSphereVsConvexPolyhedronAlgorithm;
 
         /// Sphere vs Capsule collision algorithm
         SphereVsCapsuleAlgorithm mSphereVsCapsuleAlgorithm;

src/collision/narrowphase/SAT/SATAlgorithm.cpp

@@ -27,6 +27,8 @@
 #include "SATAlgorithm.h"
 #include "constraint/ContactPoint.h"
 #include "collision/PolyhedronMesh.h"
+#include "collision/shapes/ConvexPolyhedronShape.h"
+#include "collision/shapes/SphereShape.h"
 #include "configuration.h"
 #include "engine/Profiler.h"
 #include <algorithm>
@@ -45,7 +47,7 @@ bool SATAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, Contact
         case CollisionShapeType::CONVEX_POLYHEDRON:
             return testCollisionConvexMeshVsConvexMesh(narrowPhaseInfo, contactManifoldInfo);
         case CollisionShapeType::SPHERE:
-            return testCollisionSphereVsConvexMesh(narrowPhaseInfo, contactManifoldInfo);
+            return testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo);
         case CollisionShapeType::CAPSULE:
             return testCollisionCapsuleVsConvexMesh(narrowPhaseInfo, contactManifoldInfo);
         case CollisionShapeType::TRIANGLE:
@@ -57,23 +59,81 @@ bool SATAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, Contact
 }
 
 // Test collision between a sphere and a convex mesh
-bool SATAlgorithm::testCollisionSphereVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
+bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
 
+    assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE);
+    assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
+
+    // Get the capsule collision shapes
+    const SphereShape* sphere = static_cast<const SphereShape*>(narrowPhaseInfo->collisionShape1);
+    const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(narrowPhaseInfo->collisionShape2);
+
+
+    // Get the transform from sphere local-space to polyhedron local-space
+    const Transform sphereToPolyhedronSpaceTransform = narrowPhaseInfo->shape2ToWorldTransform.getInverse() *
+                                                       narrowPhaseInfo->shape1ToWorldTransform;
+
+    // Transform the center of the sphere into the local-space of the convex polyhedron
+    const Vector3 sphereCenter = sphereToPolyhedronSpaceTransform.getPosition();
+
+    // Minimum penetration depth
+    decimal minPenetrationDepth = DECIMAL_LARGEST;
+    uint minFaceIndex = 0;
+
+    // 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);
+
+        Vector3 sphereCenterToFacePoint = polyhedron->getVertexPosition(face.faceVertices[0]) - sphereCenter;
+        decimal penetrationDepth = sphereCenterToFacePoint.dot(faceNormal) + sphere->getRadius();
+
+        // 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;
+        }
+    }
+
+    const Vector3 minFaceNormal = polyhedron->getFaceNormal(minFaceIndex);
+    const Vector3 normalWorld = -(narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minFaceNormal);
+    const Vector3 contactPointSphereLocal = narrowPhaseInfo->shape1ToWorldTransform.getInverse() * normalWorld * sphere->getRadius();
+    const Vector3 contactPointPolyhedronLocal = sphereCenter + minFaceNormal * (minPenetrationDepth - sphere->getRadius());
+
+    // Create the contact info object
+    contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth, contactPointSphereLocal, contactPointPolyhedronLocal);
+
+    return true;
 }
 
 // Test collision between a capsule and a convex mesh
 bool SATAlgorithm::testCollisionCapsuleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
 
+    assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE);
+    assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
 }
 
 // Test collision between a triangle and a convex mesh
 bool SATAlgorithm::testCollisionTriangleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
 
+    assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::TRIANGLE);
+    assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
 }
 
 // Test collision between two convex meshes
 bool SATAlgorithm::testCollisionConvexMeshVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
 
+    assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
+    assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
 }
 
 

src/collision/narrowphase/SAT/SATAlgorithm.h

@@ -47,7 +47,7 @@ class SATAlgorithm {
                                        const Vector3& c, const Vector3& d) const;
 
         /// Test collision between a sphere and a convex mesh
-        bool testCollisionSphereVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
+        bool testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
 
         /// Test collision between a capsule and a convex mesh
         bool testCollisionCapsuleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;

src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp

@@ -24,7 +24,7 @@
 ********************************************************************************/
 
 // Libraries
-#include "SphereVsConvexMeshAlgorithm.h"
+#include "SphereVsConvexPolyhedronAlgorithm.h"
 #include "SAT/SATAlgorithm.h"
 #include "collision/shapes/SphereShape.h"
 #include "collision/shapes/ConvexMeshShape.h"
@@ -32,7 +32,7 @@
 // We want to use the ReactPhysics3D namespace
 using namespace reactphysics3d;
 
-bool SphereVsConvexMeshAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
                                                 ContactManifoldInfo& contactManifoldInfo) {
 
     // Get the local-space to world-space transforms

src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h

@@ -35,12 +35,12 @@
 /// Namespace ReactPhysics3D
 namespace reactphysics3d {
 
-// Class SphereVsConvexMeshAlgorithm
+// Class SphereVsConvexPolyhedronAlgorithm
 /**
  * This class is used to compute the narrow-phase collision detection
- * between a sphere and a convex mesh.
+ * between a sphere and a convex polyhedron.
  */
-class SphereVsConvexMeshAlgorithm : public NarrowPhaseAlgorithm {
+class SphereVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
 
     protected :
 
@@ -49,16 +49,16 @@ class SphereVsConvexMeshAlgorithm : public NarrowPhaseAlgorithm {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        SphereVsConvexMeshAlgorithm() = default;
+        SphereVsConvexPolyhedronAlgorithm() = default;
 
         /// Destructor
-        virtual ~SphereVsConvexMeshAlgorithm() override = default;
+        virtual ~SphereVsConvexPolyhedronAlgorithm() override = default;
 
         /// Deleted copy-constructor
-        SphereVsConvexMeshAlgorithm(const SphereVsConvexMeshAlgorithm& algorithm) = delete;
+        SphereVsConvexPolyhedronAlgorithm(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete;
 
         /// Deleted assignment operator
-        SphereVsConvexMeshAlgorithm& operator=(const SphereVsConvexMeshAlgorithm& algorithm) = delete;
+        SphereVsConvexPolyhedronAlgorithm& operator=(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete;
 
         /// Compute a contact info if the two bounding volume collide
         virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,

src/collision/shapes/BoxShape.cpp

@@ -38,7 +38,7 @@ using namespace reactphysics3d;
  * @param margin The collision margin (in meters) around the collision shape
  */
 BoxShape::BoxShape(const Vector3& extent, decimal margin)
-         : ConvexPolyhedron(margin), mExtent(extent - Vector3(margin, margin, margin)) {
+         : ConvexPolyhedronShape(margin), mExtent(extent - Vector3(margin, margin, margin)) {
     assert(extent.x > decimal(0.0) && extent.x > margin);
     assert(extent.y > decimal(0.0) && extent.y > margin);
     assert(extent.z > decimal(0.0) && extent.z > margin);
@@ -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;
-    face0.push_back(1); face0.push_back(5); face0.push_back(6); face0.push_back(2);
+    face1.push_back(1); face0.push_back(5); face0.push_back(6); face0.push_back(2);
     std::vector<uint> face2;
-    face0.push_back(4); face0.push_back(7); face0.push_back(6); face0.push_back(5);
+    face2.push_back(4); face0.push_back(7); face0.push_back(6); face0.push_back(5);
     std::vector<uint> face3;
-    face0.push_back(4); face0.push_back(0); face0.push_back(3); face0.push_back(7);
+    face3.push_back(4); face0.push_back(0); face0.push_back(3); face0.push_back(7);
     std::vector<uint> face4;
-    face0.push_back(4); face0.push_back(5); face0.push_back(1); face0.push_back(0);
+    face4.push_back(4); face0.push_back(5); face0.push_back(1); face0.push_back(0);
     std::vector<uint> face5;
-    face0.push_back(2); face0.push_back(6); face0.push_back(7); face0.push_back(3);
+    face5.push_back(2); face0.push_back(6); face0.push_back(7); face0.push_back(3);
 
     mHalfEdgeStructure.addFace(face0);
     mHalfEdgeStructure.addFace(face1);

src/collision/shapes/BoxShape.h

@@ -28,7 +28,7 @@
 
 // Libraries
 #include <cfloat>
-#include "ConvexPolyhedron.h"
+#include "ConvexPolyhedronShape.h"
 #include "body/CollisionBody.h"
 #include "mathematics/mathematics.h"
 
@@ -50,7 +50,7 @@ namespace reactphysics3d {
  * constructor of the box shape. Otherwise, it is recommended to use the
  * default margin distance by not using the "margin" parameter in the constructor.
  */
-class BoxShape : public ConvexPolyhedron {
+class BoxShape : public ConvexPolyhedronShape {
 
     protected :
 

src/collision/shapes/CollisionShape.h

@@ -41,7 +41,7 @@ namespace reactphysics3d {
     
 /// Type of the collision shape
 enum class CollisionShapeType {TRIANGLE, SPHERE, CAPSULE, CONVEX_POLYHEDRON, CONCAVE_MESH, HEIGHTFIELD};
-const int NB_COLLISION_SHAPE_TYPES = 9;
+const int NB_COLLISION_SHAPE_TYPES = 6;
 
 // Declarations
 class ProxyShape;

src/collision/shapes/ConvexMeshShape.cpp

@@ -42,7 +42,7 @@ using namespace reactphysics3d;
  * @param margin Collision margin (in meters) around the collision shape
  */
 ConvexMeshShape::ConvexMeshShape(PolyhedronMesh* polyhedronMesh, decimal margin)
-                : ConvexPolyhedron(margin), mPolyhedronMesh(polyhedronMesh), mMinBounds(0, 0, 0), mMaxBounds(0, 0, 0) {
+                : ConvexPolyhedronShape(margin), mPolyhedronMesh(polyhedronMesh), mMinBounds(0, 0, 0), mMaxBounds(0, 0, 0) {
 
     // Recalculate the bounds of the mesh
     recalculateBounds();

src/collision/shapes/ConvexMeshShape.h

@@ -27,7 +27,7 @@
 #define REACTPHYSICS3D_CONVEX_MESH_SHAPE_H
 
 // Libraries
-#include "ConvexPolyhedron.h"
+#include "ConvexPolyhedronShape.h"
 #include "engine/CollisionWorld.h"
 #include "mathematics/mathematics.h"
 #include "collision/TriangleMesh.h"
@@ -59,7 +59,7 @@ class CollisionWorld;
  * with the addEdge() method. Then, you must use the setIsEdgesInformationUsed(true) method
  * in order to use the edges information for collision detection.
  */
-class ConvexMeshShape : public ConvexPolyhedron {
+class ConvexMeshShape : public ConvexPolyhedronShape {
 
     protected :
 

src/collision/shapes/ConvexPolyhedronShape.cpp

@@ -24,14 +24,14 @@
 ********************************************************************************/
 
 // Libraries
-#include "ConvexPolyhedron.h"
+#include "ConvexPolyhedronShape.h"
 
 
 // We want to use the ReactPhysics3D namespace
 using namespace reactphysics3d;
 
 // Constructor
-ConvexPolyhedron::ConvexPolyhedron(decimal margin)
+ConvexPolyhedronShape::ConvexPolyhedronShape(decimal margin)
             : ConvexShape(CollisionShapeType::CONVEX_POLYHEDRON, margin) {
 
 }

src/collision/shapes/ConvexPolyhedronShape.h

@@ -33,12 +33,12 @@
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
 
-// Class ConvexPolyhedron
+// Class ConvexPolyhedronShape
 /**
  * This abstract class represents a convex polyhedron collision shape associated with a
  * body that is used during the narrow-phase collision detection.
  */
-class ConvexPolyhedron : public ConvexShape {
+class ConvexPolyhedronShape : public ConvexShape {
 
     protected :
 
@@ -51,16 +51,16 @@ class ConvexPolyhedron : public ConvexShape {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ConvexPolyhedron(decimal margin);
+        ConvexPolyhedronShape(decimal margin);
 
         /// Destructor
-        virtual ~ConvexPolyhedron() override = default;
+        virtual ~ConvexPolyhedronShape() override = default;
 
         /// Deleted copy-constructor
-        ConvexPolyhedron(const ConvexPolyhedron& shape) = delete;
+        ConvexPolyhedronShape(const ConvexPolyhedronShape& shape) = delete;
 
         /// Deleted assignment operator
-        ConvexPolyhedron& operator=(const ConvexPolyhedron& shape) = delete;
+        ConvexPolyhedronShape& operator=(const ConvexPolyhedronShape& shape) = delete;
 
         /// Return the number of faces of the polyhedron
         virtual uint getNbFaces() const=0;
@@ -91,7 +91,7 @@ class ConvexPolyhedron : public ConvexShape {
 };
 
 // Return true if the collision shape is a polyhedron
-inline bool ConvexPolyhedron::isPolyhedron() const {
+inline bool ConvexPolyhedronShape::isPolyhedron() const {
     return true;
 }