Many small optimizations

src/collision/CollisionDetection.cpp

@@ -273,7 +273,7 @@ void CollisionDetection::computeNarrowPhase() {
             // Use the narrow-phase collision detection algorithm to check
             // if there really is a collision. If a collision occurs, the
             // notifyContact() callback method will be called.
-            if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, true)) {
+            if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, true, mSingleFrameAllocator)) {
 
                 // Add the contact points as a potential contact manifold into the pair                
                 currentNarrowPhaseInfo->addContactPointsAsPotentialContactManifold();
@@ -593,7 +593,7 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2)
                             // Use the narrow-phase collision detection algorithm to check
                             // if there really is a collision. If a collision occurs, the
                             // notifyContact() callback method will be called.
-                            isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false);
+                            isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false, mPoolAllocator);
                         }
                     }
 
@@ -688,7 +688,7 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl
                                     // Use the narrow-phase collision detection algorithm to check
                                     // if there really is a collision. If a collision occurs, the
                                     // notifyContact() callback method will be called.
-                                    isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false);
+                                    isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false, mPoolAllocator);
                                 }
                             }
 
@@ -767,7 +767,7 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body
                         // Use the narrow-phase collision detection algorithm to check
                         // if there really is a collision. If a collision occurs, the
                         // notifyContact() callback method will be called.
-                        if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true)) {
+                        if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mPoolAllocator)) {
 
                             // Add the contact points as a potential contact manifold into the pair
                             narrowPhaseInfo->addContactPointsAsPotentialContactManifold();
@@ -859,7 +859,7 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
                                 // Use the narrow-phase collision detection algorithm to check
                                 // if there really is a collision. If a collision occurs, the
                                 // notifyContact() callback method will be called.
-                                if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true)) {
+                                if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mPoolAllocator)) {
 
                                     // Add the contact points as a potential contact manifold into the pair
                                     narrowPhaseInfo->addContactPointsAsPotentialContactManifold();
@@ -943,7 +943,7 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
                     // Use the narrow-phase collision detection algorithm to check
                     // if there really is a collision. If a collision occurs, the
                     // notifyContact() callback method will be called.
-                    if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true)) {
+                    if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mPoolAllocator)) {
 
                         // Add the contact points as a potential contact manifold into the pair
                         narrowPhaseInfo->addContactPointsAsPotentialContactManifold();

src/collision/CollisionDetection.h

@@ -150,7 +150,6 @@ class CollisionDetection {
 
         /// Process the potential contacts where one collion is a concave shape
         void processSmoothMeshContacts(OverlappingPair* pair);
-
    
     public :
 

src/collision/HalfEdgeStructure.h

@@ -106,13 +106,13 @@ class HalfEdgeStructure {
         uint getNbVertices() const;
 
         /// Return a given face
-        Face getFace(uint index) const;
+        const Face& getFace(uint index) const;
 
         /// Return a given edge
-        Edge getHalfEdge(uint index) const;
+        const Edge& getHalfEdge(uint index) const;
 
         /// Return a given vertex
-        Vertex getVertex(uint index) const;
+        const Vertex& getVertex(uint index) const;
 
 };
 
@@ -133,33 +133,33 @@ inline void HalfEdgeStructure::addFace(std::vector<uint> faceVertices) {
 
 // Return the number of faces
 inline uint HalfEdgeStructure::getNbFaces() const {
-    return mFaces.size();
+    return static_cast<uint>(mFaces.size());
 }
 
 // Return the number of edges
 inline uint HalfEdgeStructure::getNbHalfEdges() const {
-    return mEdges.size();
+    return static_cast<uint>(mEdges.size());
 }
 
 // Return the number of vertices
 inline uint HalfEdgeStructure::getNbVertices() const {
-    return mVertices.size();
+    return static_cast<uint>(mVertices.size());
 }
 
 // Return a given face
-inline HalfEdgeStructure::Face HalfEdgeStructure::getFace(uint index) const {
+inline const HalfEdgeStructure::Face& HalfEdgeStructure::getFace(uint index) const {
     assert(index < mFaces.size());
     return mFaces[index];
 }
 
 // Return a given edge
-inline HalfEdgeStructure::Edge HalfEdgeStructure::getHalfEdge(uint index) const {
+inline const HalfEdgeStructure::Edge& HalfEdgeStructure::getHalfEdge(uint index) const {
     assert(index < mEdges.size());
     return mEdges[index];
 }
 
 // Return a given vertex
-inline HalfEdgeStructure::Vertex HalfEdgeStructure::getVertex(uint index) const {
+inline const HalfEdgeStructure::Vertex& HalfEdgeStructure::getVertex(uint index) const {
     assert(index < mVertices.size());
     return mVertices[index];
 }

src/collision/PolyhedronMesh.cpp

@@ -123,7 +123,7 @@ void PolyhedronMesh::computeFacesNormals() {
 
     // For each face
     for (uint f=0; f < mHalfEdgeStructure.getNbFaces(); f++) {
-        HalfEdgeStructure::Face face = mHalfEdgeStructure.getFace(f);
+        const HalfEdgeStructure::Face& face = mHalfEdgeStructure.getFace(f);
 
         assert(face.faceVertices.size() >= 3);
 

src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp

@@ -33,7 +33,8 @@ 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(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
+bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
+                                              Allocator& memoryAllocator) {
     
     assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE);
     assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE);

src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h

@@ -61,7 +61,7 @@ class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
 		CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete;
 
         /// Compute the narrow-phase collision detection between two capsules
-        virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override;
+        virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override;
 };
 
 }

src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp

@@ -37,11 +37,12 @@ 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(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
+bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
+                                                       Allocator& memoryAllocator) {
 
     // First, we run the GJK algorithm
     GJKAlgorithm gjkAlgorithm;
-	SATAlgorithm satAlgorithm;
+    SATAlgorithm satAlgorithm(memoryAllocator);
 
 #ifdef IS_PROFILING_ACTIVE
 
@@ -85,9 +86,6 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh
             // For each face of the polyhedron
             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;
 

src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h

@@ -61,7 +61,7 @@ class CapsuleVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
         CapsuleVsConvexPolyhedronAlgorithm& operator=(const CapsuleVsConvexPolyhedronAlgorithm& algorithm) = delete;
 
         /// Compute the narrow-phase collision detection between a capsule and a polyhedron
-        virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override;
+        virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override;
 };
 
 }

src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp

@@ -34,10 +34,11 @@ using namespace reactphysics3d;
 // Compute the narrow-phase collision detection between two convex polyhedra
 // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
 // by Dirk Gregorius.
-bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
+bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
+                                                                Allocator& memoryAllocator) {
 
     // Run the SAT algorithm to find the separating axis and compute contact point
-    SATAlgorithm satAlgorithm;
+    SATAlgorithm satAlgorithm(memoryAllocator);
 
 #ifdef IS_PROFILING_ACTIVE
 

src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h

@@ -61,7 +61,7 @@ class ConvexPolyhedronVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm
         ConvexPolyhedronVsConvexPolyhedronAlgorithm& operator=(const ConvexPolyhedronVsConvexPolyhedronAlgorithm& algorithm) = delete;
 
         /// Compute the narrow-phase collision detection between two convex polyhedra
-        virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override;
+        virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override;
 };
 
 }

src/collision/narrowphase/NarrowPhaseAlgorithm.h

@@ -90,8 +90,9 @@ class NarrowPhaseAlgorithm {
         /// Deleted assignment operator
         NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm) = delete;
 
-        /// Compute a contact info if the two bounding volume collide
+        /// Compute a contact info if the two bounding volumes collide
-        virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts)=0;
+        virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
+                                   Allocator& memoryAllocator)=0;
 
 #ifdef IS_PROFILING_ACTIVE
 

src/collision/narrowphase/SAT/SATAlgorithm.cpp

@@ -44,6 +44,11 @@ using namespace reactphysics3d;
 // Static variables initialization
 const decimal SATAlgorithm::SAME_SEPARATING_AXIS_BIAS = decimal(0.001);
 
+// Constructor
+SATAlgorithm::SATAlgorithm(Allocator& memoryAllocator) : mMemoryAllocator(memoryAllocator) {
+
+}
+
 // Test collision between a sphere and a convex mesh
 bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const {
 
@@ -125,7 +130,7 @@ decimal SATAlgorithm::computePolyhedronFaceVsSpherePenetrationDepth(uint faceInd
     PROFILE("SATAlgorithm::computePolyhedronFaceVsSpherePenetrationDepth)", mProfiler);
 
     // Get the face
-    HalfEdgeStructure::Face face = polyhedron->getFace(faceIndex);
+    const HalfEdgeStructure::Face& face = polyhedron->getFace(faceIndex);
 
     // Get the face normal
     const Vector3 faceNormal = polyhedron->getFaceNormal(faceIndex);
@@ -200,12 +205,12 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro
     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 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 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);
 
@@ -336,7 +341,7 @@ decimal SATAlgorithm::computePolyhedronFaceVsCapsulePenetrationDepth(uint polyhe
     PROFILE("SATAlgorithm::computePolyhedronFaceVsCapsulePenetrationDepth", mProfiler);
 
     // Get the face
-    HalfEdgeStructure::Face face = polyhedron->getFace(polyhedronFaceIndex);
+    const HalfEdgeStructure::Face& face = polyhedron->getFace(polyhedronFaceIndex);
 
     // Get the face normal
     const Vector3 faceNormal = polyhedron->getFaceNormal(polyhedronFaceIndex);
@@ -361,7 +366,7 @@ bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
 
     PROFILE("SATAlgorithm::computeCapsulePolyhedronFaceContactPoints", mProfiler);
 
-    HalfEdgeStructure::Face face = polyhedron->getFace(referenceFaceIndex);
+    const HalfEdgeStructure::Face& face = polyhedron->getFace(referenceFaceIndex);
 
     // Get the face normal
     Vector3 faceNormal = polyhedron->getFaceNormal(referenceFaceIndex);
@@ -375,8 +380,8 @@ bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
     // For each adjacent edge of the separating face of the polyhedron
     do {
 
-        HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(edgeIndex);
+        const HalfEdgeStructure::Edge& edge = polyhedron->getHalfEdge(edgeIndex);
-        HalfEdgeStructure::Edge twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex);
+        const HalfEdgeStructure::Edge& twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex);
 
         // Compute the edge vertices and edge direction
         Vector3 edgeV1 = polyhedron->getVertexPosition(edge.vertexIndex);
@@ -575,8 +580,8 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
         }
         else {   // If the previous separating axis (or axis with minimum penetration depth) was the cross product of two edges
 
-            HalfEdgeStructure::Edge edge1 = polyhedron1->getHalfEdge(lastFrameCollisionInfo->satMinEdge1Index);
+            const HalfEdgeStructure::Edge& edge1 = polyhedron1->getHalfEdge(lastFrameCollisionInfo->satMinEdge1Index);
-            HalfEdgeStructure::Edge edge2 = polyhedron2->getHalfEdge(lastFrameCollisionInfo->satMinEdge2Index);
+            const HalfEdgeStructure::Edge& edge2 = polyhedron2->getHalfEdge(lastFrameCollisionInfo->satMinEdge2Index);
 
             Vector3 separatingAxisPolyhedron2Space;
 
@@ -669,7 +674,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
         for (uint i=0; i < polyhedron1->getNbHalfEdges(); i += 2) {
 
             // Get an edge of polyhedron 1
-            HalfEdgeStructure::Edge edge1 = polyhedron1->getHalfEdge(i);
+            const HalfEdgeStructure::Edge& edge1 = polyhedron1->getHalfEdge(i);
 
             const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex);
             const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex);
@@ -678,7 +683,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
             for (uint j=0; j < polyhedron2->getNbHalfEdges(); j += 2) {
 
                 // Get an edge of polyhedron 2
-                HalfEdgeStructure::Edge edge2 = polyhedron2->getHalfEdge(j);
+                const HalfEdgeStructure::Edge& edge2 = polyhedron2->getHalfEdge(j);
 
                 const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex);
                 const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex);
@@ -816,23 +821,24 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene
                                     -(narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * axisReferenceSpace);
 
     // Get the reference face
-    HalfEdgeStructure::Face referenceFace = referencePolyhedron->getFace(minFaceIndex);
+    const HalfEdgeStructure::Face& referenceFace = referencePolyhedron->getFace(minFaceIndex);
 
     // Find the incident face on the other polyhedron (most anti-parallel face)
     uint incidentFaceIndex = findMostAntiParallelFaceOnPolyhedron(incidentPolyhedron, axisIncidentSpace);
 
     // Get the incident face
-    HalfEdgeStructure::Face incidentFace = incidentPolyhedron->getFace(incidentFaceIndex);
+    const HalfEdgeStructure::Face& incidentFace = incidentPolyhedron->getFace(incidentFaceIndex);
 
-    std::vector<Vector3> polygonVertices;   // Vertices to clip of the incident face
+    uint nbIncidentFaceVertices = static_cast<uint>(incidentFace.faceVertices.size());
-    std::vector<Vector3> planesNormals;     // Normals of the clipping planes
+    List<Vector3> polygonVertices(mMemoryAllocator, nbIncidentFaceVertices);   // Vertices to clip of the incident face
-    std::vector<Vector3> planesPoints;      // Points on the clipping planes
+    List<Vector3> planesNormals(mMemoryAllocator, nbIncidentFaceVertices);     // Normals of the clipping planes
+    List<Vector3> planesPoints(mMemoryAllocator, nbIncidentFaceVertices);      // Points on the clipping planes
 
     // Get all the vertices of the incident face (in the reference local-space)
     std::vector<uint>::const_iterator it;
     for (it = incidentFace.faceVertices.begin(); it != incidentFace.faceVertices.end(); ++it) {
         const Vector3 faceVertexIncidentSpace = incidentPolyhedron->getVertexPosition(*it);
-        polygonVertices.push_back(incidentToReferenceTransform * faceVertexIncidentSpace);
+        polygonVertices.add(incidentToReferenceTransform * faceVertexIncidentSpace);
     }
 
     // Get the reference face clipping planes
@@ -841,10 +847,10 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene
     do {
 
         // Get the adjacent edge
-        HalfEdgeStructure::Edge edge = referencePolyhedron->getHalfEdge(currentEdgeIndex);
+        const HalfEdgeStructure::Edge& edge = referencePolyhedron->getHalfEdge(currentEdgeIndex);
 
         // Get the twin edge
-        HalfEdgeStructure::Edge twinEdge = referencePolyhedron->getHalfEdge(edge.twinEdgeIndex);
+        const HalfEdgeStructure::Edge& twinEdge = referencePolyhedron->getHalfEdge(edge.twinEdgeIndex);
 
         // Compute the edge vertices and edge direction
         Vector3 edgeV1 = referencePolyhedron->getVertexPosition(edge.vertexIndex);
@@ -855,8 +861,8 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene
         // The clipping plane is perpendicular to the edge direction and the reference face normal
         Vector3 clipPlaneNormal = axisReferenceSpace.cross(edgeDirection);
 
-        planesNormals.push_back(clipPlaneNormal);
+        planesNormals.add(clipPlaneNormal);
-        planesPoints.push_back(edgeV1);
+        planesPoints.add(edgeV1);
 
         // Go to the next adjacent edge of the reference face
         currentEdgeIndex = edge.nextEdgeIndex;
@@ -867,17 +873,16 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene
     assert(planesNormals.size() == planesPoints.size());
 
     // Clip the reference faces with the adjacent planes of the reference face
-    std::vector<Vector3> clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals);
+    List<Vector3> clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals, mMemoryAllocator);
 
     // We only keep the clipped points that are below the reference face
     const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(referencePolyhedron->getHalfEdge(firstEdgeIndex).vertexIndex);
-    std::vector<Vector3>::const_iterator itPoints;
     bool contactPointsFound = false;
-    for (itPoints = clipPolygonVertices.begin(); itPoints != clipPolygonVertices.end(); ++itPoints) {
+    for (uint i=0; i<clipPolygonVertices.size(); i++) {
 
         // Compute the penetration depth of this contact point (can be different from the minPenetration depth which is
         // the maximal penetration depth of any contact point for this separating axis
-        decimal penetrationDepth = (referenceFaceVertex - (*itPoints)).dot(axisReferenceSpace);
+        decimal penetrationDepth = (referenceFaceVertex - clipPolygonVertices[i]).dot(axisReferenceSpace);
 
         // If the clip point is bellow the reference face
         if (penetrationDepth > decimal(0.0)) {
@@ -887,10 +892,10 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene
             Vector3 outWorldNormal = normalWorld;
 
             // Convert the clip incident polyhedron vertex into the incident polyhedron local-space
-            Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * (*itPoints);
+            Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * clipPolygonVertices[i];
 
             // Project the contact point onto the reference face
-            Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(*itPoints, axisReferenceSpace, referenceFaceVertex);
+            Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(clipPolygonVertices[i], axisReferenceSpace, referenceFaceVertex);
 
             // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
             TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2,
@@ -968,7 +973,7 @@ decimal SATAlgorithm::testSingleFaceDirectionPolyhedronVsPolyhedron(const Convex
 
     PROFILE("SATAlgorithm::testSingleFaceDirectionPolyhedronVsPolyhedron", mProfiler);
 
-    HalfEdgeStructure::Face face = polyhedron1->getFace(faceIndex);
+    const HalfEdgeStructure::Face& face = polyhedron1->getFace(faceIndex);
 
     // Get the face normal
     const Vector3 faceNormal = polyhedron1->getFaceNormal(faceIndex);

src/collision/narrowphase/SAT/SATAlgorithm.h

@@ -50,6 +50,9 @@ class SATAlgorithm {
         /// make sure the contact manifold does not change too much between frames.
         static const decimal SAME_SEPARATING_AXIS_BIAS;
 
+        /// Memory allocator
+        Allocator& mMemoryAllocator;
+
 #ifdef IS_PROFILING_ACTIVE
 
 		/// Pointer to the profiler
@@ -115,7 +118,7 @@ class SATAlgorithm {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        SATAlgorithm() = default;
+        SATAlgorithm(Allocator& memoryAllocator);
 
         /// Destructor
         ~SATAlgorithm() = default;

src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp

@@ -34,7 +34,8 @@ 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(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
+bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
+                                             Allocator& memoryAllocator) {
 
     bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE;
 

src/collision/narrowphase/SphereVsCapsuleAlgorithm.h

@@ -61,7 +61,7 @@ class SphereVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
 		SphereVsCapsuleAlgorithm& operator=(const SphereVsCapsuleAlgorithm& algorithm) = delete;
 
         /// Compute the narrow-phase collision detection between a sphere and a capsule
-        virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override;
+        virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override;
 };
 
 }

src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp

@@ -34,7 +34,8 @@ using namespace reactphysics3d;
 // Compute the narrow-phase collision detection between 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(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
+bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
+                                                      Allocator& memoryAllocator) {
 
     assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
         narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
@@ -69,7 +70,7 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPha
     if (result == GJKAlgorithm::GJKResult::INTERPENETRATE) {
 
         // Run the SAT algorithm to find the separating axis and compute contact point
-        SATAlgorithm satAlgorithm;
+        SATAlgorithm satAlgorithm(memoryAllocator);
 
 #ifdef IS_PROFILING_ACTIVE
 

src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h

@@ -61,7 +61,7 @@ class SphereVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
         SphereVsConvexPolyhedronAlgorithm& operator=(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete;
 
         /// Compute the narrow-phase collision detection between a sphere and a convex polyhedron
-        virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override;
+        virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override;
 };
 
 }

src/collision/narrowphase/SphereVsSphereAlgorithm.cpp

@@ -30,7 +30,8 @@
 // We want to use the ReactPhysics3D namespace
 using namespace reactphysics3d;  
 
-bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
+bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
+                                            Allocator& memoryAllocator) {
     
     assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE);
     assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE);

src/collision/narrowphase/SphereVsSphereAlgorithm.h

@@ -61,7 +61,7 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
         SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete;
 
         /// Compute a contact info if the two bounding volume collide
-        virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override;
+        virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override;
 };
 
 }

src/collision/shapes/BoxShape.cpp

@@ -84,10 +84,9 @@ BoxShape::BoxShape(const Vector3& extent)
  */
 void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
     decimal factor = (decimal(1.0) / decimal(3.0)) * mass;
-    Vector3 realExtent = mExtent + Vector3(mMargin, mMargin, mMargin);
+    decimal xSquare = mExtent.x * mExtent.x;
-    decimal xSquare = realExtent.x * realExtent.x;
+    decimal ySquare = mExtent.y * mExtent.y;
-    decimal ySquare = realExtent.y * realExtent.y;
+    decimal zSquare = mExtent.z * mExtent.z;
-    decimal zSquare = realExtent.z * realExtent.z;
     tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0,
                         0.0, factor * (xSquare + zSquare), 0.0,
                         0.0, 0.0, factor * (xSquare + ySquare));

src/collision/shapes/BoxShape.h

@@ -101,7 +101,7 @@ class BoxShape : public ConvexPolyhedronShape {
         virtual uint getNbFaces() const override;
 
         /// Return a given face of the polyhedron
-        virtual HalfEdgeStructure::Face getFace(uint faceIndex) const override;
+        virtual const HalfEdgeStructure::Face& getFace(uint faceIndex) const override;
 
         /// Return the number of vertices of the polyhedron
         virtual uint getNbVertices() const override;
@@ -113,7 +113,7 @@ class BoxShape : public ConvexPolyhedronShape {
         virtual uint getNbHalfEdges() const override;
 
         /// Return a given half-edge of the polyhedron
-        virtual HalfEdgeStructure::Edge getHalfEdge(uint edgeIndex) const override;
+        virtual const HalfEdgeStructure::Edge& getHalfEdge(uint edgeIndex) const override;
 
         /// Return the position of a given vertex
         virtual Vector3 getVertexPosition(uint vertexIndex) const override;
@@ -130,7 +130,7 @@ class BoxShape : public ConvexPolyhedronShape {
  * @return The vector with the three extents of the box shape (in meters)
  */
 inline Vector3 BoxShape::getExtent() const {
-    return mExtent + Vector3(mMargin, mMargin, mMargin);
+    return mExtent;
 }
 
 // Set the scaling vector of the collision shape
@@ -150,7 +150,7 @@ inline void BoxShape::setLocalScaling(const Vector3& scaling) {
 inline void BoxShape::getLocalBounds(Vector3& min, Vector3& max) const {
 
     // Maximum bounds
-    max = mExtent + Vector3(mMargin, mMargin, mMargin);
+    max = mExtent;
 
     // Minimum bounds
     min = -max;
@@ -161,7 +161,7 @@ inline size_t BoxShape::getSizeInBytes() const {
     return sizeof(BoxShape);
 }
 
-// Return a local support point in a given direction without the objec margin
+// Return a local support point in a given direction without the object margin
 inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const {
 
     return Vector3(direction.x < decimal(0.0) ? -mExtent.x : mExtent.x,
@@ -182,7 +182,7 @@ inline uint BoxShape::getNbFaces() const {
 }
 
 // Return a given face of the polyhedron
-inline HalfEdgeStructure::Face BoxShape::getFace(uint faceIndex) const {
+inline const HalfEdgeStructure::Face& BoxShape::getFace(uint faceIndex) const {
     assert(faceIndex < mHalfEdgeStructure.getNbFaces());
     return mHalfEdgeStructure.getFace(faceIndex);
 }
@@ -243,7 +243,7 @@ inline uint BoxShape::getNbHalfEdges() const {
 }
 
 // Return a given half-edge of the polyhedron
-inline HalfEdgeStructure::Edge BoxShape::getHalfEdge(uint edgeIndex) const {
+inline const HalfEdgeStructure::Edge& BoxShape::getHalfEdge(uint edgeIndex) const {
     assert(edgeIndex < getNbHalfEdges());
     return mHalfEdgeStructure.getHalfEdge(edgeIndex);
 }

src/collision/shapes/ConvexMeshShape.h

@@ -111,7 +111,7 @@ class ConvexMeshShape : public ConvexPolyhedronShape {
         virtual uint getNbFaces() const override;
 
         /// Return a given face of the polyhedron
-        virtual HalfEdgeStructure::Face getFace(uint faceIndex) const override;
+        virtual const HalfEdgeStructure::Face& getFace(uint faceIndex) const override;
 
         /// Return the number of vertices of the polyhedron
         virtual uint getNbVertices() const override;
@@ -123,7 +123,7 @@ class ConvexMeshShape : public ConvexPolyhedronShape {
         virtual uint getNbHalfEdges() const override;
 
         /// Return a given half-edge of the polyhedron
-        virtual HalfEdgeStructure::Edge getHalfEdge(uint edgeIndex) const override;
+        virtual const HalfEdgeStructure::Edge& getHalfEdge(uint edgeIndex) const override;
 
         /// Return the position of a given vertex
         virtual Vector3 getVertexPosition(uint vertexIndex) const override;
@@ -191,7 +191,7 @@ inline uint ConvexMeshShape::getNbFaces() const {
 }
 
 // Return a given face of the polyhedron
-inline HalfEdgeStructure::Face ConvexMeshShape::getFace(uint faceIndex) const {
+inline const HalfEdgeStructure::Face& ConvexMeshShape::getFace(uint faceIndex) const {
     assert(faceIndex < getNbFaces());
     return mPolyhedronMesh->getHalfEdgeStructure().getFace(faceIndex);
 }
@@ -213,7 +213,7 @@ inline uint ConvexMeshShape::getNbHalfEdges() const {
 }
 
 // Return a given half-edge of the polyhedron
-inline HalfEdgeStructure::Edge ConvexMeshShape::getHalfEdge(uint edgeIndex) const {
+inline const HalfEdgeStructure::Edge& ConvexMeshShape::getHalfEdge(uint edgeIndex) const {
     assert(edgeIndex < getNbHalfEdges());
     return mPolyhedronMesh->getHalfEdgeStructure().getHalfEdge(edgeIndex);
 }

src/collision/shapes/ConvexPolyhedronShape.h

@@ -62,7 +62,7 @@ class ConvexPolyhedronShape : public ConvexShape {
         virtual uint getNbFaces() const=0;
 
         /// Return a given face of the polyhedron
-        virtual HalfEdgeStructure::Face getFace(uint faceIndex) const=0;
+        virtual const HalfEdgeStructure::Face& getFace(uint faceIndex) const=0;
 
         /// Return the number of vertices of the polyhedron
         virtual uint getNbVertices() const=0;
@@ -80,7 +80,7 @@ class ConvexPolyhedronShape : public ConvexShape {
         virtual uint getNbHalfEdges() const=0;
 
         /// Return a given half-edge of the polyhedron
-        virtual HalfEdgeStructure::Edge getHalfEdge(uint edgeIndex) const=0;
+        virtual const HalfEdgeStructure::Edge& getHalfEdge(uint edgeIndex) const=0;
 
         /// Return true if the collision shape is a polyhedron
         virtual bool isPolyhedron() const override;

src/collision/shapes/TriangleShape.cpp

@@ -58,6 +58,57 @@ TriangleShape::TriangleShape(const Vector3* vertices, const Vector3* verticesNor
     mVerticesNormals[1] = verticesNormals[1];
     mVerticesNormals[2] = verticesNormals[2];
 
+    // Faces
+    for (uint i=0; i<2; i++) {
+        mFaces[i].faceVertices.push_back(0);
+        mFaces[i].faceVertices.push_back(1);
+        mFaces[i].faceVertices.push_back(2);
+        mFaces[i].edgeIndex = i;
+    }
+
+    // Edges
+    for (uint i=0; i<6; i++) {
+        switch(i) {
+            case 0:
+                mEdges[0].vertexIndex = 0;
+                mEdges[0].twinEdgeIndex = 1;
+                mEdges[0].faceIndex = 0;
+                mEdges[0].nextEdgeIndex = 2;
+                break;
+            case 1:
+                mEdges[1].vertexIndex = 1;
+                mEdges[1].twinEdgeIndex = 0;
+                mEdges[1].faceIndex = 1;
+                mEdges[1].nextEdgeIndex = 5;
+                break;
+            case 2:
+                mEdges[2].vertexIndex = 1;
+                mEdges[2].twinEdgeIndex = 3;
+                mEdges[2].faceIndex = 0;
+                mEdges[2].nextEdgeIndex = 4;
+                break;
+            case 3:
+                mEdges[3].vertexIndex = 2;
+                mEdges[3].twinEdgeIndex = 2;
+                mEdges[3].faceIndex = 1;
+                mEdges[3].nextEdgeIndex = 1;
+                break;
+            case 4:
+                mEdges[4].vertexIndex = 2;
+                mEdges[4].twinEdgeIndex = 5;
+                mEdges[4].faceIndex = 0;
+                mEdges[4].nextEdgeIndex = 0;
+                break;
+            case 5:
+                mEdges[5].vertexIndex = 0;
+                mEdges[5].twinEdgeIndex = 4;
+                mEdges[5].faceIndex = 1;
+                mEdges[5].nextEdgeIndex = 3;
+                break;
+        }
+    }
+
+
     mRaycastTestType = TriangleRaycastSide::FRONT;
 
     mId = shapeId;
@@ -227,51 +278,3 @@ 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

@@ -72,6 +72,12 @@ class TriangleShape : public ConvexPolyhedronShape {
         /// Raycast test type for the triangle (front, back, front-back)
         TriangleRaycastSide mRaycastTestType;
 
+        /// Faces information for the two faces of the triangle
+        HalfEdgeStructure::Face mFaces[2];
+
+        /// Edges information for the six edges of the triangle
+        HalfEdgeStructure::Edge mEdges[6];
+
         // -------------------- Methods -------------------- //
 
         /// Return a local support point in a given direction without the object margin
@@ -137,7 +143,7 @@ class TriangleShape : public ConvexPolyhedronShape {
         virtual uint getNbFaces() const override;
 
         /// Return a given face of the polyhedron
-        virtual HalfEdgeStructure::Face getFace(uint faceIndex) const override;
+        virtual const HalfEdgeStructure::Face& getFace(uint faceIndex) const override;
 
         /// Return the number of vertices of the polyhedron
         virtual uint getNbVertices() const override;
@@ -155,7 +161,7 @@ class TriangleShape : public ConvexPolyhedronShape {
         virtual uint getNbHalfEdges() const override;
 
         /// Return a given half-edge of the polyhedron
-        virtual HalfEdgeStructure::Edge getHalfEdge(uint edgeIndex) const override;
+        virtual const HalfEdgeStructure::Edge& getHalfEdge(uint edgeIndex) const override;
 
         /// Return the centroid of the polyhedron
         virtual Vector3 getCentroid() const override;
@@ -252,26 +258,9 @@ inline uint TriangleShape::getNbFaces() const {
 }
 
 // Return a given face of the polyhedron
-inline HalfEdgeStructure::Face TriangleShape::getFace(uint faceIndex) const {
+inline const HalfEdgeStructure::Face& TriangleShape::getFace(uint faceIndex) const {
     assert(faceIndex < 2);
-
+    return mFaces[faceIndex];
-    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
@@ -292,6 +281,12 @@ inline HalfEdgeStructure::Vertex TriangleShape::getVertex(uint vertexIndex) cons
     return vertex;
 }
 
+// Return a given half-edge of the polyhedron
+inline const HalfEdgeStructure::Edge& TriangleShape::getHalfEdge(uint edgeIndex) const {
+    assert(edgeIndex < getNbHalfEdges());
+    return mEdges[edgeIndex];
+}
+
 // Return the position of a given vertex
 inline Vector3 TriangleShape::getVertexPosition(uint vertexIndex) const {
     assert(vertexIndex < 3);

src/mathematics/Quaternion.h

@@ -306,16 +306,35 @@ inline Quaternion Quaternion::operator*(decimal nb) const {
 
 // Overloaded operator for the multiplication of two quaternions
 inline Quaternion Quaternion::operator*(const Quaternion& quaternion) const {
+
+    /* The followin code is equivalent to this
     return Quaternion(w * quaternion.w - getVectorV().dot(quaternion.getVectorV()),
-                      w * quaternion.getVectorV() + quaternion.w * getVectorV() +
+                          w * quaternion.getVectorV() + quaternion.w * getVectorV() +
-                      getVectorV().cross(quaternion.getVectorV()));
+                          getVectorV().cross(quaternion.getVectorV()));
+    */
+
+    return Quaternion(w * quaternion.x + quaternion.w * x + y * quaternion.z - z * quaternion.y,
+                      w * quaternion.y + quaternion.w * y + z * quaternion.x - x * quaternion.z,
+                      w * quaternion.z + quaternion.w * z + x * quaternion.y - y * quaternion.x,
+                      w * quaternion.w - x * quaternion.x - y * quaternion.y - z * quaternion.z);
 }
 
 // Overloaded operator for the multiplication with a vector.
 /// This methods rotates a point given the rotation of a quaternion.
 inline Vector3 Quaternion::operator*(const Vector3& point) const {
-    Quaternion p(point.x, point.y, point.z, 0.0);
+
-    return (((*this) * p) * getConjugate()).getVectorV();
+    /* The following code is equivalent to this
+     * Quaternion p(point.x, point.y, point.z, 0.0);
+     * return (((*this) * p) * getConjugate()).getVectorV();
+    */
+
+    const decimal prodX = w * point.x + y * point.z - z * point.y;
+    const decimal prodY = w * point.y + z * point.x - x * point.z;
+    const decimal prodZ = w * point.z + x * point.y - y * point.x;
+    const decimal prodW = -x * point.x - y * point.y - z * point.z;
+    return Vector3(w * prodX - prodY * z + prodZ * y - prodW * x,
+                   w * prodY - prodZ * x + prodX * z - prodW * y,
+                   w * prodZ - prodX * y + prodY * x - prodW * z);
 }
 
 // Overloaded operator for the assignment

src/mathematics/Transform.h

@@ -194,7 +194,7 @@ inline Transform Transform::identity() {
 
 // Return the transformed vector
 inline Vector3 Transform::operator*(const Vector3& vector) const {
-    return (mOrientation.getMatrix() * vector) + mPosition;
+    return (mOrientation * vector) + mPosition;
 }
 
 // Operator of multiplication of a transform with another one

src/mathematics/mathematics_functions.cpp

@@ -198,10 +198,7 @@ decimal reactphysics3d::computePlaneSegmentIntersection(const Vector3& segA, con
     const decimal parallelEpsilon = decimal(0.0001);
 	decimal t = decimal(-1);
 
-	// Segment AB
+    decimal nDotAB = planeNormal.dot(segB - segA);
-	const Vector3 ab = segB - segA;
-
-    decimal nDotAB = planeNormal.dot(ab);
 
 	// If the segment is not parallel to the plane
     if (std::abs(nDotAB) > parallelEpsilon) {
@@ -297,23 +294,27 @@ std::vector<Vector3> reactphysics3d::clipSegmentWithPlanes(const Vector3& segA,
 
 // Clip a polygon against multiple planes and return the clipped polygon vertices
 // This method implements the Sutherland–Hodgman clipping algorithm
-std::vector<Vector3> reactphysics3d::clipPolygonWithPlanes(const std::vector<Vector3>& polygonVertices, const std::vector<Vector3>& planesPoints,
+List<Vector3> reactphysics3d::clipPolygonWithPlanes(const List<Vector3>& polygonVertices, const List<Vector3>& planesPoints,
-                                                           const std::vector<Vector3>& planesNormals) {
+                                                    const List<Vector3>& planesNormals, Allocator& allocator) {
 
     assert(planesPoints.size() == planesNormals.size());
 
-    std::vector<Vector3> inputVertices(polygonVertices);
+    uint nbMaxElements = polygonVertices.size() + planesPoints.size();
-    std::vector<Vector3> outputVertices;
+    List<Vector3> inputVertices(allocator, nbMaxElements);
+    List<Vector3> outputVertices(allocator, nbMaxElements);
+
+    inputVertices.addRange(polygonVertices);
 
     // For each clipping plane
     for (uint p=0; p<planesPoints.size(); p++) {
 
         outputVertices.clear();
 
-        uint vStart = inputVertices.size() - 1;
+        uint nbInputVertices = inputVertices.size();
+        uint vStart = nbInputVertices - 1;
 
         // For each edge of the polygon
-        for (uint vEnd = 0; vEnd<inputVertices.size(); vEnd++) {
+        for (uint vEnd = 0; vEnd<nbInputVertices; vEnd++) {
 
             Vector3& v1 = inputVertices[vStart];
             Vector3& v2 = inputVertices[vEnd];
@@ -331,15 +332,15 @@ std::vector<Vector3> reactphysics3d::clipPolygonWithPlanes(const std::vector<Vec
                     decimal t = computePlaneSegmentIntersection(v1, v2, planesNormals[p].dot(planesPoints[p]), planesNormals[p]);
 
                     if (t >= decimal(0) && t <= decimal(1.0)) {
-                        outputVertices.push_back(v1 + t * (v2 - v1));
+                        outputVertices.add(v1 + t * (v2 - v1));
                     }
                     else {
-                        outputVertices.push_back(v2);
+                        outputVertices.add(v2);
                     } 
                 }
 
                 // Add the second vertex
-                outputVertices.push_back(v2);
+                outputVertices.add(v2);
             }
             else {  // If the second vertex is behind the clipping plane
 
@@ -350,10 +351,10 @@ std::vector<Vector3> reactphysics3d::clipPolygonWithPlanes(const std::vector<Vec
                     decimal t = computePlaneSegmentIntersection(v1, v2, -planesNormals[p].dot(planesPoints[p]), -planesNormals[p]);
 
                     if (t >= decimal(0.0) && t <= decimal(1.0)) {
-                        outputVertices.push_back(v1 + t * (v2 - v1));
+                        outputVertices.add(v1 + t * (v2 - v1));
                     }
                     else {
-                        outputVertices.push_back(v1);
+                        outputVertices.add(v1);
                     }
                 }
             }

src/mathematics/mathematics_functions.h

@@ -33,6 +33,7 @@
 #include <cassert>
 #include <cmath>
 #include <vector>
+#include "containers/List.h"
 
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
@@ -116,8 +117,8 @@ std::vector<Vector3> clipSegmentWithPlanes(const Vector3& segA, const Vector3& s
                                                            const std::vector<Vector3>& planesNormals);
 
 /// Clip a polygon against multiple planes and return the clipped polygon vertices
-std::vector<Vector3> clipPolygonWithPlanes(const std::vector<Vector3>& polygonVertices, const std::vector<Vector3>& planesPoints,
+List<Vector3> clipPolygonWithPlanes(const List<Vector3>& polygonVertices, const List<Vector3>& planesPoints,
-                                           const std::vector<Vector3>& planesNormals);
+                                    const List<Vector3>& planesNormals, Allocator& allocator);
 
 /// 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);

test/tests/mathematics/TestMathematicsFunctions.h

@@ -27,6 +27,8 @@
 #define TEST_MATHEMATICS_FUNCTIONS_H
 
 // Libraries
+#include "containers/List.h"
+#include "memory/DefaultAllocator.h"
 
 /// Reactphysics3D namespace
 namespace reactphysics3d {
@@ -41,7 +43,7 @@ class TestMathematicsFunctions : public Test {
 
         // ---------- Atributes ---------- //
 
-
+        DefaultAllocator mAllocator;
 
     public :
 
@@ -223,37 +225,37 @@ class TestMathematicsFunctions : public Test {
             test(clipSegmentVertices.size() == 0);
 
             // Test clipPolygonWithPlanes()
-            std::vector<Vector3> polygonVertices;
+            List<Vector3> polygonVertices(mAllocator);
-            polygonVertices.push_back(Vector3(-4, 2, 0));
+            polygonVertices.add(Vector3(-4, 2, 0));
-            polygonVertices.push_back(Vector3(7, 2, 0));
+            polygonVertices.add(Vector3(7, 2, 0));
-            polygonVertices.push_back(Vector3(7, 4, 0));
+            polygonVertices.add(Vector3(7, 4, 0));
-            polygonVertices.push_back(Vector3(-4, 4, 0));
+            polygonVertices.add(Vector3(-4, 4, 0));
 
-            planesNormals.clear();
+            List<Vector3> polygonPlanesNormals(mAllocator);
-            planesPoints.clear();
+            List<Vector3> polygonPlanesPoints(mAllocator);
-            planesNormals.push_back(Vector3(1, 0, 0));
+            polygonPlanesNormals.add(Vector3(1, 0, 0));
-            planesPoints.push_back(Vector3(0, 0, 0));
+            polygonPlanesPoints.add(Vector3(0, 0, 0));
-            planesNormals.push_back(Vector3(0, 1, 0));
+            polygonPlanesNormals.add(Vector3(0, 1, 0));
-            planesPoints.push_back(Vector3(0, 0, 0));
+            polygonPlanesPoints.add(Vector3(0, 0, 0));
-            planesNormals.push_back(Vector3(-1, 0, 0));
+            polygonPlanesNormals.add(Vector3(-1, 0, 0));
-            planesPoints.push_back(Vector3(10, 0, 0));
+            polygonPlanesPoints.add(Vector3(10, 0, 0));
-            planesNormals.push_back(Vector3(0, -1, 0));
+            polygonPlanesNormals.add(Vector3(0, -1, 0));
-            planesPoints.push_back(Vector3(10, 5, 0));
+            polygonPlanesPoints.add(Vector3(10, 5, 0));
 
-            clipSegmentVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals);
+            List<Vector3> clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, polygonPlanesPoints, polygonPlanesNormals, mAllocator);
-            test(clipSegmentVertices.size() == 4);
+            test(clipPolygonVertices.size() == 4);
-            test(approxEqual(clipSegmentVertices[0].x, 0, 0.000001));
+            test(approxEqual(clipPolygonVertices[0].x, 0, 0.000001));
-            test(approxEqual(clipSegmentVertices[0].y, 2, 0.000001));
+            test(approxEqual(clipPolygonVertices[0].y, 2, 0.000001));
-            test(approxEqual(clipSegmentVertices[0].z, 0, 0.000001));
+            test(approxEqual(clipPolygonVertices[0].z, 0, 0.000001));
-            test(approxEqual(clipSegmentVertices[1].x, 7, 0.000001));
+            test(approxEqual(clipPolygonVertices[1].x, 7, 0.000001));
-            test(approxEqual(clipSegmentVertices[1].y, 2, 0.000001));
+            test(approxEqual(clipPolygonVertices[1].y, 2, 0.000001));
-            test(approxEqual(clipSegmentVertices[1].z, 0, 0.000001));
+            test(approxEqual(clipPolygonVertices[1].z, 0, 0.000001));
-            test(approxEqual(clipSegmentVertices[2].x, 7, 0.000001));
+            test(approxEqual(clipPolygonVertices[2].x, 7, 0.000001));
-            test(approxEqual(clipSegmentVertices[2].y, 4, 0.000001));
+            test(approxEqual(clipPolygonVertices[2].y, 4, 0.000001));
-            test(approxEqual(clipSegmentVertices[2].z, 0, 0.000001));
+            test(approxEqual(clipPolygonVertices[2].z, 0, 0.000001));
-            test(approxEqual(clipSegmentVertices[3].x, 0, 0.000001));
+            test(approxEqual(clipPolygonVertices[3].x, 0, 0.000001));
-            test(approxEqual(clipSegmentVertices[3].y, 4, 0.000001));
+            test(approxEqual(clipPolygonVertices[3].y, 4, 0.000001));
-            test(approxEqual(clipSegmentVertices[3].z, 0, 0.000001));
+            test(approxEqual(clipPolygonVertices[3].z, 0, 0.000001));
 
         }