Make TriangleShape inherits from ConvexPolyhedronShape

src/collision/HalfEdgeStructure.h

@@ -53,6 +53,9 @@ class HalfEdgeStructure {
             uint edgeIndex;                     // Index of an half-edge of the face
             std::vector<uint> faceVertices;     // Index of the vertices of the face
 
+            /// Constructor
+            Face() {}
+
             /// Constructor
             Face(std::vector<uint> vertices) : faceVertices(vertices) {}
         };
@@ -155,7 +158,7 @@ inline HalfEdgeStructure::Edge HalfEdgeStructure::getHalfEdge(uint index) const
     return mEdges[index];
 }
 
-// Retunr a given vertex
+// Return a given vertex
 inline HalfEdgeStructure::Vertex HalfEdgeStructure::getVertex(uint index) const {
     assert(index < mVertices.size());
     return mVertices[index];

src/collision/narrowphase/SAT/SATAlgorithm.cpp

@@ -46,6 +46,8 @@ const decimal SATAlgorithm::SAME_SEPARATING_AXIS_BIAS = decimal(0.001);
 // Test collision between a sphere and a convex mesh
 bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
 
+    PROFILE("SATAlgorithm::testCollisionSphereVsConvexPolyhedron()");
+
     bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE;
 
     assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
@@ -77,32 +79,37 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo*
 
     LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo();
 
-    // If the last frame collision info is valid and was also using SAT algorithm
+    // If the shapes are not triangles (no temporal coherence for triangle collision because we do not store previous
-    if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) {
+    // frame collision data per triangle)
+    if (polyhedron->getType() != CollisionShapeType::TRIANGLE) {
 
-        // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating
+        // If the last frame collision info is valid and was also using SAT algorithm
-        // axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If
+        if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) {
-        // the shapes are still separated along this axis, we directly exit with no collision.
 
-        // Compute the penetration depth of the shapes along the face normal direction
+            // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating
-        decimal penetrationDepth = computePolyhedronFaceVsSpherePenetrationDepth(lastFrameInfo.satMinAxisFaceIndex, polyhedron,
+            // axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If
-                                                                                 sphere, sphereCenter);
+            // the shapes are still separated along this axis, we directly exit with no collision.
 
-        // If the previous axis is a separating axis
+            // Compute the penetration depth of the shapes along the face normal direction
-        if (penetrationDepth <= decimal(0.0)) {
+            decimal penetrationDepth = computePolyhedronFaceVsSpherePenetrationDepth(lastFrameInfo.satMinAxisFaceIndex, polyhedron,
+                                                                                     sphere, sphereCenter);
 
-            // Return no collision
+            // If the previous axis is a separating axis
-            return false;
+            if (penetrationDepth <= decimal(0.0)) {
-        }
 
-        // The two shapes are overlapping as in the previous frame and on the same axis, therefore
+                // Return no collision
-        // we will skip the entire SAT algorithm because the minimum separating axis did not change
+                return false;
-        isTemporalCoherenceValid = lastFrameInfo.wasColliding;
+            }
 
-        if (isTemporalCoherenceValid) {
+            // The two shapes are overlapping as in the previous frame and on the same axis, therefore
+            // we will skip the entire SAT algorithm because the minimum separating axis did not change
+            isTemporalCoherenceValid = lastFrameInfo.wasColliding;
 
-            minPenetrationDepth = penetrationDepth;
+            if (isTemporalCoherenceValid) {
-            minFaceIndex = lastFrameInfo.satMinAxisFaceIndex;
+
+                minPenetrationDepth = penetrationDepth;
+                minFaceIndex = lastFrameInfo.satMinAxisFaceIndex;
+            }
         }
     }
 
@@ -170,6 +177,8 @@ decimal SATAlgorithm::computePolyhedronFaceVsSpherePenetrationDepth(uint faceInd
 // Test collision between a capsule and a convex mesh
 bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
 
+    PROFILE("SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron()");
+
     bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE;
 
     assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
@@ -206,78 +215,83 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo*
 
     LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo();
 
-    // If the last frame collision info is valid and was also using SAT algorithm
+    // If the shapes are not triangles (no temporal coherence for triangle collision because we do not store previous
-    if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) {
+    // frame collision data per triangle)
+    if (polyhedron->getType() != CollisionShapeType::TRIANGLE) {
 
-        // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating
+        // If the last frame collision info is valid and was also using SAT algorithm
-        // axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If
+        if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) {
-        // the shapes are still separated along this axis, we directly exit with no collision.
 
-        // If the previous minimum separation axis was a face normal of the polyhedron
+            // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating
-        if (lastFrameInfo.satIsAxisFacePolyhedron1) {
+            // axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If
+            // the shapes are still separated along this axis, we directly exit with no collision.
 
-            Vector3 outFaceNormalCapsuleSpace;
+            // If the previous minimum separation axis was a face normal of the polyhedron
+            if (lastFrameInfo.satIsAxisFacePolyhedron1) {
 
-            // Compute the penetration depth along the polyhedron face normal direction
+                Vector3 outFaceNormalCapsuleSpace;
-            const decimal penetrationDepth = computePolyhedronFaceVsCapsulePenetrationDepth(lastFrameInfo.satMinAxisFaceIndex, polyhedron,
-                                                                                            capsuleShape, polyhedronToCapsuleTransform,
-                                                                                            outFaceNormalCapsuleSpace);
 
-            // If the previous axis is a separating axis
+                // Compute the penetration depth along the polyhedron face normal direction
-            if (penetrationDepth <= decimal(0.0)) {
+                const decimal penetrationDepth = computePolyhedronFaceVsCapsulePenetrationDepth(lastFrameInfo.satMinAxisFaceIndex, polyhedron,
+                                                                                                capsuleShape, polyhedronToCapsuleTransform,
+                                                                                                outFaceNormalCapsuleSpace);
 
-                // Return no collision
+                // If the previous axis is a separating axis
-                return false;
+                if (penetrationDepth <= decimal(0.0)) {
+
+                    // Return no collision
+                    return false;
+                }
+
+                // The two shapes are overlapping as in the previous frame and on the same axis, therefore
+                // we will skip the entire SAT algorithm because the minimum separating axis did not change
+                isTemporalCoherenceValid = lastFrameInfo.wasColliding;
+
+                if (isTemporalCoherenceValid) {
+
+                    minPenetrationDepth = penetrationDepth;
+                    minFaceIndex = lastFrameInfo.satMinAxisFaceIndex;
+                    isMinPenetrationFaceNormal = true;
+                    separatingAxisCapsuleSpace = outFaceNormalCapsuleSpace;
+                }
             }
+            else {   // If the previous minimum separation axis the cross product of the capsule inner segment and an edge of the polyhedron
 
-            // The two shapes are overlapping as in the previous frame and on the same axis, therefore
+                // Get an edge from the polyhedron (convert it into the capsule local-space)
-            // we will skip the entire SAT algorithm because the minimum separating axis did not change
+                HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(lastFrameInfo.satMinEdge1Index);
-            isTemporalCoherenceValid = lastFrameInfo.wasColliding;
+                const Vector3 edgeVertex1 = polyhedron->getVertexPosition(edge.vertexIndex);
+                const Vector3 edgeVertex2 = polyhedron->getVertexPosition(polyhedron->getHalfEdge(edge.nextEdgeIndex).vertexIndex);
+                const Vector3 edgeDirectionCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * (edgeVertex2 - edgeVertex1);
 
-            if (isTemporalCoherenceValid) {
+                Vector3 outAxis;
 
-                minPenetrationDepth = penetrationDepth;
+                // Compute the penetration depth along this axis
-                minFaceIndex = lastFrameInfo.satMinAxisFaceIndex;
+                const decimal penetrationDepth = computeEdgeVsCapsuleInnerSegmentPenetrationDepth(polyhedron, capsuleShape,
-                isMinPenetrationFaceNormal = true;
+                                                                                                  capsuleSegmentAxis, edgeVertex1,
-                separatingAxisCapsuleSpace = outFaceNormalCapsuleSpace;
+                                                                                                  edgeDirectionCapsuleSpace,
-            }
+                                                                                                  polyhedronToCapsuleTransform,
-        }
+                                                                                                  outAxis);
-        else {   // If the previous minimum separation axis the cross product of the capsule inner segment and an edge of the polyhedron
 
-            // Get an edge from the polyhedron (convert it into the capsule local-space)
+                // If the previous axis is a separating axis
-            HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(lastFrameInfo.satMinEdge1Index);
+                if (penetrationDepth <= decimal(0.0)) {
-            const Vector3 edgeVertex1 = polyhedron->getVertexPosition(edge.vertexIndex);
-            const Vector3 edgeVertex2 = polyhedron->getVertexPosition(polyhedron->getHalfEdge(edge.nextEdgeIndex).vertexIndex);
-            const Vector3 edgeDirectionCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * (edgeVertex2 - edgeVertex1);
 
-            Vector3 outAxis;
+                    // Return no collision
+                    return false;
+                }
 
-            // Compute the penetration depth along this axis
+                // The two shapes are overlapping as in the previous frame and on the same axis, therefore
-            const decimal penetrationDepth = computeEdgeVsCapsuleInnerSegmentPenetrationDepth(polyhedron, capsuleShape,
+                // we will skip the entire SAT algorithm because the minimum separating axis did not change
-                                                                                              capsuleSegmentAxis, edgeVertex1,
+                isTemporalCoherenceValid = lastFrameInfo.wasColliding;
-                                                                                              edgeDirectionCapsuleSpace,
-                                                                                              polyhedronToCapsuleTransform,
-                                                                                              outAxis);
 
-            // If the previous axis is a separating axis
+                if (isTemporalCoherenceValid) {
-            if (penetrationDepth <= decimal(0.0)) {
 
-                // Return no collision
+                    minPenetrationDepth = penetrationDepth;
-                return false;
+                    minEdgeIndex = lastFrameInfo.satMinEdge1Index;
-            }
+                    isMinPenetrationFaceNormal = false;
-
+                    separatingAxisCapsuleSpace = outAxis;
-            // The two shapes are overlapping as in the previous frame and on the same axis, therefore
+                    separatingPolyhedronEdgeVertex1 = edgeVertex1;
-            // we will skip the entire SAT algorithm because the minimum separating axis did not change
+                    separatingPolyhedronEdgeVertex2 = edgeVertex2;
-            isTemporalCoherenceValid = lastFrameInfo.wasColliding;
+                }
-
-            if (isTemporalCoherenceValid) {
-
-                minPenetrationDepth = penetrationDepth;
-                minEdgeIndex = lastFrameInfo.satMinEdge1Index;
-                isMinPenetrationFaceNormal = false;
-                separatingAxisCapsuleSpace = outAxis;
-                separatingPolyhedronEdgeVertex1 = edgeVertex1;
-                separatingPolyhedronEdgeVertex2 = edgeVertex2;
             }
         }
     }
@@ -527,6 +541,8 @@ bool SATAlgorithm::isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, c
 bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo,
                                                                    ContactManifoldInfo& contactManifoldInfo) const {
 
+    PROFILE("SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron()");
+
     assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
     assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
 

src/collision/shapes/ConvexPolyhedronShape.h

@@ -42,10 +42,6 @@ class ConvexPolyhedronShape : public ConvexShape {
 
     protected :
 
-        // -------------------- Attributes -------------------- //
-
-        // -------------------- Methods -------------------- //
-
     public :
 
         // -------------------- Methods -------------------- //

src/collision/shapes/TriangleShape.cpp

@@ -40,10 +40,16 @@ using namespace reactphysics3d;
  * @param margin The collision margin (in meters) around the collision shape
  */
 TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, decimal margin)
-              : ConvexShape(CollisionShapeType::TRIANGLE, margin) {
+              : ConvexPolyhedronShape(margin) {
+
     mPoints[0] = point1;
     mPoints[1] = point2;
     mPoints[2] = point3;
+
+    // Compute the triangle normal
+    mNormal = (point3 - point1).cross(point2 - point1);
+    mNormal.normalize();
+
     mRaycastTestType = TriangleRaycastSide::FRONT;
 }
 
@@ -120,3 +126,51 @@ bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
     return true;
 }
 
+// Return a given half-edge of the polyhedron
+HalfEdgeStructure::Edge TriangleShape::getHalfEdge(uint edgeIndex) const {
+    assert(edgeIndex < getNbHalfEdges());
+
+    HalfEdgeStructure::Edge edge;
+
+    switch(edgeIndex) {
+        case 0:
+            edge.vertexIndex = 0;
+            edge.twinEdgeIndex = 1;
+            edge.faceIndex = 0;
+            edge.nextEdgeIndex = 2;
+            break;
+        case 1:
+            edge.vertexIndex = 1;
+            edge.twinEdgeIndex = 0;
+            edge.faceIndex = 1;
+            edge.nextEdgeIndex = 5;
+            break;
+        case 2:
+            edge.vertexIndex = 1;
+            edge.twinEdgeIndex = 3;
+            edge.faceIndex = 0;
+            edge.nextEdgeIndex = 4;
+            break;
+        case 3:
+            edge.vertexIndex = 2;
+            edge.twinEdgeIndex = 2;
+            edge.faceIndex = 1;
+            edge.nextEdgeIndex = 1;
+            break;
+        case 4:
+            edge.vertexIndex = 2;
+            edge.twinEdgeIndex = 5;
+            edge.faceIndex = 0;
+            edge.nextEdgeIndex = 0;
+            break;
+        case 5:
+            edge.vertexIndex = 0;
+            edge.twinEdgeIndex = 4;
+            edge.faceIndex = 1;
+            edge.nextEdgeIndex = 3;
+            break;
+    }
+
+    return edge;
+
+}

src/collision/shapes/TriangleShape.h

@@ -28,7 +28,7 @@
 
 // Libraries
 #include "mathematics/mathematics.h"
-#include "ConvexShape.h"
+#include "ConvexPolyhedronShape.h"
 
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
@@ -51,7 +51,7 @@ enum class TriangleRaycastSide {
  * This class represents a triangle collision shape that is centered
  * at the origin and defined three points.
  */
-class TriangleShape : public ConvexShape {
+class TriangleShape : public ConvexPolyhedronShape {
 
     protected:
 
@@ -60,6 +60,9 @@ class TriangleShape : public ConvexShape {
         /// Three points of the triangle
         Vector3 mPoints[3];
 
+        /// Normal of the triangle
+        Vector3 mNormal;
+
         /// Raycast test type for the triangle (front, back, front-back)
         TriangleRaycastSide mRaycastTestType;
 
@@ -113,11 +116,32 @@ class TriangleShape : public ConvexShape {
         // Set the raycast test type (front, back, front-back)
         void setRaycastTestType(TriangleRaycastSide testType);
 
-        /// Return the coordinates of a given vertex of the triangle
+        /// Return the number of faces of the polyhedron
-        Vector3 getVertex(int index) const;
+        virtual uint getNbFaces() const override;
 
-        /// Return true if the collision shape is a polyhedron
+        /// Return a given face of the polyhedron
-        virtual bool isPolyhedron() const override;
+        virtual HalfEdgeStructure::Face getFace(uint faceIndex) const override;
+
+        /// Return the number of vertices of the polyhedron
+        virtual uint getNbVertices() const override;
+
+        /// Return a given vertex of the polyhedron
+        virtual HalfEdgeStructure::Vertex getVertex(uint vertexIndex) const override;
+
+        /// Return the position of a given vertex
+        virtual Vector3 getVertexPosition(uint vertexIndex) const override;
+
+        /// Return the normal vector of a given face of the polyhedron
+        virtual Vector3 getFaceNormal(uint faceIndex) const override;
+
+        /// Return the number of half-edges of the polyhedron
+        virtual uint getNbHalfEdges() const override;
+
+        /// Return a given half-edge of the polyhedron
+        virtual HalfEdgeStructure::Edge getHalfEdge(uint edgeIndex) const override;
+
+        /// Return the centroid of the polyhedron
+        virtual Vector3 getCentroid() const override;
 
         // ---------- Friendship ---------- //
 
@@ -199,6 +223,74 @@ inline bool TriangleShape::testPointInside(const Vector3& localPoint, ProxyShape
     return false;
 }
 
+// Return the number of faces of the polyhedron
+inline uint TriangleShape::getNbFaces() const {
+    return 2;
+}
+
+// Return a given face of the polyhedron
+inline HalfEdgeStructure::Face TriangleShape::getFace(uint faceIndex) const {
+    assert(faceIndex < 2);
+
+    HalfEdgeStructure::Face face;
+
+    if (faceIndex == 0) {
+        face.faceVertices.push_back(0);
+        face.faceVertices.push_back(1);
+        face.faceVertices.push_back(2);
+        face.edgeIndex = 0;
+
+    }
+    else {
+        face.faceVertices.push_back(0);
+        face.faceVertices.push_back(2);
+        face.faceVertices.push_back(1);
+        face.edgeIndex = 1;
+    }
+
+    return face;
+}
+
+// Return the number of vertices of the polyhedron
+inline uint TriangleShape::getNbVertices() const {
+    return 3;
+}
+
+// Return a given vertex of the polyhedron
+inline HalfEdgeStructure::Vertex TriangleShape::getVertex(uint vertexIndex) const {
+    assert(vertexIndex < 3);
+
+    HalfEdgeStructure::Vertex vertex(vertexIndex);
+    switch (vertexIndex) {
+        case 0: vertex.edgeIndex = 0; break;
+        case 1: vertex.edgeIndex = 2; break;
+        case 2: vertex.edgeIndex = 4; break;
+    }
+    return vertex;
+}
+
+// Return the position of a given vertex
+inline Vector3 TriangleShape::getVertexPosition(uint vertexIndex) const {
+    assert(vertexIndex < 3);
+    return mPoints[vertexIndex];
+}
+
+// Return the normal vector of a given face of the polyhedron
+inline Vector3 TriangleShape::getFaceNormal(uint faceIndex) const {
+    assert(faceIndex < 2);
+    return faceIndex == 0 ? mNormal : -mNormal;
+}
+
+// Return the centroid of the box
+inline Vector3 TriangleShape::getCentroid() const {
+    return (mPoints[0] + mPoints[1] + mPoints[2]) / decimal(3.0);
+}
+
+// Return the number of half-edges of the polyhedron
+inline uint TriangleShape::getNbHalfEdges() const {
+    return 6;
+}
+
 // Return the raycast test type (front, back, front-back)
 inline TriangleRaycastSide TriangleShape::getRaycastTestType() const {
     return mRaycastTestType;
@@ -212,20 +304,6 @@ inline void TriangleShape::setRaycastTestType(TriangleRaycastSide testType) {
     mRaycastTestType = testType;
 }
 
-// Return the coordinates of a given vertex of the triangle
-/**
- * @param index Index (0 to 2) of a vertex of the triangle
- */
-inline Vector3 TriangleShape::getVertex(int index) const {
-    assert(index >= 0 && index < 3);
-    return mPoints[index];
-}
-
-// Return true if the collision shape is a polyhedron
-inline bool TriangleShape::isPolyhedron() const {
-    return true;
-}
-
 }
 
 #endif