Merge branch 'sat_opti' into sat

CMakeLists.txt

@@ -192,8 +192,12 @@ SET (REACTPHYSICS3D_SOURCES
     "src/memory/PoolAllocator.cpp"
     "src/memory/SingleFrameAllocator.h"
     "src/memory/SingleFrameAllocator.cpp"
+    "src/memory/DefaultAllocator.h"
+    "src/memory/MemoryManager.h"
+    "src/memory/MemoryManager.cpp"
     "src/containers/Stack.h"
     "src/containers/LinkedList.h"
+    "src/containers/List.h"
 )
 
 # Create the library

src/body/CollisionBody.cpp

@@ -72,7 +72,7 @@ ProxyShape* CollisionBody::addCollisionShape(CollisionShape* collisionShape,
     // Create a new proxy collision shape to attach the collision shape to the body
     ProxyShape* proxyShape = new (mWorld.mPoolAllocator.allocate(
                                       sizeof(ProxyShape))) ProxyShape(this, collisionShape,
-                                                                      transform, decimal(1));
+                                                                      transform, decimal(1), mWorld.mPoolAllocator);
 
 #ifdef IS_PROFILING_ACTIVE
 

src/body/RigidBody.cpp

@@ -39,7 +39,7 @@ using namespace reactphysics3d;
 * @param id The ID of the body
 */
 RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, bodyindex id)
-          : CollisionBody(transform, world, id), mInitMass(decimal(1.0)),
+          : CollisionBody(transform, world, id), mArrayIndex(0), mInitMass(decimal(1.0)),
             mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()),
             mIsGravityEnabled(true), mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)),
             mJointsList(nullptr) {
@@ -226,7 +226,7 @@ ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape,
     // Create a new proxy collision shape to attach the collision shape to the body
     ProxyShape* proxyShape = new (mWorld.mPoolAllocator.allocate(
                                       sizeof(ProxyShape))) ProxyShape(this, collisionShape,
-                                                                      transform, mass);
+                                                                      transform, mass, mWorld.mPoolAllocator);
 
 #ifdef IS_PROFILING_ACTIVE
 

src/body/RigidBody.h

@@ -50,6 +50,11 @@ class DynamicsWorld;
   */
 class RigidBody : public CollisionBody {
 
+    private :
+
+        /// Index of the body in arrays for contact/constraint solver
+        uint mArrayIndex;
+
     protected :
 
         // -------------------- Attributes -------------------- //
@@ -102,7 +107,7 @@ class RigidBody : public CollisionBody {
         decimal mAngularDamping;
 
         /// First element of the linked list of joints involving this body
-        JointListElement* mJointsList;        
+        JointListElement* mJointsList;
 
         // -------------------- Methods -------------------- //
 

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();
@@ -288,7 +288,14 @@ void CollisionDetection::computeNarrowPhase() {
             lastCollisionFrameInfo->isValid = true;
         }
 
+        NarrowPhaseInfo* narrowPhaseInfoToDelete = currentNarrowPhaseInfo;
         currentNarrowPhaseInfo = currentNarrowPhaseInfo->next;
+
+        // Call the destructor
+        narrowPhaseInfoToDelete->~NarrowPhaseInfo();
+
+        // Release the allocated memory for the narrow phase info
+        mSingleFrameAllocator.release(narrowPhaseInfoToDelete, sizeof(NarrowPhaseInfo));
     }
 
     // Convert the potential contact into actual contacts
@@ -593,7 +600,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 +695,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 +774,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 +866,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 +950,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/ContactManifoldSet.cpp

@@ -225,7 +225,6 @@ void ContactManifoldSet::removeManifold(ContactManifold* manifold) {
     // Delete the contact manifold
     manifold->~ContactManifold();
     mMemoryAllocator.release(manifold, sizeof(ContactManifold));
-
     mNbManifolds--;
 }
 

src/collision/HalfEdgeStructure.cpp

@@ -96,8 +96,8 @@ void HalfEdgeStructure::init() {
                 mapEdgeToIndex.insert(std::make_pair(pairV1V2, edgeIndex + 1));
                 mapEdgeToIndex.insert(std::make_pair(pairV2V1, edgeIndex));
 
-                mEdges.push_back(itEdge->second);
-                mEdges.push_back(edge);
+                mEdges.add(itEdge->second);
+                mEdges.add(edge);
             }
 
             currentFaceEdges.push_back(pairV1V2);

src/collision/HalfEdgeStructure.h

@@ -50,14 +50,14 @@ class HalfEdgeStructure {
         };
 
         struct Face {
-            uint edgeIndex;                     // Index of an half-edge of the face
-            std::vector<uint> faceVertices;     // Index of the vertices of the face
+            uint edgeIndex;             // Index of an half-edge of the face
+            List<uint> faceVertices;	// Index of the vertices of the face
 
             /// Constructor
-            Face() {}
+            Face(Allocator& allocator) : faceVertices(allocator) {}
 
             /// Constructor
-            Face(std::vector<uint> vertices) : faceVertices(vertices) {}
+            Face(List<uint> vertices) : faceVertices(vertices) {}
         };
 
         struct Vertex {
@@ -70,19 +70,24 @@ class HalfEdgeStructure {
 
     private:
 
+        /// Reference to a memory allocator
+        Allocator& mAllocator;
+
         /// All the faces
-        std::vector<Face> mFaces;
+        List<Face> mFaces;
 
         /// All the vertices
-        std::vector<Vertex> mVertices;
+        List<Vertex> mVertices;
 
         /// All the half-edges
-        std::vector<Edge> mEdges;
+        List<Edge> mEdges;
 
     public:
 
         /// Constructor
-        HalfEdgeStructure() = default;
+        HalfEdgeStructure(Allocator& allocator, uint facesCapacity, uint verticesCapacity,
+                          uint edgesCapacity) :mAllocator(allocator), mFaces(allocator, facesCapacity),
+                          mVertices(allocator, verticesCapacity), mEdges(allocator, edgesCapacity) {}
 
         /// Destructor
         ~HalfEdgeStructure() = default;
@@ -94,7 +99,7 @@ class HalfEdgeStructure {
         uint addVertex(uint vertexPointIndex);
 
         /// Add a face
-        void addFace(std::vector<uint> faceVertices);
+        void addFace(List<uint> faceVertices);
 
         /// Return the number of faces
         uint getNbFaces() const;
@@ -106,60 +111,60 @@ 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;
 
 };
 
 // Add a vertex
 inline uint HalfEdgeStructure::addVertex(uint vertexPointIndex) {
     Vertex vertex(vertexPointIndex);
-    mVertices.push_back(vertex);
+    mVertices.add(vertex);
     return mVertices.size() - 1;
 }
 
 // Add a face
-inline void HalfEdgeStructure::addFace(std::vector<uint> faceVertices) {
+inline void HalfEdgeStructure::addFace(List<uint> faceVertices) {
 
     // Create a new face
     Face face(faceVertices);
-    mFaces.push_back(face);
+    mFaces.add(face);
 }
 
 // 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/MiddlePhaseTriangleCallback.cpp

@@ -34,7 +34,7 @@ void MiddlePhaseTriangleCallback::testTriangle(const Vector3* trianglePoints, co
     // Create a triangle collision shape (the allocated memory for the TriangleShape will be released in the
 	// destructor of the corresponding NarrowPhaseInfo.
     TriangleShape* triangleShape = new (mAllocator.allocate(sizeof(TriangleShape)))
-                                   TriangleShape(trianglePoints, verticesNormals, shapeId);
+                                   TriangleShape(trianglePoints, verticesNormals, shapeId, mAllocator);
 
 #ifdef IS_PROFILING_ACTIVE
 

src/collision/NarrowPhaseInfo.cpp

@@ -52,11 +52,13 @@ NarrowPhaseInfo::~NarrowPhaseInfo() {
 	// Release the memory of the TriangleShape (this memory was allocated in the
 	// MiddlePhaseTriangleCallback::testTriangle() method)
 	if (collisionShape1->getName() == CollisionShapeName::TRIANGLE) {
+        collisionShape1->~CollisionShape();
 		collisionShapeAllocator.release(collisionShape1, sizeof(TriangleShape));
 	}
 	if (collisionShape2->getName() == CollisionShapeName::TRIANGLE) {
-		collisionShapeAllocator.release(collisionShape2, sizeof(TriangleShape));
-	}
+        collisionShape2->~CollisionShape();
+        collisionShapeAllocator.release(collisionShape2, sizeof(TriangleShape));
+    }
 }
 
 // Add a new contact point

src/collision/PolyhedronMesh.cpp

@@ -25,6 +25,7 @@
 
 // Libraries
 #include "PolyhedronMesh.h"
+#include "memory/MemoryManager.h"
 
 using namespace reactphysics3d;
 
@@ -34,7 +35,11 @@ using namespace reactphysics3d;
  * Create a polyhedron mesh given an array of polygons.
  * @param polygonVertexArray Pointer to the array of polygons and their vertices
  */
-PolyhedronMesh::PolyhedronMesh(PolygonVertexArray* polygonVertexArray) {
+PolyhedronMesh::PolyhedronMesh(PolygonVertexArray* polygonVertexArray)
+               : mHalfEdgeStructure(MemoryManager::getDefaultAllocator(),
+                                    polygonVertexArray->getNbFaces(),
+                                    polygonVertexArray->getNbVertices(),
+                                    (polygonVertexArray->getNbFaces() + polygonVertexArray->getNbVertices() - 2) * 2) {
 
    mPolygonVertexArray = polygonVertexArray;
 
@@ -70,11 +75,11 @@ void PolyhedronMesh::createHalfEdgeStructure() {
         // Get the polygon face
         PolygonVertexArray::PolygonFace* face = mPolygonVertexArray->getPolygonFace(f);
 
-        std::vector<uint> faceVertices;
+        List<uint> faceVertices(MemoryManager::getDefaultAllocator(), face->nbVertices);
 
         // For each vertex of the face
         for (uint v=0; v < face->nbVertices; v++) {
-            faceVertices.push_back(mPolygonVertexArray->getVertexIndexInFace(f, v));
+            faceVertices.add(mPolygonVertexArray->getVertexIndexInFace(f, v));
         }
 
         assert(faceVertices.size() >= 3);
@@ -123,7 +128,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/PolyhedronMesh.h

@@ -30,6 +30,7 @@
 #include "mathematics/mathematics.h"
 #include "HalfEdgeStructure.h"
 #include "collision/PolygonVertexArray.h"
+#include "memory/DefaultAllocator.h"
 #include <vector>
 
 namespace reactphysics3d {

src/collision/ProxyShape.cpp

@@ -35,9 +35,9 @@ using namespace reactphysics3d;
  * @param transform Transformation from collision shape local-space to body local-space
  * @param mass Mass of the collision shape (in kilograms)
  */
-ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const Transform& transform, decimal mass)
+ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const Transform& transform, decimal mass, Allocator& allocator)
            :mBody(body), mCollisionShape(shape), mLocalToBodyTransform(transform), mMass(mass),
-            mNext(nullptr), mBroadPhaseID(-1), mCollisionCategoryBits(0x0001), mCollideWithMaskBits(0xFFFF) {
+            mNext(nullptr), mBroadPhaseID(-1), mCollisionCategoryBits(0x0001), mCollideWithMaskBits(0xFFFF), mAllocator(allocator) {
 
 }
 
@@ -76,7 +76,7 @@ bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
                  worldToLocalTransform * ray.point2,
                  ray.maxFraction);
 
-    bool isHit = mCollisionShape->raycast(rayLocal, raycastInfo, this);
+    bool isHit = mCollisionShape->raycast(rayLocal, raycastInfo, this, mAllocator);
 
     // Convert the raycast info into world-space
     raycastInfo.worldPoint = localToWorldTransform * raycastInfo.worldPoint;

src/collision/ProxyShape.h

@@ -82,6 +82,9 @@ class ProxyShape {
         /// proxy shape will collide with every collision categories by default.
         unsigned short mCollideWithMaskBits;
 
+        /// Memory allocator
+        Allocator& mAllocator;
+
 #ifdef IS_PROFILING_ACTIVE
 
 		/// Pointer to the profiler
@@ -100,7 +103,7 @@ class ProxyShape {
 
         /// Constructor
         ProxyShape(CollisionBody* body, CollisionShape* shape,
-                   const Transform& transform, decimal mass);
+                   const Transform& transform, decimal mass, Allocator& allocator);
 
         /// Destructor
         virtual ~ProxyShape();

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
-        virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts)=0;
+        /// Compute a contact info if the two bounding volumes collide
+        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);
@@ -369,14 +374,14 @@ bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
     uint firstEdgeIndex = face.edgeIndex;
     uint edgeIndex = firstEdgeIndex;
 
-    std::vector<Vector3> planesPoints;
-    std::vector<Vector3> planesNormals;
+    List<Vector3> planesPoints(mMemoryAllocator, 2);
+    List<Vector3> planesNormals(mMemoryAllocator, 2);
 
     // For each adjacent edge of the separating face of the polyhedron
     do {
 
-        HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(edgeIndex);
-        HalfEdgeStructure::Edge twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex);
+        const HalfEdgeStructure::Edge& edge = polyhedron->getHalfEdge(edgeIndex);
+        const HalfEdgeStructure::Edge& twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex);
 
         // Compute the edge vertices and edge direction
         Vector3 edgeV1 = polyhedron->getVertexPosition(edge.vertexIndex);
@@ -388,15 +393,15 @@ bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
         Vector3 clipPlaneNormal = faceNormal.cross(edgeDirection);
 
         // Construct a clipping plane for each adjacent edge of the separating face of the polyhedron
-        planesPoints.push_back(polyhedron->getVertexPosition(edge.vertexIndex));
-        planesNormals.push_back(clipPlaneNormal);
+        planesPoints.add(polyhedron->getVertexPosition(edge.vertexIndex));
+        planesNormals.add(clipPlaneNormal);
 
         edgeIndex = edge.nextEdgeIndex;
 
     } while(edgeIndex != firstEdgeIndex);
 
     // First we clip the inner segment of the capsule with the four planes of the adjacent faces
-    std::vector<Vector3> clipSegment = clipSegmentWithPlanes(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, planesPoints, planesNormals);
+    List<Vector3> clipSegment = clipSegmentWithPlanes(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, planesPoints, planesNormals, mMemoryAllocator);
 
 	// Project the two clipped points into the polyhedron face
 	const Vector3 delta = faceNormal * (penetrationDepth - capsuleRadius);
@@ -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);
-            HalfEdgeStructure::Edge edge2 = polyhedron2->getHalfEdge(lastFrameCollisionInfo->satMinEdge2Index);
+            const HalfEdgeStructure::Edge& edge1 = polyhedron1->getHalfEdge(lastFrameCollisionInfo->satMinEdge1Index);
+            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,23 @@ 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);
+    uint incidentFaceIndex = incidentPolyhedron->findMostAntiParallelFace(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
-    std::vector<Vector3> planesNormals;     // Normals of the clipping planes
-    std::vector<Vector3> planesPoints;      // Points on the clipping planes
+    uint nbIncidentFaceVertices = static_cast<uint>(incidentFace.faceVertices.size());
+    List<Vector3> polygonVertices(mMemoryAllocator, nbIncidentFaceVertices);   // Vertices to clip of the incident face
+    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);
+    for (uint i=0; i < incidentFace.faceVertices.size(); i++) {
+        const Vector3 faceVertexIncidentSpace = incidentPolyhedron->getVertexPosition(incidentFace.faceVertices[i]);
+        polygonVertices.add(incidentToReferenceTransform * faceVertexIncidentSpace);
     }
 
     // Get the reference face clipping planes
@@ -841,10 +846,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 +860,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);
-        planesPoints.push_back(edgeV1);
+        planesNormals.add(clipPlaneNormal);
+        planesPoints.add(edgeV1);
 
         // Go to the next adjacent edge of the reference face
         currentEdgeIndex = edge.nextEdgeIndex;
@@ -867,17 +872,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 +891,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,
@@ -909,28 +913,6 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene
     return contactPointsFound;
 }
 
-// Find and return the index of the polyhedron face with the most anti-parallel face normal given a direction vector
-// This is used to find the incident face on a polyhedron of a given reference face of another polyhedron
-uint SATAlgorithm::findMostAntiParallelFaceOnPolyhedron(const ConvexPolyhedronShape* polyhedron, const Vector3& direction) const {
-
-    PROFILE("SATAlgorithm::findMostAntiParallelFaceOnPolyhedron", mProfiler);
-
-    decimal minDotProduct = DECIMAL_LARGEST;
-    uint mostAntiParallelFace = 0;
-
-    // For each face of the polyhedron
-    for (uint i=0; i < polyhedron->getNbFaces(); i++) {
-
-        // Get the face normal
-        decimal dotProduct = polyhedron->getFaceNormal(i).dot(direction);
-        if (dotProduct < minDotProduct) {
-            minDotProduct = dotProduct;
-            mostAntiParallelFace = i;
-        }
-    }
-
-    return mostAntiParallelFace;
-}
 
 // Compute and return the distance between the two edges in the direction of the candidate separating axis
 decimal SATAlgorithm::computeDistanceBetweenEdges(const Vector3& edge1A, const Vector3& edge2A, const Vector3& polyhedron2Centroid,
@@ -968,7 +950,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
@@ -69,9 +72,6 @@ class SATAlgorithm {
                                        const Vector3& c, const Vector3& d,
                                        const Vector3& bCrossA, const Vector3& dCrossC) const;
 
-        // Find and return the index of the polyhedron face with the most anti-parallel face normal given a direction vector
-        uint findMostAntiParallelFaceOnPolyhedron(const ConvexPolyhedronShape* polyhedron, const Vector3& direction) const;
-
         /// Compute and return the distance between the two edges in the direction of the candidate separating axis
         decimal computeDistanceBetweenEdges(const Vector3& edge1A, const Vector3& edge2A, const Vector3& polyhedron2Centroid,
                                             const Vector3& edge1Direction, const Vector3& edge2Direction,
@@ -115,7 +115,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

@@ -27,6 +27,7 @@
 #include "BoxShape.h"
 #include "collision/ProxyShape.h"
 #include "configuration.h"
+#include "memory/MemoryManager.h"
 #include <vector>
 #include <cassert>
 
@@ -37,7 +38,9 @@ using namespace reactphysics3d;
  * @param extent The vector with the three extents of the box (in meters)
  */
 BoxShape::BoxShape(const Vector3& extent)
-         : ConvexPolyhedronShape(CollisionShapeName::BOX), mExtent(extent) {
+         : ConvexPolyhedronShape(CollisionShapeName::BOX), mExtent(extent),
+           mHalfEdgeStructure(MemoryManager::getDefaultAllocator(), 6, 8, 24) {
+
     assert(extent.x > decimal(0.0));
     assert(extent.y > decimal(0.0));
     assert(extent.z > decimal(0.0));
@@ -52,19 +55,21 @@ BoxShape::BoxShape(const Vector3& extent)
     mHalfEdgeStructure.addVertex(6);
     mHalfEdgeStructure.addVertex(7);
 
+    DefaultAllocator& allocator = MemoryManager::getDefaultAllocator();
+
     // Faces
-    std::vector<uint> face0;
-    face0.push_back(0); face0.push_back(1); face0.push_back(2); face0.push_back(3);
-    std::vector<uint> face1;
-    face1.push_back(1); face1.push_back(5); face1.push_back(6); face1.push_back(2);
-    std::vector<uint> face2;
-    face2.push_back(4); face2.push_back(7); face2.push_back(6); face2.push_back(5);
-    std::vector<uint> face3;
-    face3.push_back(4); face3.push_back(0); face3.push_back(3); face3.push_back(7);
-    std::vector<uint> face4;
-    face4.push_back(4); face4.push_back(5); face4.push_back(1); face4.push_back(0);
-    std::vector<uint> face5;
-    face5.push_back(2); face5.push_back(6); face5.push_back(7); face5.push_back(3);
+    List<uint> face0(allocator, 4);
+    face0.add(0); face0.add(1); face0.add(2); face0.add(3);
+    List<uint> face1(allocator, 4);
+    face1.add(1); face1.add(5); face1.add(6); face1.add(2);
+    List<uint> face2(allocator, 4);
+    face2.add(4); face2.add(7); face2.add(6); face2.add(5);
+    List<uint> face3(allocator, 4);
+    face3.add(4); face3.add(0); face3.add(3); face3.add(7);
+    List<uint> face4(allocator, 4);
+    face4.add(4); face4.add(5); face4.add(1); face4.add(0);
+    List<uint> face5(allocator, 4);
+    face5.add(2); face5.add(6); face5.add(7); face5.add(3);
 
     mHalfEdgeStructure.addFace(face0);
     mHalfEdgeStructure.addFace(face1);
@@ -84,17 +89,16 @@ 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 = realExtent.x * realExtent.x;
-    decimal ySquare = realExtent.y * realExtent.y;
-    decimal zSquare = realExtent.z * realExtent.z;
+    decimal xSquare = mExtent.x * mExtent.x;
+    decimal ySquare = mExtent.y * mExtent.y;
+    decimal zSquare = mExtent.z * mExtent.z;
     tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0,
                         0.0, factor * (xSquare + zSquare), 0.0,
                         0.0, 0.0, factor * (xSquare + ySquare));
 }
 
 // Raycast method with feedback information
-bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
+bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const {
 
     Vector3 rayDirection = ray.point2 - ray.point1;
     decimal tMin = DECIMAL_SMALLEST;

src/collision/shapes/BoxShape.h

@@ -31,7 +31,7 @@
 #include "ConvexPolyhedronShape.h"
 #include "body/CollisionBody.h"
 #include "mathematics/mathematics.h"
-
+#include "memory/DefaultAllocator.h"
 
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
@@ -64,7 +64,7 @@ class BoxShape : public ConvexPolyhedronShape {
         virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override;
 
         /// Raycast method with feedback information
-        virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override;
+        virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override;
 
         /// Return the number of bytes used by the collision shape
         virtual size_t getSizeInBytes() const override;
@@ -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/CapsuleShape.cpp

@@ -85,7 +85,7 @@ bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyS
 }
 
 // Raycast method with feedback information
-bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
+bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const {
 
     const Vector3 n = ray.point2 - ray.point1;
 

src/collision/shapes/CapsuleShape.h

@@ -62,7 +62,7 @@ class CapsuleShape : public ConvexShape {
         virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override;
 
         /// Raycast method with feedback information
-        virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override;
+        virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override;
 
         /// Raycasting method between a ray one of the two spheres end cap of the capsule
         bool raycastWithSphereEndCap(const Vector3& point1, const Vector3& point2,

src/collision/shapes/CollisionShape.h

@@ -87,7 +87,7 @@ class CollisionShape {
         virtual bool testPointInside(const Vector3& worldPoint, ProxyShape* proxyShape) const=0;
 
         /// Raycast method with feedback information
-        virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const=0;
+        virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const=0;
 
         /// Return the number of bytes used by the collision shape
         virtual size_t getSizeInBytes() const = 0;

src/collision/shapes/ConcaveMeshShape.cpp

@@ -111,12 +111,12 @@ void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB&
 // Raycast method with feedback information
 /// Note that only the first triangle hit by the ray in the mesh will be returned, even if
 /// the ray hits many triangles.
-bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
+bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const {
 
     PROFILE("ConcaveMeshShape::raycast()", mProfiler);
 
     // Create the callback object that will compute ray casting against triangles
-    ConcaveMeshRaycastCallback raycastCallback(mDynamicAABBTree, *this, proxyShape, raycastInfo, ray);
+    ConcaveMeshRaycastCallback raycastCallback(mDynamicAABBTree, *this, proxyShape, raycastInfo, ray, allocator);
 
 #ifdef IS_PROFILING_ACTIVE
 
@@ -180,7 +180,7 @@ void ConcaveMeshRaycastCallback::raycastTriangles() {
         mConcaveMeshShape.getTriangleVerticesNormals(data[0], data[1], verticesNormals);
 
         // Create a triangle collision shape
-        TriangleShape triangleShape(trianglePoints, verticesNormals, mConcaveMeshShape.computeTriangleShapeId(data[0], data[1]));
+        TriangleShape triangleShape(trianglePoints, verticesNormals, mConcaveMeshShape.computeTriangleShapeId(data[0], data[1]), mAllocator);
         triangleShape.setRaycastTestType(mConcaveMeshShape.getRaycastTestType());
 		
 #ifdef IS_PROFILING_ACTIVE
@@ -192,7 +192,7 @@ void ConcaveMeshRaycastCallback::raycastTriangles() {
 
         // Ray casting test against the collision shape
         RaycastInfo raycastInfo;
-        bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape);
+        bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape, mAllocator);
 
         // If the ray hit the collision shape
         if (isTriangleHit && raycastInfo.hitFraction <= smallestHitFraction) {

src/collision/shapes/ConcaveMeshShape.h

@@ -77,6 +77,7 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback {
         RaycastInfo& mRaycastInfo;
         const Ray& mRay;
         bool mIsHit;
+        Allocator& mAllocator;
 
 #ifdef IS_PROFILING_ACTIVE
 
@@ -89,9 +90,9 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback {
 
         // Constructor
         ConcaveMeshRaycastCallback(const DynamicAABBTree& dynamicAABBTree, const ConcaveMeshShape& concaveMeshShape,
-                                   ProxyShape* proxyShape, RaycastInfo& raycastInfo, const Ray& ray)
+                                   ProxyShape* proxyShape, RaycastInfo& raycastInfo, const Ray& ray, Allocator& allocator)
             : mDynamicAABBTree(dynamicAABBTree), mConcaveMeshShape(concaveMeshShape), mProxyShape(proxyShape),
-              mRaycastInfo(raycastInfo), mRay(ray), mIsHit(false) {
+              mRaycastInfo(raycastInfo), mRay(ray), mIsHit(false), mAllocator(allocator) {
 
         }
 
@@ -141,7 +142,7 @@ class ConcaveMeshShape : public ConcaveShape {
         // -------------------- Methods -------------------- //
 
         /// Raycast method with feedback information
-        virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override;
+        virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override;
 
         /// Return the number of bytes used by the collision shape
         virtual size_t getSizeInBytes() const override;

src/collision/shapes/ConvexMeshShape.cpp

@@ -112,7 +112,7 @@ void ConvexMeshShape::recalculateBounds() {
 }
 
 // Raycast method with feedback information
-bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
+bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const {
     return proxyShape->mBody->mWorld.mCollisionDetection.mNarrowPhaseGJKAlgorithm.raycast(
                                      ray, proxyShape, raycastInfo);
 }

src/collision/shapes/ConvexMeshShape.h

@@ -77,7 +77,7 @@ class ConvexMeshShape : public ConvexPolyhedronShape {
         virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override;
 
         /// Raycast method with feedback information
-        virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override;
+        virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override;
 
         /// Return the number of bytes used by the collision shape
         virtual size_t getSizeInBytes() const override;
@@ -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.cpp

@@ -35,3 +35,25 @@ ConvexPolyhedronShape::ConvexPolyhedronShape(CollisionShapeName name)
             : ConvexShape(name, CollisionShapeType::CONVEX_POLYHEDRON) {
 
 }
+
+// Find and return the index of the polyhedron face with the most anti-parallel face
+// normal given a direction vector. This is used to find the incident face on
+// a polyhedron of a given reference face of another polyhedron
+uint ConvexPolyhedronShape::findMostAntiParallelFace(const Vector3& direction) const {
+
+    decimal minDotProduct = DECIMAL_LARGEST;
+    uint mostAntiParallelFace = 0;
+
+    // For each face of the polyhedron
+    for (uint i=0; i < getNbFaces(); i++) {
+
+        // Get the face normal
+        decimal dotProduct = getFaceNormal(i).dot(direction);
+        if (dotProduct < minDotProduct) {
+            minDotProduct = dotProduct;
+            mostAntiParallelFace = i;
+        }
+    }
+
+    return mostAntiParallelFace;
+}

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,13 +80,17 @@ 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;
 
         /// Return the centroid of the polyhedron
         virtual Vector3 getCentroid() const=0;
+
+        /// Find and return the index of the polyhedron face with the most anti-parallel face
+        /// normal given a direction vector
+        uint findMostAntiParallelFace(const Vector3& direction) const;
 };
 
 // Return true if the collision shape is a polyhedron
@@ -94,6 +98,7 @@ inline bool ConvexPolyhedronShape::isPolyhedron() const {
     return true;
 }
 
+
 }
 
 #endif

src/collision/shapes/HeightFieldShape.cpp

@@ -212,14 +212,14 @@ void HeightFieldShape::computeMinMaxGridCoordinates(int* minCoords, int* maxCoor
 // Raycast method with feedback information
 /// Note that only the first triangle hit by the ray in the mesh will be returned, even if
 /// the ray hits many triangles.
-bool HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
+bool HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const {
 
     // TODO : Implement raycasting without using an AABB for the ray
     //        but using a dynamic AABB tree or octree instead
 
     PROFILE("HeightFieldShape::raycast()", mProfiler);
 
-    TriangleOverlapCallback triangleCallback(ray, proxyShape, raycastInfo, *this);
+    TriangleOverlapCallback triangleCallback(ray, proxyShape, raycastInfo, *this, allocator);
 
 #ifdef IS_PROFILING_ACTIVE
 
@@ -266,7 +266,7 @@ Vector3 HeightFieldShape::getVertexAt(int x, int y) const {
 void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId) {
 
     // Create a triangle collision shape
-    TriangleShape triangleShape(trianglePoints, verticesNormals, shapeId);
+    TriangleShape triangleShape(trianglePoints, verticesNormals, shapeId, mAllocator);
     triangleShape.setRaycastTestType(mHeightFieldShape.getRaycastTestType());
 
 #ifdef IS_PROFILING_ACTIVE
@@ -278,7 +278,7 @@ void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints, const
 
     // Ray casting test against the collision shape
     RaycastInfo raycastInfo;
-    bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape);
+    bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape, mAllocator);
 
     // If the ray hit the collision shape
     if (isTriangleHit && raycastInfo.hitFraction <= mSmallestHitFraction) {

src/collision/shapes/HeightFieldShape.h

@@ -49,6 +49,7 @@ class TriangleOverlapCallback : public TriangleCallback {
         bool mIsHit;
         decimal mSmallestHitFraction;
         const HeightFieldShape& mHeightFieldShape;
+        Allocator& mAllocator;
 		
 #ifdef IS_PROFILING_ACTIVE
 
@@ -61,9 +62,9 @@ class TriangleOverlapCallback : public TriangleCallback {
 
         // Constructor
         TriangleOverlapCallback(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo,
-                                const HeightFieldShape& heightFieldShape)
+                                const HeightFieldShape& heightFieldShape, Allocator& allocator)
                                : mRay(ray), mProxyShape(proxyShape), mRaycastInfo(raycastInfo),
-                                 mHeightFieldShape (heightFieldShape) {
+                                 mHeightFieldShape (heightFieldShape), mAllocator(allocator) {
             mIsHit = false;
             mSmallestHitFraction = mRay.maxFraction;
         }
@@ -143,7 +144,7 @@ class HeightFieldShape : public ConcaveShape {
         // -------------------- Methods -------------------- //
 
         /// Raycast method with feedback information
-        virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override;
+        virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override;
 
         /// Return the number of bytes used by the collision shape
         virtual size_t getSizeInBytes() const override;

src/collision/shapes/SphereShape.cpp

@@ -41,7 +41,7 @@ SphereShape::SphereShape(decimal radius)
 }
 
 // Raycast method with feedback information
-bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
+bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const {
 
     const Vector3 m = ray.point1;
     decimal c = m.dot(m) - mMargin * mMargin;

src/collision/shapes/SphereShape.h

@@ -55,7 +55,7 @@ class SphereShape : public ConvexShape {
         virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override;
 
         /// Raycast method with feedback information
-        virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override;
+        virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override;
 
         /// Return the number of bytes used by the collision shape
         virtual size_t getSizeInBytes() const override;

src/collision/shapes/TriangleShape.cpp

@@ -33,6 +33,7 @@
 
 using namespace reactphysics3d;
 
+
 // Constructor
 /**
  * Do not use this constructor. It is supposed to be used internally only.
@@ -43,8 +44,9 @@ using namespace reactphysics3d;
  * @param verticesNormals The three vertices normals for smooth mesh collision
  * @param margin The collision margin (in meters) around the collision shape
  */
-TriangleShape::TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, uint shapeId)
-              : ConvexPolyhedronShape(CollisionShapeName::TRIANGLE) {
+TriangleShape::TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, uint shapeId,
+                             Allocator& allocator)
+    : ConvexPolyhedronShape(CollisionShapeName::TRIANGLE), mFaces{HalfEdgeStructure::Face(allocator), HalfEdgeStructure::Face(allocator)} {
 
     mPoints[0] = vertices[0];
     mPoints[1] = vertices[1];
@@ -58,6 +60,58 @@ 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.reserve(3);
+        mFaces[i].faceVertices.add(0);
+        mFaces[i].faceVertices.add(1);
+        mFaces[i].faceVertices.add(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;
@@ -157,7 +211,7 @@ Vector3 TriangleShape::computeSmoothLocalContactNormalForTriangle(const Vector3&
 // Raycast method with feedback information
 /// This method use the line vs triangle raycasting technique described in
 /// Real-time Collision Detection by Christer Ericson.
-bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
+bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const {
 
     PROFILE("TriangleShape::raycast()", mProfiler);
 
@@ -227,51 +281,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

@@ -60,6 +60,7 @@ class TriangleShape : public ConvexPolyhedronShape {
 
         // -------------------- Attribute -------------------- //
 
+
         /// Three points of the triangle
         Vector3 mPoints[3];
 
@@ -72,6 +73,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
@@ -84,7 +91,8 @@ class TriangleShape : public ConvexPolyhedronShape {
         virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override;
 
         /// Raycast method with feedback information
-        virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override;
+        virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,
+                             Allocator& allocator) const override;
 
         /// Return the number of bytes used by the collision shape
         virtual size_t getSizeInBytes() const override;
@@ -104,7 +112,8 @@ class TriangleShape : public ConvexPolyhedronShape {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, uint shapeId);
+        TriangleShape(const Vector3* vertices, const Vector3* verticesNormals,
+                      uint shapeId, Allocator& allocator);
 
         /// Destructor
         virtual ~TriangleShape() override = default;
@@ -137,7 +146,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 +164,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 +261,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);
-
-    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 mFaces[faceIndex];
 }
 
 // Return the number of vertices of the polyhedron
@@ -292,6 +284,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/constraint/BallAndSocketJoint.cpp

@@ -45,8 +45,8 @@ BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo)
 void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
 
     // Initialize the bodies index in the velocity array
-    mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
-    mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
+    mIndexBody1 = mBody1->mArrayIndex;
+    mIndexBody2 = mBody2->mArrayIndex;
 
     // Get the bodies center of mass and orientations
     const Vector3& x1 = mBody1->mCenterOfMassWorld;

src/constraint/ContactPoint.h

@@ -122,10 +122,10 @@ class ContactPoint {
         Vector3 getNormal() const;
 
         /// Return the contact point on the first proxy shape in the local-space of the proxy shape
-        Vector3 getLocalPointOnShape1() const;
+        const Vector3& getLocalPointOnShape1() const;
 
         /// Return the contact point on the second proxy shape in the local-space of the proxy shape
-        Vector3 getLocalPointOnShape2() const;
+        const Vector3& getLocalPointOnShape2() const;
 
         /// Return the cached penetration impulse
         decimal getPenetrationImpulse() const;
@@ -157,12 +157,12 @@ inline Vector3 ContactPoint::getNormal() const {
 }
 
 // Return the contact point on the first proxy shape in the local-space of the proxy shape
-inline Vector3 ContactPoint::getLocalPointOnShape1() const {
+inline const Vector3& ContactPoint::getLocalPointOnShape1() const {
     return mLocalPointOnShape1;
 }
 
 // Return the contact point on the second proxy shape in the local-space of the proxy shape
-inline Vector3 ContactPoint::getLocalPointOnShape2() const {
+inline const Vector3& ContactPoint::getLocalPointOnShape2() const {
     return mLocalPointOnShape2;
 }
 

src/constraint/FixedJoint.cpp

@@ -53,8 +53,8 @@ FixedJoint::FixedJoint(const FixedJointInfo& jointInfo)
 void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
 
     // Initialize the bodies index in the velocity array
-    mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
-    mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
+    mIndexBody1 = mBody1->mArrayIndex;
+    mIndexBody2 = mBody2->mArrayIndex;
 
     // Get the bodies positions and orientations
     const Vector3& x1 = mBody1->mCenterOfMassWorld;

src/constraint/HingeJoint.cpp

@@ -68,8 +68,8 @@ HingeJoint::HingeJoint(const HingeJointInfo& jointInfo)
 void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
 
     // Initialize the bodies index in the velocity array
-    mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
-    mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
+    mIndexBody1 = mBody1->mArrayIndex;
+    mIndexBody2 = mBody2->mArrayIndex;
 
     // Get the bodies positions and orientations
     const Vector3& x1 = mBody1->mCenterOfMassWorld;

src/constraint/SliderJoint.cpp

@@ -67,8 +67,8 @@ SliderJoint::SliderJoint(const SliderJointInfo& jointInfo)
 void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
 
     // Initialize the bodies index in the veloc ity array
-    mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
-    mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
+    mIndexBody1 = mBody1->mArrayIndex;
+    mIndexBody2 = mBody2->mArrayIndex;
 
     // Get the bodies positions and orientations
     const Vector3& x1 = mBody1->mCenterOfMassWorld;

src/containers/List.h

@@ -0,0 +1,220 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_LIST_H
+#define REACTPHYSICS3D_LIST_H
+
+// Libraries
+#include "configuration.h"
+#include "memory/Allocator.h"
+#include <cstring>
+
+namespace reactphysics3d {
+
+// Class List
+/**
+ * This class represents a simple generic list with custom memory allocator.
+  */
+template<typename T>
+class List {
+
+    private:
+
+        // -------------------- Attributes -------------------- //
+
+        /// Buffer for the list elements
+        void* mBuffer;
+
+        /// Number of elements in the list
+        size_t mSize;
+
+        /// Number of allocated elements in the list
+        size_t mCapacity;
+
+        /// Memory allocator
+        Allocator& mAllocator;
+
+        // -------------------- Methods -------------------- //
+
+
+    public:
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        List(Allocator& allocator, size_t capacity = 0)
+            : mBuffer(nullptr), mSize(0), mCapacity(0), mAllocator(allocator) {
+
+            if (capacity > 0) {
+
+                // Allocate memory
+                reserve(capacity);
+            }
+        }
+
+        /// Copy constructor
+        List(const List<T>& list) : mBuffer(nullptr), mSize(0), mCapacity(0), mAllocator(list.mAllocator) {
+
+            // All all the elements of the list to the current one
+            addRange(list);
+        }
+
+        /// Destructor
+        ~List() {
+
+            // If elements have been allocated
+            if (mCapacity > 0) {
+
+                // Clear the list
+                clear();
+
+                // Release the memory allocated on the heap
+                mAllocator.release(mBuffer, mCapacity * sizeof(T));
+            }
+        }
+
+        /// Allocate memory for a given number of elements
+        void reserve(size_t capacity) {
+
+            if (capacity <= mCapacity) return;
+
+            // Allocate memory for the new array
+            void* newMemory = mAllocator.allocate(capacity * sizeof(T));
+
+            if (mBuffer != nullptr) {
+
+                // Copy the elements to the new allocated memory location
+                std::memcpy(newMemory, mBuffer, mSize * sizeof(T));
+
+                // Release the previously allocated memory
+                mAllocator.release(mBuffer, mCapacity * sizeof(T));
+            }
+
+            mBuffer = newMemory;
+            assert(mBuffer != nullptr);
+
+            mCapacity = capacity;
+        }
+
+        /// Add an element into the list
+        void add(const T& element) {
+
+            // If we need to allocate more memory
+            if (mSize == mCapacity) {
+                reserve(mCapacity == 0 ? 1 : mCapacity * 2);
+            }
+
+            // Use the copy-constructor to construct the element
+            new (static_cast<char*>(mBuffer) + mSize * sizeof(T)) T(element);
+
+            mSize++;
+        }
+
+        /// Remove an element from the list at a given index
+        void remove(uint index) {
+
+          assert(index >= 0 && index < mSize);
+
+          // Call the destructor
+          (static_cast<T*>(mBuffer)[index]).~T();
+
+          mSize--;
+
+          if (index != mSize) {
+
+              // Move the elements to fill in the empty slot
+              char* dest = static_cast<char*>(mBuffer) + index * sizeof(T);
+              char* src = dest + sizeof(T);
+              std::memcpy(static_cast<void*>(dest), static_cast<void*>(src), (mSize - index) * sizeof(T));
+          }
+        }
+
+        /// Append another list to the current one
+        void addRange(const List<T>& list) {
+
+            // If we need to allocate more memory
+            if (mSize + list.size() > mCapacity) {
+
+                // Allocate memory
+                reserve(mSize + list.size());
+            }
+
+            // Add the elements of the list to the current one
+            for(uint i=0; i<list.size(); i++) {
+
+                new (static_cast<char*>(mBuffer) + mSize * sizeof(T)) T(list[i]);
+                mSize++;
+            }
+        }
+
+        /// Clear the list
+        void clear() {
+
+            // Call the destructor of each element
+            for (uint i=0; i < mSize; i++) {
+                (static_cast<T*>(mBuffer)[i]).~T();
+            }
+
+            mSize = 0;
+        }
+
+        /// Return the number of elments in the list
+        size_t size() const {
+            return mSize;
+        }
+
+        /// Return the capacity of the list
+        size_t capacity() const {
+            return mCapacity;
+        }
+
+        /// Overloaded index operator
+        T& operator[](const uint index) {
+           assert(index >= 0 && index < mSize);
+           return (static_cast<T*>(mBuffer)[index]);
+        }
+
+        /// Overloaded const index operator
+        const T& operator[](const uint index) const {
+           assert(index >= 0 && index < mSize);
+           return (static_cast<T*>(mBuffer)[index]);
+        }
+
+        /// Overloaded assignment operator
+        List<T>& operator=(const List<T>& list) {
+
+            // Clear all the elements
+            clear();
+
+            // Add all the elements of the list to the current one
+            addRange(list);
+
+            return *this;
+        }
+};
+
+}
+
+#endif

src/engine/CollisionWorld.cpp

@@ -187,4 +187,3 @@ AABB CollisionWorld::getWorldAABB(const ProxyShape* proxyShape) const {
 
    return mCollisionDetection.getWorldAABB(proxyShape);
 }
-

src/engine/CollisionWorld.h

@@ -39,6 +39,7 @@
 #include "collision/CollisionDetection.h"
 #include "constraint/Joint.h"
 #include "constraint/ContactPoint.h"
+#include "memory/DefaultAllocator.h"
 #include "memory/PoolAllocator.h"
 #include "EventListener.h"
 

src/engine/ConstraintSolver.cpp

@@ -30,9 +30,7 @@
 using namespace reactphysics3d;
 
 // Constructor
-ConstraintSolver::ConstraintSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
-                 : mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
-                   mIsWarmStartingActive(true), mConstraintSolverData(mapBodyToVelocityIndex) {
+ConstraintSolver::ConstraintSolver() : mIsWarmStartingActive(true) {
 
 #ifdef IS_PROFILING_ACTIVE
 

src/engine/ConstraintSolver.h

@@ -60,18 +60,12 @@ struct ConstraintSolverData {
         /// Reference to the bodies orientations
         Quaternion* orientations;
 
-        /// Reference to the map that associates rigid body to their index
-        /// in the constrained velocities array
-        const std::map<RigidBody*, uint>& mapBodyToConstrainedVelocityIndex;
-
         /// True if warm starting of the solver is active
         bool isWarmStartingActive;
 
         /// Constructor
-        ConstraintSolverData(const std::map<RigidBody*, uint>& refMapBodyToConstrainedVelocityIndex)
-                           :linearVelocities(nullptr), angularVelocities(nullptr),
-                            positions(nullptr), orientations(nullptr),
-                            mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){
+        ConstraintSolverData() :linearVelocities(nullptr), angularVelocities(nullptr),
+                                positions(nullptr), orientations(nullptr) {
 
         }
 
@@ -152,10 +146,6 @@ class ConstraintSolver {
 
         // -------------------- Attributes -------------------- //
 
-        /// Reference to the map that associates rigid body to their index in
-        /// the constrained velocities array
-        const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex;
-
         /// Current time step
         decimal mTimeStep;
 
@@ -176,7 +166,7 @@ class ConstraintSolver {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ConstraintSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
+        ConstraintSolver();
 
         /// Destructor
         ~ConstraintSolver() = default;

src/engine/ContactSolver.cpp

@@ -39,12 +39,10 @@ const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2);
 const decimal ContactSolver::SLOP = decimal(0.01);
 
 // Constructor
-ContactSolver::ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex,
-                             SingleFrameAllocator& allocator)
+ContactSolver::ContactSolver(SingleFrameAllocator& allocator)
               :mSplitLinearVelocities(nullptr), mSplitAngularVelocities(nullptr),
                mContactConstraints(nullptr), mSingleFrameAllocator(allocator),
                mLinearVelocities(nullptr), mAngularVelocities(nullptr),
-               mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
                mIsSplitImpulseActive(true) {
 
 }
@@ -131,8 +129,8 @@ void ContactSolver::initializeForIsland(Island* island) {
         // Initialize the internal contact manifold structure using the external
         // contact manifold
         new (mContactConstraints + mNbContactManifolds) ContactManifoldSolver();
-        mContactConstraints[mNbContactManifolds].indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second;
-        mContactConstraints[mNbContactManifolds].indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second;
+        mContactConstraints[mNbContactManifolds].indexBody1 = body1->mArrayIndex;
+        mContactConstraints[mNbContactManifolds].indexBody2 = body2->mArrayIndex;
         mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld();
         mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld();
         mContactConstraints[mNbContactManifolds].massInverseBody1 = body1->mMassInverse;
@@ -163,22 +161,48 @@ void ContactSolver::initializeForIsland(Island* island) {
             new (mContactPoints + mNbContactPoints) ContactPointSolver();
             mContactPoints[mNbContactPoints].externalContact = externalContact;
             mContactPoints[mNbContactPoints].normal = externalContact->getNormal();
-            mContactPoints[mNbContactPoints].r1 = p1 - x1;
-            mContactPoints[mNbContactPoints].r2 = p2 - x2;
+            mContactPoints[mNbContactPoints].r1.x = p1.x - x1.x;
+            mContactPoints[mNbContactPoints].r1.y = p1.y - x1.y;
+            mContactPoints[mNbContactPoints].r1.z = p1.z - x1.z;
+            mContactPoints[mNbContactPoints].r2.x = p2.x - x2.x;
+            mContactPoints[mNbContactPoints].r2.y = p2.y - x2.y;
+            mContactPoints[mNbContactPoints].r2.z = p2.z - x2.z;
             mContactPoints[mNbContactPoints].penetrationDepth = externalContact->getPenetrationDepth();
             mContactPoints[mNbContactPoints].isRestingContact = externalContact->getIsRestingContact();
             externalContact->setIsRestingContact(true);
             mContactPoints[mNbContactPoints].penetrationImpulse = externalContact->getPenetrationImpulse();
             mContactPoints[mNbContactPoints].penetrationSplitImpulse = 0.0;
 
-            mContactConstraints[mNbContactManifolds].frictionPointBody1 += p1;
-            mContactConstraints[mNbContactManifolds].frictionPointBody2 += p2;
+            mContactConstraints[mNbContactManifolds].frictionPointBody1.x += p1.x;
+            mContactConstraints[mNbContactManifolds].frictionPointBody1.y += p1.y;
+            mContactConstraints[mNbContactManifolds].frictionPointBody1.z += p1.z;
+            mContactConstraints[mNbContactManifolds].frictionPointBody2.x += p2.x;
+            mContactConstraints[mNbContactManifolds].frictionPointBody2.y += p2.y;
+            mContactConstraints[mNbContactManifolds].frictionPointBody2.z += p2.z;
 
             // Compute the velocity difference
-            Vector3 deltaV = v2 + w2.cross(mContactPoints[mNbContactPoints].r2) - v1 - w1.cross(mContactPoints[mNbContactPoints].r1);
+            //deltaV = v2 + w2.cross(mContactPoints[mNbContactPoints].r2) - v1 - w1.cross(mContactPoints[mNbContactPoints].r1);
+            Vector3 deltaV(v2.x + w2.y * mContactPoints[mNbContactPoints].r2.z - w2.z * mContactPoints[mNbContactPoints].r2.y
+                           - v1.x - w1.y * mContactPoints[mNbContactPoints].r1.z - w1.z * mContactPoints[mNbContactPoints].r1.y,
+                           v2.y + w2.z * mContactPoints[mNbContactPoints].r2.x - w2.x * mContactPoints[mNbContactPoints].r2.z
+                           - v1.y - w1.z * mContactPoints[mNbContactPoints].r1.x - w1.x * mContactPoints[mNbContactPoints].r1.z,
+                           v2.z + w2.x * mContactPoints[mNbContactPoints].r2.y - w2.y * mContactPoints[mNbContactPoints].r2.x
+                           - v1.z - w1.x * mContactPoints[mNbContactPoints].r1.y - w1.y * mContactPoints[mNbContactPoints].r1.x);
 
-            Vector3 r1CrossN = mContactPoints[mNbContactPoints].r1.cross(mContactPoints[mNbContactPoints].normal);
-            Vector3 r2CrossN = mContactPoints[mNbContactPoints].r2.cross(mContactPoints[mNbContactPoints].normal);
+            // r1CrossN = mContactPoints[mNbContactPoints].r1.cross(mContactPoints[mNbContactPoints].normal);
+            Vector3 r1CrossN(mContactPoints[mNbContactPoints].r1.y * mContactPoints[mNbContactPoints].normal.z -
+                             mContactPoints[mNbContactPoints].r1.z * mContactPoints[mNbContactPoints].normal.y,
+                             mContactPoints[mNbContactPoints].r1.z * mContactPoints[mNbContactPoints].normal.x -
+                             mContactPoints[mNbContactPoints].r1.x * mContactPoints[mNbContactPoints].normal.z,
+                             mContactPoints[mNbContactPoints].r1.x * mContactPoints[mNbContactPoints].normal.y -
+                             mContactPoints[mNbContactPoints].r1.y * mContactPoints[mNbContactPoints].normal.x);
+            // r2CrossN = mContactPoints[mNbContactPoints].r2.cross(mContactPoints[mNbContactPoints].normal);
+            Vector3 r2CrossN(mContactPoints[mNbContactPoints].r2.y * mContactPoints[mNbContactPoints].normal.z -
+                             mContactPoints[mNbContactPoints].r2.z * mContactPoints[mNbContactPoints].normal.y,
+                             mContactPoints[mNbContactPoints].r2.z * mContactPoints[mNbContactPoints].normal.x -
+                             mContactPoints[mNbContactPoints].r2.x * mContactPoints[mNbContactPoints].normal.z,
+                             mContactPoints[mNbContactPoints].r2.x * mContactPoints[mNbContactPoints].normal.y -
+                             mContactPoints[mNbContactPoints].r2.y * mContactPoints[mNbContactPoints].normal.x);
 
             mContactPoints[mNbContactPoints].i1TimesR1CrossN = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 * r1CrossN;
             mContactPoints[mNbContactPoints].i2TimesR2CrossN = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 * r2CrossN;
@@ -194,13 +218,18 @@ void ContactSolver::initializeForIsland(Island* island) {
             // at the beginning of the contact. Note that if it is a resting contact (normal
             // velocity bellow a given threshold), we do not add a restitution velocity bias
             mContactPoints[mNbContactPoints].restitutionBias = 0.0;
-            decimal deltaVDotN = deltaV.dot(mContactPoints[mNbContactPoints].normal);
+            // deltaVDotN = deltaV.dot(mContactPoints[mNbContactPoints].normal);
+            decimal deltaVDotN = deltaV.x * mContactPoints[mNbContactPoints].normal.x +
+                                 deltaV.y * mContactPoints[mNbContactPoints].normal.y +
+                                 deltaV.z * mContactPoints[mNbContactPoints].normal.z;
             const decimal restitutionFactor = computeMixedRestitutionFactor(body1, body2);
             if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) {
                 mContactPoints[mNbContactPoints].restitutionBias = restitutionFactor * deltaVDotN;
             }
 
-            mContactConstraints[mNbContactManifolds].normal += mContactPoints[mNbContactPoints].normal;
+            mContactConstraints[mNbContactManifolds].normal.x += mContactPoints[mNbContactPoints].normal.x;
+            mContactConstraints[mNbContactManifolds].normal.y += mContactPoints[mNbContactPoints].normal.y;
+            mContactConstraints[mNbContactManifolds].normal.z += mContactPoints[mNbContactPoints].normal.z;
 
             mNbContactPoints++;
 
@@ -209,8 +238,12 @@ void ContactSolver::initializeForIsland(Island* island) {
 
         mContactConstraints[mNbContactManifolds].frictionPointBody1 /=static_cast<decimal>(mContactConstraints[mNbContactManifolds].nbContacts);
         mContactConstraints[mNbContactManifolds].frictionPointBody2 /=static_cast<decimal>(mContactConstraints[mNbContactManifolds].nbContacts);
-        mContactConstraints[mNbContactManifolds].r1Friction = mContactConstraints[mNbContactManifolds].frictionPointBody1 - x1;
-        mContactConstraints[mNbContactManifolds].r2Friction = mContactConstraints[mNbContactManifolds].frictionPointBody2 - x2;
+        mContactConstraints[mNbContactManifolds].r1Friction.x = mContactConstraints[mNbContactManifolds].frictionPointBody1.x - x1.x;
+        mContactConstraints[mNbContactManifolds].r1Friction.y = mContactConstraints[mNbContactManifolds].frictionPointBody1.y - x1.y;
+        mContactConstraints[mNbContactManifolds].r1Friction.z = mContactConstraints[mNbContactManifolds].frictionPointBody1.z - x1.z;
+        mContactConstraints[mNbContactManifolds].r2Friction.x = mContactConstraints[mNbContactManifolds].frictionPointBody2.x - x2.x;
+        mContactConstraints[mNbContactManifolds].r2Friction.y = mContactConstraints[mNbContactManifolds].frictionPointBody2.y - x2.y;
+        mContactConstraints[mNbContactManifolds].r2Friction.z = mContactConstraints[mNbContactManifolds].frictionPointBody2.z - x2.z;
         mContactConstraints[mNbContactManifolds].oldFrictionVector1 = externalManifold->getFrictionVector1();
         mContactConstraints[mNbContactManifolds].oldFrictionVector2 = externalManifold->getFrictionVector2();
 
@@ -230,8 +263,20 @@ void ContactSolver::initializeForIsland(Island* island) {
 
         mContactConstraints[mNbContactManifolds].normal.normalize();
 
-        Vector3 deltaVFrictionPoint = v2 + w2.cross(mContactConstraints[mNbContactManifolds].r2Friction) -
-                                      v1 - w1.cross(mContactConstraints[mNbContactManifolds].r1Friction);
+        // deltaVFrictionPoint = v2 + w2.cross(mContactConstraints[mNbContactManifolds].r2Friction) -
+        //                              v1 - w1.cross(mContactConstraints[mNbContactManifolds].r1Friction);
+        Vector3 deltaVFrictionPoint(v2.x + w2.y * mContactConstraints[mNbContactManifolds].r2Friction.z -
+                                    w2.z * mContactConstraints[mNbContactManifolds].r2Friction.y -
+                                      v1.x - w1.y * mContactConstraints[mNbContactManifolds].r1Friction.z -
+                                      w1.z * mContactConstraints[mNbContactManifolds].r1Friction.y,
+                                   v2.y + w2.z * mContactConstraints[mNbContactManifolds].r2Friction.x -
+                                    w2.x * mContactConstraints[mNbContactManifolds].r2Friction.z -
+                                      v1.y - w1.z * mContactConstraints[mNbContactManifolds].r1Friction.x -
+                                      w1.x * mContactConstraints[mNbContactManifolds].r1Friction.z,
+                                   v2.z + w2.x * mContactConstraints[mNbContactManifolds].r2Friction.y -
+                                    w2.y * mContactConstraints[mNbContactManifolds].r2Friction.x -
+                                      v1.z - w1.x * mContactConstraints[mNbContactManifolds].r1Friction.y -
+                                      w1.y * mContactConstraints[mNbContactManifolds].r1Friction.x);
 
         // Compute the friction vectors
         computeFrictionVectors(deltaVFrictionPoint, mContactConstraints[mNbContactManifolds]);
@@ -289,13 +334,25 @@ void ContactSolver::warmStart() {
                 // --------- Penetration --------- //
 
                 // Update the velocities of the body 1 by applying the impulse P
-                Vector3 impulsePenetration = mContactPoints[contactPointIndex].normal * mContactPoints[contactPointIndex].penetrationImpulse;
-                mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * impulsePenetration;
-                mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactPoints[contactPointIndex].i1TimesR1CrossN * mContactPoints[contactPointIndex].penetrationImpulse;
+                Vector3 impulsePenetration(mContactPoints[contactPointIndex].normal.x * mContactPoints[contactPointIndex].penetrationImpulse,
+                                           mContactPoints[contactPointIndex].normal.y * mContactPoints[contactPointIndex].penetrationImpulse,
+                                           mContactPoints[contactPointIndex].normal.z * mContactPoints[contactPointIndex].penetrationImpulse);
+                mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * impulsePenetration.x;
+                mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * impulsePenetration.y;
+                mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * impulsePenetration.z;
+
+                mAngularVelocities[mContactConstraints[c].indexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse;
+                mAngularVelocities[mContactConstraints[c].indexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse;
+                mAngularVelocities[mContactConstraints[c].indexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse;
 
                 // Update the velocities of the body 2 by applying the impulse P
-                mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * impulsePenetration;
-                mAngularVelocities[mContactConstraints[c].indexBody2] += mContactPoints[contactPointIndex].i2TimesR2CrossN * mContactPoints[contactPointIndex].penetrationImpulse;
+                mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * impulsePenetration.x;
+                mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * impulsePenetration.y;
+                mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * impulsePenetration.z;
+
+                mAngularVelocities[mContactConstraints[c].indexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse;
+                mAngularVelocities[mContactConstraints[c].indexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse;
+                mAngularVelocities[mContactConstraints[c].indexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse;
             }
             else {  // If it is a new contact point
 
@@ -312,20 +369,27 @@ void ContactSolver::warmStart() {
 
             // Project the old friction impulses (with old friction vectors) into the new friction
             // vectors to get the new friction impulses
-            Vector3 oldFrictionImpulse = mContactConstraints[c].friction1Impulse * mContactConstraints[c].oldFrictionVector1 +
-                                         mContactConstraints[c].friction2Impulse * mContactConstraints[c].oldFrictionVector2;
+            Vector3 oldFrictionImpulse(mContactConstraints[c].friction1Impulse * mContactConstraints[c].oldFrictionVector1.x +
+                                         mContactConstraints[c].friction2Impulse * mContactConstraints[c].oldFrictionVector2.x,
+                                       mContactConstraints[c].friction1Impulse * mContactConstraints[c].oldFrictionVector1.y +
+                                         mContactConstraints[c].friction2Impulse * mContactConstraints[c].oldFrictionVector2.y,
+                                       mContactConstraints[c].friction1Impulse * mContactConstraints[c].oldFrictionVector1.z +
+                                         mContactConstraints[c].friction2Impulse * mContactConstraints[c].oldFrictionVector2.z);
             mContactConstraints[c].friction1Impulse = oldFrictionImpulse.dot(mContactConstraints[c].frictionVector1);
             mContactConstraints[c].friction2Impulse = oldFrictionImpulse.dot(mContactConstraints[c].frictionVector2);
 
             // ------ First friction constraint at the center of the contact manifold ------ //
 
             // Compute the impulse P = J^T * lambda
-            Vector3 angularImpulseBody1 = -mContactConstraints[c].r1CrossT1 *
-                                           mContactConstraints[c].friction1Impulse;
-            Vector3 linearImpulseBody2 = mContactConstraints[c].frictionVector1 *
-                                         mContactConstraints[c].friction1Impulse;
-            Vector3 angularImpulseBody2 = mContactConstraints[c].r2CrossT1 *
-                                          mContactConstraints[c].friction1Impulse;
+            Vector3 angularImpulseBody1(-mContactConstraints[c].r1CrossT1.x * mContactConstraints[c].friction1Impulse,
+                                        -mContactConstraints[c].r1CrossT1.y * mContactConstraints[c].friction1Impulse,
+                                        -mContactConstraints[c].r1CrossT1.z * mContactConstraints[c].friction1Impulse);
+            Vector3 linearImpulseBody2(mContactConstraints[c].frictionVector1.x * mContactConstraints[c].friction1Impulse,
+                                       mContactConstraints[c].frictionVector1.y * mContactConstraints[c].friction1Impulse,
+                                       mContactConstraints[c].frictionVector1.z * mContactConstraints[c].friction1Impulse);
+            Vector3 angularImpulseBody2(mContactConstraints[c].r2CrossT1.x * mContactConstraints[c].friction1Impulse,
+                                        mContactConstraints[c].r2CrossT1.y * mContactConstraints[c].friction1Impulse,
+                                        mContactConstraints[c].r2CrossT1.z * mContactConstraints[c].friction1Impulse);
 
             // Update the velocities of the body 1 by applying the impulse P
             mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2;
@@ -338,23 +402,40 @@ void ContactSolver::warmStart() {
             // ------ Second friction constraint at the center of the contact manifold ----- //
 
             // Compute the impulse P = J^T * lambda
-            angularImpulseBody1 = -mContactConstraints[c].r1CrossT2 * mContactConstraints[c].friction2Impulse;
-            linearImpulseBody2 = mContactConstraints[c].frictionVector2 * mContactConstraints[c].friction2Impulse;
-            angularImpulseBody2 = mContactConstraints[c].r2CrossT2 * mContactConstraints[c].friction2Impulse;
+            angularImpulseBody1.x = -mContactConstraints[c].r1CrossT2.x * mContactConstraints[c].friction2Impulse;
+            angularImpulseBody1.y = -mContactConstraints[c].r1CrossT2.y * mContactConstraints[c].friction2Impulse;
+            angularImpulseBody1.z = -mContactConstraints[c].r1CrossT2.z * mContactConstraints[c].friction2Impulse;
+            linearImpulseBody2.x = mContactConstraints[c].frictionVector2.x * mContactConstraints[c].friction2Impulse;
+            linearImpulseBody2.y = mContactConstraints[c].frictionVector2.y * mContactConstraints[c].friction2Impulse;
+            linearImpulseBody2.z = mContactConstraints[c].frictionVector2.z * mContactConstraints[c].friction2Impulse;
+            angularImpulseBody2.x = mContactConstraints[c].r2CrossT2.x * mContactConstraints[c].friction2Impulse;
+            angularImpulseBody2.y = mContactConstraints[c].r2CrossT2.y * mContactConstraints[c].friction2Impulse;
+            angularImpulseBody2.z = mContactConstraints[c].r2CrossT2.z * mContactConstraints[c].friction2Impulse;
 
             // Update the velocities of the body 1 by applying the impulse P
-            mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2;
+            mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
+            mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
+            mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
+
             mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
 
             // Update the velocities of the body 2 by applying the impulse P
-            mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2;
+            mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
+            mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
+            mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
+
             mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
 
             // ------ Twist friction constraint at the center of the contact manifold ------ //
 
             // Compute the impulse P = J^T * lambda
-            angularImpulseBody1 = -mContactConstraints[c].normal * mContactConstraints[c].frictionTwistImpulse;
-            angularImpulseBody2 = mContactConstraints[c].normal * mContactConstraints[c].frictionTwistImpulse;
+            angularImpulseBody1.x = -mContactConstraints[c].normal.x * mContactConstraints[c].frictionTwistImpulse;
+            angularImpulseBody1.y = -mContactConstraints[c].normal.y * mContactConstraints[c].frictionTwistImpulse;
+            angularImpulseBody1.z = -mContactConstraints[c].normal.z * mContactConstraints[c].frictionTwistImpulse;
+
+            angularImpulseBody2.x = mContactConstraints[c].normal.x * mContactConstraints[c].frictionTwistImpulse;
+            angularImpulseBody2.y = mContactConstraints[c].normal.y * mContactConstraints[c].frictionTwistImpulse;
+            angularImpulseBody2.z = mContactConstraints[c].normal.z * mContactConstraints[c].frictionTwistImpulse;
 
             // Update the velocities of the body 1 by applying the impulse P
             mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
@@ -409,8 +490,15 @@ void ContactSolver::solve() {
             // --------- Penetration --------- //
 
             // Compute J*v
-            Vector3 deltaV = v2 + w2.cross(mContactPoints[contactPointIndex].r2) - v1 - w1.cross(mContactPoints[contactPointIndex].r1);
-            decimal deltaVDotN = deltaV.dot(mContactPoints[contactPointIndex].normal);
+            //Vector3 deltaV = v2 + w2.cross(mContactPoints[contactPointIndex].r2) - v1 - w1.cross(mContactPoints[contactPointIndex].r1);
+            Vector3 deltaV(v2.x + w2.y * mContactPoints[contactPointIndex].r2.z - w2.z * mContactPoints[contactPointIndex].r2.y - v1.x -
+                           w1.y * mContactPoints[contactPointIndex].r1.z + w1.z * mContactPoints[contactPointIndex].r1.y,
+                           v2.y + w2.z * mContactPoints[contactPointIndex].r2.x - w2.x * mContactPoints[contactPointIndex].r2.z - v1.y -
+                           w1.z * mContactPoints[contactPointIndex].r1.x + w1.x * mContactPoints[contactPointIndex].r1.z,
+                           v2.z + w2.x * mContactPoints[contactPointIndex].r2.y - w2.y * mContactPoints[contactPointIndex].r2.x - v1.z -
+                           w1.x * mContactPoints[contactPointIndex].r1.y + w1.y * mContactPoints[contactPointIndex].r1.x);
+            decimal deltaVDotN = deltaV.x * mContactPoints[contactPointIndex].normal.x + deltaV.y * mContactPoints[contactPointIndex].normal.y +
+                                 deltaV.z * mContactPoints[contactPointIndex].normal.z;
             decimal Jv = deltaVDotN;
 
             // Compute the bias "b" of the constraint
@@ -433,15 +521,27 @@ void ContactSolver::solve() {
                                                        deltaLambda, decimal(0.0));
             deltaLambda = mContactPoints[contactPointIndex].penetrationImpulse - lambdaTemp;
 
-            Vector3 linearImpulse = mContactPoints[contactPointIndex].normal * deltaLambda;
+            Vector3 linearImpulse(mContactPoints[contactPointIndex].normal.x * deltaLambda,
+                                  mContactPoints[contactPointIndex].normal.y * deltaLambda,
+                                  mContactPoints[contactPointIndex].normal.z * deltaLambda);
 
             // Update the velocities of the body 1 by applying the impulse P
-            mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulse;
-            mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactPoints[contactPointIndex].i1TimesR1CrossN * deltaLambda;
+            mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x;
+            mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y;
+            mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z;
+
+            mAngularVelocities[mContactConstraints[c].indexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambda;
+            mAngularVelocities[mContactConstraints[c].indexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambda;
+            mAngularVelocities[mContactConstraints[c].indexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambda;
 
             // Update the velocities of the body 2 by applying the impulse P
-            mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulse;
-            mAngularVelocities[mContactConstraints[c].indexBody2] += mContactPoints[contactPointIndex].i2TimesR2CrossN * deltaLambda;
+            mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x;
+            mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y;
+            mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z;
+
+            mAngularVelocities[mContactConstraints[c].indexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambda;
+            mAngularVelocities[mContactConstraints[c].indexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambda;
+            mAngularVelocities[mContactConstraints[c].indexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambda;
 
             sumPenetrationImpulse += mContactPoints[contactPointIndex].penetrationImpulse;
 
@@ -453,9 +553,17 @@ void ContactSolver::solve() {
                 const Vector3& w1Split = mSplitAngularVelocities[mContactConstraints[c].indexBody1];
                 const Vector3& v2Split = mSplitLinearVelocities[mContactConstraints[c].indexBody2];
                 const Vector3& w2Split = mSplitAngularVelocities[mContactConstraints[c].indexBody2];
-                Vector3 deltaVSplit = v2Split + w2Split.cross(mContactPoints[contactPointIndex].r2) -
-                        v1Split - w1Split.cross(mContactPoints[contactPointIndex].r1);
-                decimal JvSplit = deltaVSplit.dot(mContactPoints[contactPointIndex].normal);
+
+                //Vector3 deltaVSplit = v2Split + w2Split.cross(mContactPoints[contactPointIndex].r2) - v1Split - w1Split.cross(mContactPoints[contactPointIndex].r1);
+                Vector3 deltaVSplit(v2Split.x + w2Split.y * mContactPoints[contactPointIndex].r2.z - w2Split.z * mContactPoints[contactPointIndex].r2.y - v1Split.x -
+                                    w1Split.y * mContactPoints[contactPointIndex].r1.z + w1Split.z * mContactPoints[contactPointIndex].r1.y,
+                                    v2Split.y + w2Split.z * mContactPoints[contactPointIndex].r2.x - w2Split.x * mContactPoints[contactPointIndex].r2.z - v1Split.y -
+                                    w1Split.z * mContactPoints[contactPointIndex].r1.x + w1Split.x * mContactPoints[contactPointIndex].r1.z,
+                                    v2Split.z + w2Split.x * mContactPoints[contactPointIndex].r2.y - w2Split.y * mContactPoints[contactPointIndex].r2.x - v1Split.z -
+                                    w1Split.x * mContactPoints[contactPointIndex].r1.y + w1Split.y * mContactPoints[contactPointIndex].r1.x);
+                decimal JvSplit = deltaVSplit.x * mContactPoints[contactPointIndex].normal.x +
+                                  deltaVSplit.y * mContactPoints[contactPointIndex].normal.y +
+                                  deltaVSplit.z * mContactPoints[contactPointIndex].normal.z;
                 decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) *
                         mContactPoints[contactPointIndex].inversePenetrationMass;
                 decimal lambdaTempSplit = mContactPoints[contactPointIndex].penetrationSplitImpulse;
@@ -464,15 +572,27 @@ void ContactSolver::solve() {
                             deltaLambdaSplit, decimal(0.0));
                 deltaLambdaSplit = mContactPoints[contactPointIndex].penetrationSplitImpulse - lambdaTempSplit;
 
-                Vector3 linearImpulse = mContactPoints[contactPointIndex].normal * deltaLambdaSplit;
+                Vector3 linearImpulse(mContactPoints[contactPointIndex].normal.x * deltaLambdaSplit,
+                                      mContactPoints[contactPointIndex].normal.y * deltaLambdaSplit,
+                                      mContactPoints[contactPointIndex].normal.z * deltaLambdaSplit);
 
                 // Update the velocities of the body 1 by applying the impulse P
-                mSplitLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulse;
-                mSplitAngularVelocities[mContactConstraints[c].indexBody1] -= mContactPoints[contactPointIndex].i1TimesR1CrossN * deltaLambdaSplit;
+                mSplitLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x;
+                mSplitLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y;
+                mSplitLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z;
+
+                mSplitAngularVelocities[mContactConstraints[c].indexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambdaSplit;
+                mSplitAngularVelocities[mContactConstraints[c].indexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambdaSplit;
+                mSplitAngularVelocities[mContactConstraints[c].indexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambdaSplit;
 
                 // Update the velocities of the body 1 by applying the impulse P
-                mSplitLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulse;
-                mSplitAngularVelocities[mContactConstraints[c].indexBody2] += mContactPoints[contactPointIndex].i2TimesR2CrossN * deltaLambdaSplit;
+                mSplitLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x;
+                mSplitLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y;
+                mSplitLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z;
+
+                mSplitAngularVelocities[mContactConstraints[c].indexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambdaSplit;
+                mSplitAngularVelocities[mContactConstraints[c].indexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambdaSplit;
+                mSplitAngularVelocities[mContactConstraints[c].indexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambdaSplit;
             }
 
             contactPointIndex++;
@@ -481,9 +601,16 @@ void ContactSolver::solve() {
         // ------ First friction constraint at the center of the contact manifol ------ //
 
         // Compute J*v
-        Vector3 deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction)
-                - v1 - w1.cross(mContactConstraints[c].r1Friction);
-        decimal Jv = deltaV.dot(mContactConstraints[c].frictionVector1);
+        // deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction) - v1 - w1.cross(mContactConstraints[c].r1Friction);
+        Vector3 deltaV(v2.x + w2.y * mContactConstraints[c].r2Friction.z - w2.z * mContactConstraints[c].r2Friction.y - v1.x -
+                       w1.y * mContactConstraints[c].r1Friction.z + w1.z * mContactConstraints[c].r1Friction.y,
+                       v2.y + w2.z * mContactConstraints[c].r2Friction.x - w2.x * mContactConstraints[c].r2Friction.z - v1.y -
+                       w1.z * mContactConstraints[c].r1Friction.x + w1.x * mContactConstraints[c].r1Friction.z,
+                       v2.z + w2.x * mContactConstraints[c].r2Friction.y - w2.y * mContactConstraints[c].r2Friction.x - v1.z -
+                       w1.x * mContactConstraints[c].r1Friction.y + w1.y * mContactConstraints[c].r1Friction.x);
+        decimal Jv = deltaV.x * mContactConstraints[c].frictionVector1.x +
+                     deltaV.y * mContactConstraints[c].frictionVector1.y +
+                     deltaV.z * mContactConstraints[c].frictionVector1.z;
 
         // Compute the Lagrange multiplier lambda
         decimal deltaLambda = -Jv * mContactConstraints[c].inverseFriction1Mass;
@@ -495,23 +622,42 @@ void ContactSolver::solve() {
         deltaLambda = mContactConstraints[c].friction1Impulse - lambdaTemp;
 
         // Compute the impulse P=J^T * lambda
-        Vector3 angularImpulseBody1 = -mContactConstraints[c].r1CrossT1 * deltaLambda;
-        Vector3 linearImpulseBody2 = mContactConstraints[c].frictionVector1 * deltaLambda;
-        Vector3 angularImpulseBody2 = mContactConstraints[c].r2CrossT1 * deltaLambda;
+        Vector3 angularImpulseBody1(-mContactConstraints[c].r1CrossT1.x * deltaLambda,
+                                    -mContactConstraints[c].r1CrossT1.y * deltaLambda,
+                                    -mContactConstraints[c].r1CrossT1.z * deltaLambda);
+        Vector3 linearImpulseBody2(mContactConstraints[c].frictionVector1.x * deltaLambda,
+                                   mContactConstraints[c].frictionVector1.y * deltaLambda,
+                                   mContactConstraints[c].frictionVector1.z * deltaLambda);
+        Vector3 angularImpulseBody2(mContactConstraints[c].r2CrossT1.x * deltaLambda,
+                                    mContactConstraints[c].r2CrossT1.y * deltaLambda,
+                                    mContactConstraints[c].r2CrossT1.z * deltaLambda);
 
         // Update the velocities of the body 1 by applying the impulse P
-        mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2;
+        mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
+        mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
+        mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
+
         mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
 
         // Update the velocities of the body 2 by applying the impulse P
-        mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2;
+        mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
+        mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
+        mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
+
         mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
 
         // ------ Second friction constraint at the center of the contact manifol ----- //
 
         // Compute J*v
-        deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction) - v1 - w1.cross(mContactConstraints[c].r1Friction);
-        Jv = deltaV.dot(mContactConstraints[c].frictionVector2);
+        //deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction) - v1 - w1.cross(mContactConstraints[c].r1Friction);
+        deltaV.x = v2.x + w2.y * mContactConstraints[c].r2Friction.z - v2.z * mContactConstraints[c].r2Friction.y  - v1.x -
+                   w1.y * mContactConstraints[c].r1Friction.z + w1.z * mContactConstraints[c].r1Friction.y;
+        deltaV.y = v2.y + w2.z * mContactConstraints[c].r2Friction.x - v2.x * mContactConstraints[c].r2Friction.z  - v1.y -
+                   w1.z * mContactConstraints[c].r1Friction.x + w1.x * mContactConstraints[c].r1Friction.z;
+        deltaV.z = v2.z + w2.x * mContactConstraints[c].r2Friction.y - v2.y * mContactConstraints[c].r2Friction.x  - v1.z -
+                   w1.x * mContactConstraints[c].r1Friction.y + w1.y * mContactConstraints[c].r1Friction.x;
+        Jv = deltaV.x * mContactConstraints[c].frictionVector2.x + deltaV.y * mContactConstraints[c].frictionVector2.y +
+             deltaV.z * mContactConstraints[c].frictionVector2.z;
 
         // Compute the Lagrange multiplier lambda
         deltaLambda = -Jv * mContactConstraints[c].inverseFriction2Mass;
@@ -523,23 +669,36 @@ void ContactSolver::solve() {
         deltaLambda = mContactConstraints[c].friction2Impulse - lambdaTemp;
 
         // Compute the impulse P=J^T * lambda
-        angularImpulseBody1 = -mContactConstraints[c].r1CrossT2 * deltaLambda;
-        linearImpulseBody2 = mContactConstraints[c].frictionVector2 * deltaLambda;
-        angularImpulseBody2 = mContactConstraints[c].r2CrossT2 * deltaLambda;
+        angularImpulseBody1.x = -mContactConstraints[c].r1CrossT2.x * deltaLambda;
+        angularImpulseBody1.y = -mContactConstraints[c].r1CrossT2.y * deltaLambda;
+        angularImpulseBody1.z = -mContactConstraints[c].r1CrossT2.z * deltaLambda;
+
+        linearImpulseBody2.x = mContactConstraints[c].frictionVector2.x * deltaLambda;
+        linearImpulseBody2.y = mContactConstraints[c].frictionVector2.y * deltaLambda;
+        linearImpulseBody2.z = mContactConstraints[c].frictionVector2.z * deltaLambda;
+
+        angularImpulseBody2.x = mContactConstraints[c].r2CrossT2.x * deltaLambda;
+        angularImpulseBody2.y = mContactConstraints[c].r2CrossT2.y * deltaLambda;
+        angularImpulseBody2.z = mContactConstraints[c].r2CrossT2.z * deltaLambda;
 
         // Update the velocities of the body 1 by applying the impulse P
-        mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2;
+        mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
+        mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
+        mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
         mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
 
         // Update the velocities of the body 2 by applying the impulse P
-        mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2;
+        mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
+        mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
+        mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
         mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
 
         // ------ Twist friction constraint at the center of the contact manifol ------ //
 
         // Compute J*v
         deltaV = w2 - w1;
-        Jv = deltaV.dot(mContactConstraints[c].normal);
+        Jv = deltaV.x * mContactConstraints[c].normal.x + deltaV.y * mContactConstraints[c].normal.y +
+             deltaV.z * mContactConstraints[c].normal.z;
 
         deltaLambda = -Jv * (mContactConstraints[c].inverseTwistFrictionMass);
         frictionLimit = mContactConstraints[c].frictionCoefficient * sumPenetrationImpulse;
@@ -550,7 +709,9 @@ void ContactSolver::solve() {
         deltaLambda = mContactConstraints[c].frictionTwistImpulse - lambdaTemp;
 
         // Compute the impulse P=J^T * lambda
-        angularImpulseBody2 = mContactConstraints[c].normal * deltaLambda;
+        angularImpulseBody2.x = mContactConstraints[c].normal.x * deltaLambda;
+        angularImpulseBody2.y = mContactConstraints[c].normal.y * deltaLambda;
+        angularImpulseBody2.z = mContactConstraints[c].normal.z * deltaLambda;
 
         // Update the velocities of the body 1 by applying the impulse P
         mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2;
@@ -619,8 +780,11 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
     assert(contact.normal.length() > decimal(0.0));
 
     // Compute the velocity difference vector in the tangential plane
-    Vector3 normalVelocity = deltaVelocity.dot(contact.normal) * contact.normal;
-    Vector3 tangentVelocity = deltaVelocity - normalVelocity;
+    Vector3 normalVelocity(deltaVelocity.x * contact.normal.x * contact.normal.x,
+                           deltaVelocity.y * contact.normal.y * contact.normal.y,
+                           deltaVelocity.z * contact.normal.z * contact.normal.z);
+    Vector3 tangentVelocity(deltaVelocity.x - normalVelocity.x, deltaVelocity.y - normalVelocity.y,
+                            deltaVelocity.z - normalVelocity.z);
 
     // If the velocty difference in the tangential plane is not zero
     decimal lengthTangenVelocity = tangentVelocity.length();

src/engine/ContactSolver.h

@@ -302,9 +302,6 @@ class ContactSolver {
         /// Array of angular velocities
         Vector3* mAngularVelocities;
 
-        /// Reference to the map of rigid body to their index in the constrained velocities array
-        const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex;
-
         /// True if the split impulse position correction is active
         bool mIsSplitImpulseActive;
 
@@ -342,8 +339,7 @@ class ContactSolver {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex,
-                      SingleFrameAllocator& allocator);
+        ContactSolver(SingleFrameAllocator& allocator);
 
         /// Destructor
         ~ContactSolver() = default;

src/engine/DynamicsWorld.cpp

@@ -41,8 +41,7 @@ using namespace std;
  */
 DynamicsWorld::DynamicsWorld(const Vector3 &gravity)
               : CollisionWorld(),
-                mContactSolver(mMapBodyToConstrainedVelocityIndex, mSingleFrameAllocator),
-                mConstraintSolver(mMapBodyToConstrainedVelocityIndex),
+                mContactSolver(mSingleFrameAllocator),
                 mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS),
                 mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS),
                 mIsSleepingEnabled(SLEEPING_ENABLED), mGravity(gravity), mTimeStep(decimal(1.0f / 60.0f)),
@@ -167,7 +166,7 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
         for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
 
             // Get the constrained velocity
-            uint indexArray = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
+            uint indexArray = bodies[b]->mArrayIndex;
             Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray];
             Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray];
 
@@ -205,7 +204,7 @@ void DynamicsWorld::updateBodiesState() {
 
         for (uint b=0; b < mIslands[islandIndex]->getNbBodies(); b++) {
 
-            uint index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
+            uint index = bodies[b]->mArrayIndex;
 
             // Update the linear and angular velocity of the body
             bodies[b]->mLinearVelocity = mConstrainedLinearVelocities[index];
@@ -250,21 +249,14 @@ void DynamicsWorld::initVelocityArrays() {
     assert(mConstrainedPositions != nullptr);
     assert(mConstrainedOrientations != nullptr);
 
-    // Reset the velocities arrays
-    for (uint i=0; i<nbBodies; i++) {
+    // Initialize the map of body indexes in the velocity arrays
+    uint i = 0;
+    for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
+
         mSplitLinearVelocities[i].setToZero();
         mSplitAngularVelocities[i].setToZero();
-    }
 
-    // Initialize the map of body indexes in the velocity arrays
-    mMapBodyToConstrainedVelocityIndex.clear();
-    std::set<RigidBody*>::const_iterator it;
-    uint indexBody = 0;
-    for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
-
-        // Add the body into the map
-        mMapBodyToConstrainedVelocityIndex.insert(std::make_pair(*it, indexBody));
-        indexBody++;
+        (*it)->mArrayIndex = i++;
     }
 }
 
@@ -289,7 +281,7 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
         for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
 
             // Insert the body into the map of constrained velocities
-            uint indexBody = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
+            uint indexBody = bodies[b]->mArrayIndex;
 
             assert(mSplitLinearVelocities[indexBody] == Vector3(0, 0, 0));
             assert(mSplitAngularVelocities[indexBody] == Vector3(0, 0, 0));
@@ -358,20 +350,6 @@ void DynamicsWorld::solveContactsAndConstraints() {
 
         // Check if there are contacts and constraints to solve
         bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0;
-        //bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0;
-        //if (!isConstraintsToSolve && !isContactsToSolve) continue;
-
-        // If there are contacts in the current island
-//        if (isContactsToSolve) {
-
-//            // Initialize the solver
-//            mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]);
-
-//            // Warm start the contact solver
-//            if (mContactSolver.IsWarmStartingActive()) {
-//                mContactSolver.warmStart();
-//            }
-//        }
 
         // If there are constraints
         if (isConstraintsToSolve) {
@@ -393,15 +371,6 @@ void DynamicsWorld::solveContactsAndConstraints() {
         }
 
         mContactSolver.solve();
-
-        // Solve the contacts
-//            if (isContactsToSolve) {
-
-//                mContactSolver.resetTotalPenetrationImpulse();
-
-//                mContactSolver.solvePenetrationConstraints();
-//                mContactSolver.solveFrictionConstraints();
-//            }
     }
 
     mContactSolver.storeImpulses();

src/engine/DynamicsWorld.h

@@ -100,9 +100,6 @@ class DynamicsWorld : public CollisionWorld {
         /// Array of constrained rigid bodies orientation (for position error correction)
         Quaternion* mConstrainedOrientations;
 
-        /// Map body to their index in the constrained velocities array
-        std::map<RigidBody*, uint> mMapBodyToConstrainedVelocityIndex;
-
         /// Number of islands in the world
         uint mNbIslands;
 

src/engine/OverlappingPair.cpp

@@ -116,20 +116,16 @@ void OverlappingPair::addPotentialContactPoints(NarrowPhaseInfo* narrowPhaseInfo
 // Clear all the potential contact manifolds
 void OverlappingPair::clearPotentialContactManifolds() {
 
-    // Do we need to release memory
-    if (mTempMemoryAllocator.isReleaseNeeded()) {
+    ContactManifoldInfo* element = mPotentialContactManifolds;
+    while(element != nullptr) {
 
-        ContactManifoldInfo* element = mPotentialContactManifolds;
-        while(element != nullptr) {
+        // Remove the proxy collision shape
+        ContactManifoldInfo* elementToRemove = element;
+        element = element->getNext();
 
-            // Remove the proxy collision shape
-            ContactManifoldInfo* elementToRemove = element;
-            element = element->getNext();
-
-            // Delete the element
-            elementToRemove->~ContactManifoldInfo();
-            mTempMemoryAllocator.release(elementToRemove, sizeof(ContactManifoldInfo));
-        }
+        // Delete the element
+        elementToRemove->~ContactManifoldInfo();
+        mTempMemoryAllocator.release(elementToRemove, sizeof(ContactManifoldInfo));
     }
 
     mPotentialContactManifolds = nullptr;

src/mathematics/Matrix2x2.cpp

@@ -28,31 +28,6 @@
 
 using namespace reactphysics3d;
 
-// Constructor of the class Matrix2x2
-Matrix2x2::Matrix2x2() {
-
-    // Initialize all values in the matrix to zero
-    setAllValues(0.0, 0.0, 0.0, 0.0);
-}
-
-// Constructor
-Matrix2x2::Matrix2x2(decimal value) {
-    setAllValues(value, value, value, value);
-}
-
-// Constructor with arguments
-Matrix2x2::Matrix2x2(decimal a1, decimal a2, decimal b1, decimal b2) {
-
-    // Initialize the matrix with the values
-    setAllValues(a1, a2, b1, b2);
-}
-
-// Copy-constructor
-Matrix2x2::Matrix2x2(const Matrix2x2& matrix) {
-    setAllValues(matrix.mRows[0][0], matrix.mRows[0][1],
-                 matrix.mRows[1][0], matrix.mRows[1][1]);
-}
-
 // Assignment operator
 Matrix2x2& Matrix2x2::operator=(const Matrix2x2& matrix) {
 

src/mathematics/Matrix2x2.h

@@ -147,6 +147,31 @@ class Matrix2x2 {
         Vector2& operator[](int row);
 };
 
+// Constructor of the class Matrix2x2
+inline Matrix2x2::Matrix2x2() {
+
+    // Initialize all values in the matrix to zero
+    setAllValues(0.0, 0.0, 0.0, 0.0);
+}
+
+// Constructor
+inline Matrix2x2::Matrix2x2(decimal value) {
+    setAllValues(value, value, value, value);
+}
+
+// Constructor with arguments
+inline Matrix2x2::Matrix2x2(decimal a1, decimal a2, decimal b1, decimal b2) {
+
+    // Initialize the matrix with the values
+    setAllValues(a1, a2, b1, b2);
+}
+
+// Copy-constructor
+inline Matrix2x2::Matrix2x2(const Matrix2x2& matrix) {
+    setAllValues(matrix.mRows[0][0], matrix.mRows[0][1],
+                 matrix.mRows[1][0], matrix.mRows[1][1]);
+}
+
 // Method to set all the values in the matrix
 inline void Matrix2x2::setAllValues(decimal a1, decimal a2,
                                     decimal b1, decimal b2) {

src/mathematics/Matrix3x3.cpp

@@ -30,32 +30,6 @@
 // Namespaces
 using namespace reactphysics3d;
 
-// Constructor of the class Matrix3x3
-Matrix3x3::Matrix3x3() {
-    // Initialize all values in the matrix to zero
-    setAllValues(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
-}
-
-// Constructor
-Matrix3x3::Matrix3x3(decimal value) {
-    setAllValues(value, value, value, value, value, value, value, value, value);
-}
-
-// Constructor with arguments
-Matrix3x3::Matrix3x3(decimal a1, decimal a2, decimal a3,
-                     decimal b1, decimal b2, decimal b3,
-                     decimal c1, decimal c2, decimal c3) {
-    // Initialize the matrix with the values
-    setAllValues(a1, a2, a3, b1, b2, b3, c1, c2, c3);
-}
-
-// Copy-constructor
-Matrix3x3::Matrix3x3(const Matrix3x3& matrix) {
-    setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], matrix.mRows[0][2],
-                 matrix.mRows[1][0], matrix.mRows[1][1], matrix.mRows[1][2],
-                 matrix.mRows[2][0], matrix.mRows[2][1], matrix.mRows[2][2]);
-}
-
 // Assignment operator
 Matrix3x3& Matrix3x3::operator=(const Matrix3x3& matrix) {
 

src/mathematics/Matrix3x3.h

@@ -155,6 +155,32 @@ class Matrix3x3 {
         Vector3& operator[](int row);
 };
 
+// Constructor of the class Matrix3x3
+inline Matrix3x3::Matrix3x3() {
+    // Initialize all values in the matrix to zero
+    setAllValues(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+}
+
+// Constructor
+inline Matrix3x3::Matrix3x3(decimal value) {
+    setAllValues(value, value, value, value, value, value, value, value, value);
+}
+
+// Constructor with arguments
+inline Matrix3x3::Matrix3x3(decimal a1, decimal a2, decimal a3,
+                     decimal b1, decimal b2, decimal b3,
+                     decimal c1, decimal c2, decimal c3) {
+    // Initialize the matrix with the values
+    setAllValues(a1, a2, a3, b1, b2, b3, c1, c2, c3);
+}
+
+// Copy-constructor
+inline Matrix3x3::Matrix3x3(const Matrix3x3& matrix) {
+    setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], matrix.mRows[0][2],
+                 matrix.mRows[1][0], matrix.mRows[1][1], matrix.mRows[1][2],
+                 matrix.mRows[2][0], matrix.mRows[2][1], matrix.mRows[2][2]);
+}
+
 // Method to set all the values in the matrix
 inline void Matrix3x3::setAllValues(decimal a1, decimal a2, decimal a3,
                                     decimal b1, decimal b2, decimal b3,

src/mathematics/Quaternion.cpp

@@ -31,27 +31,6 @@
 // Namespace
 using namespace reactphysics3d;
 
-// Constructor of the class
-Quaternion::Quaternion() : x(0.0), y(0.0), z(0.0), w(0.0) {
-
-}
-
-// Constructor with arguments
-Quaternion::Quaternion(decimal newX, decimal newY, decimal newZ, decimal newW)
-           :x(newX), y(newY), z(newZ), w(newW) {
-
-}
-
-// Constructor with the component w and the vector v=(x y z)
-Quaternion::Quaternion(decimal newW, const Vector3& v) : x(v.x), y(v.y), z(v.z), w(newW) {
-
-}
-
-// Constructor with the component w and the vector v=(x y z)
-Quaternion::Quaternion(const Vector3& v, decimal newW) : x(v.x), y(v.y), z(v.z), w(newW) {
-
-}
-
 // Return a quaternion constructed from Euler angles (in radians)
 Quaternion Quaternion::fromEulerAngles(decimal angleX, decimal angleY, decimal angleZ) {
 

src/mathematics/Quaternion.h

@@ -169,7 +169,28 @@ struct Quaternion {
         void initWithEulerAngles(decimal angleX, decimal angleY, decimal angleZ);
 };
 
-/// Set all the values
+// Constructor of the class
+inline Quaternion::Quaternion() : x(0.0), y(0.0), z(0.0), w(0.0) {
+
+}
+
+// Constructor with arguments
+inline Quaternion::Quaternion(decimal newX, decimal newY, decimal newZ, decimal newW)
+           :x(newX), y(newY), z(newZ), w(newW) {
+
+}
+
+// Constructor with the component w and the vector v=(x y z)
+inline Quaternion::Quaternion(decimal newW, const Vector3& v) : x(v.x), y(v.y), z(v.z), w(newW) {
+
+}
+
+// Constructor with the component w and the vector v=(x y z)
+inline Quaternion::Quaternion(const Vector3& v, decimal newW) : x(v.x), y(v.y), z(v.z), w(newW) {
+
+}
+
+// Set all the values
 inline void Quaternion::setAllValues(decimal newX, decimal newY, decimal newZ, decimal newW) {
     x = newX;
     y = newY;
@@ -177,7 +198,7 @@ inline void Quaternion::setAllValues(decimal newX, decimal newY, decimal newZ, d
     w = newW;
 }
 
-/// Set the quaternion to zero
+// Set the quaternion to zero
 inline void Quaternion::setToZero() {
     x = 0;
     y = 0;
@@ -306,16 +327,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() +
-                      getVectorV().cross(quaternion.getVectorV()));
+                          w * quaternion.getVectorV() + quaternion.w * 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.cpp

@@ -29,25 +29,3 @@
 // Namespaces
 using namespace reactphysics3d;
 
-// Constructor
-Transform::Transform() : mPosition(Vector3(0.0, 0.0, 0.0)), mOrientation(Quaternion::identity()) {
-
-}
-
-// Constructor
-Transform::Transform(const Vector3& position, const Matrix3x3& orientation)
-          : mPosition(position), mOrientation(Quaternion(orientation)) {
-
-}
-
-// Constructor
-Transform::Transform(const Vector3& position, const Quaternion& orientation)
-          : mPosition(position), mOrientation(orientation) {
-
-}
-
-// Copy-constructor
-Transform::Transform(const Transform& transform)
-          : mPosition(transform.mPosition), mOrientation(transform.mOrientation) {
-
-}

src/mathematics/Transform.h

@@ -118,6 +118,29 @@ class Transform {
         Transform& operator=(const Transform& transform);
 };
 
+// Constructor
+inline Transform::Transform() : mPosition(Vector3(0.0, 0.0, 0.0)), mOrientation(Quaternion::identity()) {
+
+}
+
+// Constructor
+inline Transform::Transform(const Vector3& position, const Matrix3x3& orientation)
+          : mPosition(position), mOrientation(Quaternion(orientation)) {
+
+}
+
+// Constructor
+inline Transform::Transform(const Vector3& position, const Quaternion& orientation)
+          : mPosition(position), mOrientation(orientation) {
+
+}
+
+// Copy-constructor
+inline Transform::Transform(const Transform& transform)
+          : mPosition(transform.mPosition), mOrientation(transform.mOrientation) {
+
+}
+
 // Return the position of the transform
 inline const Vector3& Transform::getPosition() const {
     return mPosition;
@@ -194,13 +217,36 @@ 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
 inline Transform Transform::operator*(const Transform& transform2) const {
-    return Transform(mPosition + mOrientation * transform2.mPosition,
-                     mOrientation * transform2.mOrientation);
+
+    // The following code is equivalent to this
+    //return Transform(mPosition + mOrientation * transform2.mPosition,
+    //                 mOrientation * transform2.mOrientation);
+
+    const decimal prodX = mOrientation.w * transform2.mPosition.x + mOrientation.y * transform2.mPosition.z
+                          - mOrientation.z * transform2.mPosition.y;
+    const decimal prodY = mOrientation.w * transform2.mPosition.y + mOrientation.z * transform2.mPosition.x
+                          - mOrientation.x * transform2.mPosition.z;
+    const decimal prodZ = mOrientation.w * transform2.mPosition.z + mOrientation.x * transform2.mPosition.y
+                          - mOrientation.y * transform2.mPosition.x;
+    const decimal prodW = -mOrientation.x * transform2.mPosition.x - mOrientation.y * transform2.mPosition.y
+                          - mOrientation.z * transform2.mPosition.z;
+
+    return Transform(Vector3(mPosition.x + mOrientation.w * prodX - prodY * mOrientation.z + prodZ * mOrientation.y - prodW * mOrientation.x,
+                             mPosition.y + mOrientation.w * prodY - prodZ * mOrientation.x + prodX * mOrientation.z - prodW * mOrientation.y,
+                             mPosition.z + mOrientation.w * prodZ - prodX * mOrientation.y + prodY * mOrientation.x - prodW * mOrientation.z),
+                     Quaternion(mOrientation.w * transform2.mOrientation.x + transform2.mOrientation.w * mOrientation.x
+                       + mOrientation.y * transform2.mOrientation.z - mOrientation.z * transform2.mOrientation.y,
+                      mOrientation.w * transform2.mOrientation.y + transform2.mOrientation.w * mOrientation.y
+                       + mOrientation.z * transform2.mOrientation.x - mOrientation.x * transform2.mOrientation.z,
+                      mOrientation.w * transform2.mOrientation.z + transform2.mOrientation.w * mOrientation.z
+                       + mOrientation.x * transform2.mOrientation.y - mOrientation.y * transform2.mOrientation.x,
+                      mOrientation.w * transform2.mOrientation.w - mOrientation.x * transform2.mOrientation.x
+                       - mOrientation.y * transform2.mOrientation.y - mOrientation.z * transform2.mOrientation.z));
 }
 
 // Return true if the two transforms are equal

src/mathematics/Vector2.cpp

@@ -30,21 +30,6 @@
 // Namespaces
 using namespace reactphysics3d;
 
-// Constructor
-Vector2::Vector2() : x(0.0), y(0.0) {
-
-}
-
-// Constructor with arguments
-Vector2::Vector2(decimal newX, decimal newY) : x(newX), y(newY) {
-
-}
-
-// Copy-constructor
-Vector2::Vector2(const Vector2& vector) : x(vector.x), y(vector.y) {
-
-}
-
 // Return the corresponding unit vector
 Vector2 Vector2::getUnit() const {
     decimal lengthVector = length();

src/mathematics/Vector2.h

@@ -156,6 +156,22 @@ struct Vector2 {
         friend Vector2 operator/(const Vector2& vector1, const Vector2& vector2);
 };
 
+// Constructor
+inline Vector2::Vector2() : x(0.0), y(0.0) {
+
+}
+
+// Constructor with arguments
+inline Vector2::Vector2(decimal newX, decimal newY) : x(newX), y(newY) {
+
+}
+
+// Copy-constructor
+inline Vector2::Vector2(const Vector2& vector) : x(vector.x), y(vector.y) {
+
+}
+
+
 // Set the vector to zero
 inline void Vector2::setToZero() {
     x = 0;

src/mathematics/Vector3.cpp

@@ -31,21 +31,6 @@
 // Namespaces
 using namespace reactphysics3d;
 
-// Constructor of the class Vector3D
-Vector3::Vector3() : x(0.0), y(0.0), z(0.0) {
-
-}
-
-// Constructor with arguments
-Vector3::Vector3(decimal newX, decimal newY, decimal newZ) : x(newX), y(newY), z(newZ) {
-
-}
-
-// Copy-constructor
-Vector3::Vector3(const Vector3& vector) : x(vector.x), y(vector.y), z(vector.z) {
-
-}
-
 // Return the corresponding unit vector
 Vector3 Vector3::getUnit() const {
     decimal lengthVector = length();

src/mathematics/Vector3.h

@@ -168,6 +168,21 @@ struct Vector3 {
         friend Vector3 operator/(const Vector3& vector1, const Vector3& vector2);
 };
 
+// Constructor of the class Vector3D
+inline Vector3::Vector3() : x(0.0), y(0.0), z(0.0) {
+
+}
+
+// Constructor with arguments
+inline Vector3::Vector3(decimal newX, decimal newY, decimal newZ) : x(newX), y(newY), z(newZ) {
+
+}
+
+// Copy-constructor
+inline Vector3::Vector3(const Vector3& vector) : x(vector.x), y(vector.y), z(vector.z) {
+
+}
+
 // Set the vector to zero
 inline void Vector3::setToZero() {
     x = 0;

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
-	const Vector3 ab = segB - segA;
-
-    decimal nDotAB = planeNormal.dot(ab);
+    decimal nDotAB = planeNormal.dot(segB - segA);
 
 	// If the segment is not parallel to the plane
     if (std::abs(nDotAB) > parallelEpsilon) {
@@ -225,27 +222,34 @@ decimal reactphysics3d::computePointToLineDistance(const Vector3& linePointA, co
 
 // Clip a segment against multiple planes and return the clipped segment vertices
 // This method implements the Sutherland–Hodgman clipping algorithm
-std::vector<Vector3> reactphysics3d::clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB,
-                                                           const std::vector<Vector3>& planesPoints,
-                                                           const std::vector<Vector3>& planesNormals) {
+List<Vector3> reactphysics3d::clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB,
+                                                           const List<Vector3>& planesPoints,
+                                                           const List<Vector3>& planesNormals,
+                                                           Allocator& allocator) {
 
     assert(planesPoints.size() == planesNormals.size());
 
-    std::vector<Vector3> inputVertices = {segA, segB};
-    std::vector<Vector3> outputVertices;
+    List<Vector3> list1(allocator, 2);
+    List<Vector3> list2(allocator, 2);
+
+    List<Vector3>* inputVertices = &list1;
+    List<Vector3>* outputVertices = &list2;
+
+    inputVertices->add(segA);
+    inputVertices->add(segB);
 
     // For each clipping plane
     for (uint p=0; p<planesPoints.size(); p++) {
 
         // If there is no more vertices, stop
-        if (inputVertices.empty()) return inputVertices;
+        if (inputVertices->size() == 0) return *inputVertices;
 
-        assert(inputVertices.size() == 2);
+        assert(inputVertices->size() == 2);
 
-        outputVertices.clear();
+        outputVertices->clear();
 
-        Vector3& v1 = inputVertices[0];
-        Vector3& v2 = inputVertices[1];
+        Vector3& v1 = (*inputVertices)[0];
+        Vector3& v2 = (*inputVertices)[1];
 
         decimal v1DotN = (v1 - planesPoints[p]).dot(planesNormals[p]);
         decimal v2DotN = (v2 - planesPoints[p]).dot(planesNormals[p]);
@@ -260,63 +264,69 @@ std::vector<Vector3> reactphysics3d::clipSegmentWithPlanes(const Vector3& segA,
                 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);
                 }
             }
             else {
-                outputVertices.push_back(v1);
+                outputVertices->add(v1);
             }
 
             // Add the second vertex
-            outputVertices.push_back(v2);
+            outputVertices->add(v2);
         }
         else {  // If the second vertex is behind the clipping plane
 
             // If the first vertex is in front of the clippling plane
             if (v1DotN >= decimal(0.0)) {
 
-                outputVertices.push_back(v1);
+                outputVertices->add(v1);
 
                 // The first point we keep is the intersection between the segment v1, v2 and the clipping plane
                 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));
                 }
             }
         }
 
         inputVertices = outputVertices;
+        outputVertices = p % 2 == 0 ? &list1 : &list2;
     }
 
-    return outputVertices;
+    return *outputVertices;
 }
 
 // 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,
-                                                           const std::vector<Vector3>& planesNormals) {
+List<Vector3> reactphysics3d::clipPolygonWithPlanes(const List<Vector3>& polygonVertices, const List<Vector3>& planesPoints,
+                                                    const List<Vector3>& planesNormals, Allocator& allocator) {
 
     assert(planesPoints.size() == planesNormals.size());
 
-    std::vector<Vector3> inputVertices(polygonVertices);
-    std::vector<Vector3> outputVertices;
+    uint nbMaxElements = polygonVertices.size() + planesPoints.size();
+    List<Vector3> list1(allocator, nbMaxElements);
+    List<Vector3> list2(allocator, nbMaxElements);
+
+    const List<Vector3>* inputVertices = &polygonVertices;
+    List<Vector3>* outputVertices = &list2;
 
     // For each clipping plane
     for (uint p=0; p<planesPoints.size(); p++) {
 
-        outputVertices.clear();
+        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];
+            const Vector3& v1 = (*inputVertices)[vStart];
+            const Vector3& v2 = (*inputVertices)[vEnd];
 
             decimal v1DotN = (v1 - planesPoints[p]).dot(planesNormals[p]);
             decimal v2DotN = (v2 - planesPoints[p]).dot(planesNormals[p]);
@@ -331,15 +341,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 +360,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);
                     }
                 }
             }
@@ -362,9 +372,10 @@ std::vector<Vector3> reactphysics3d::clipPolygonWithPlanes(const std::vector<Vec
         }
 
         inputVertices = outputVertices;
+        outputVertices = p % 2 == 0 ? &list1 : &list2;
     }
 
-    return outputVertices;
+    return *outputVertices;
 }
 
 // Project a point onto a plane that is given by a point and its unit length normal

src/mathematics/mathematics_functions.h

@@ -33,6 +33,7 @@
 #include <cassert>
 #include <cmath>
 #include <vector>
+#include "containers/List.h"
 
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
@@ -111,13 +112,14 @@ decimal computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB
 decimal computePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point);
 
 /// Clip a segment against multiple planes and return the clipped segment vertices
-std::vector<Vector3> clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB,
-                                                           const std::vector<Vector3>& planesPoints,
-                                                           const std::vector<Vector3>& planesNormals);
+List<Vector3> clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB,
+                                                           const List<Vector3>& planesPoints,
+                                                           const List<Vector3>& planesNormals,
+                                                           Allocator& allocator);
 
 /// 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,
-                                           const std::vector<Vector3>& planesNormals);
+List<Vector3> clipPolygonWithPlanes(const List<Vector3>& polygonVertices, const List<Vector3>& planesPoints,
+                                    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);

src/memory/DefaultAllocator.h

@@ -0,0 +1,66 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_DEFAULT_ALLOCATOR_H
+#define REACTPHYSICS3D_DEFAULT_ALLOCATOR_H
+
+// Libraries
+#include "memory/Allocator.h"
+#include <cstdlib>
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Class DefaultAllocator
+/**
+ * This class represents a default memory allocator that uses default malloc/free methods
+ */
+class DefaultAllocator : public Allocator {
+
+    public:
+
+        /// Destructor
+        virtual ~DefaultAllocator() = default;
+
+        /// Allocate memory of a given size (in bytes) and return a pointer to the
+        /// allocated memory.
+        virtual void* allocate(size_t size) override {
+            return malloc(size);
+        }
+
+        /// Release previously allocated memory.
+        virtual void release(void* pointer, size_t size) override {
+            free(pointer);
+        }
+
+        /// Return true if memory needs to be release with this allocator
+        virtual bool isReleaseNeeded() const override {
+            return true;
+        }
+};
+
+}
+
+#endif

src/memory/MemoryManager.cpp

@@ -0,0 +1,32 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2015 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "MemoryManager.h"
+
+using namespace reactphysics3d;
+
+// Static variables
+DefaultAllocator MemoryManager::mDefaultAllocator;

src/memory/MemoryManager.h

@@ -0,0 +1,74 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_MEMORY_MANAGER_H
+#define REACTPHYSICS3D_MEMORY_MANAGER_H
+
+// Libraries
+#include "memory/DefaultAllocator.h"
+
+/// Namespace ReactPhysics3D
+namespace reactphysics3d {
+
+// Class MemoryManager
+/**
+ * The memory manager is used to store the different memory allocators that are used
+ * by the library.
+ */
+class MemoryManager {
+
+    private:
+
+       /// Default memory allocator
+       static DefaultAllocator mDefaultAllocator;
+
+    public:
+
+        /// Memory allocation types
+       enum class AllocationType {
+           Default, // Default memory allocator
+           Pool,	// Memory pool allocator
+           Frame,   // Single frame memory allocator
+       };
+
+        /// Constructor
+        MemoryManager();
+
+        /// Destructor
+        ~MemoryManager();
+
+        /// Return the default memory allocator
+        static DefaultAllocator& getDefaultAllocator();
+};
+
+// Return the default memory allocator
+inline DefaultAllocator& MemoryManager::getDefaultAllocator() {
+    return mDefaultAllocator;
+}
+
+}
+
+#endif
+

src/memory/PoolAllocator.cpp

@@ -126,7 +126,7 @@ void* PoolAllocator::allocate(size_t size) {
     }
     else {  // If there is no more free memory units in the corresponding heap
 
-        // If we need to allocate more memory to containsthe blocks
+        // If we need to allocate more memory to contains the blocks
         if (mNbCurrentMemoryBlocks == mNbAllocatedMemoryBlocks) {
 
             // Allocate more memory to contain the blocks

src/memory/PoolAllocator.h

@@ -94,7 +94,7 @@ class PoolAllocator : public Allocator {
         /// Size of the memory units that each heap is responsible to allocate
         static size_t mUnitSizes[NB_HEAPS];
 
-        /// Lookup table that mape size to allocate to the index of the
+        /// Lookup table that map the size to allocate to the index of the
         /// corresponding heap we will use for the allocation.
         static int mMapSizeToHeapIndex[MAX_UNIT_SIZE + 1];
 

test/tests/collision/TestCollisionWorld.h

@@ -40,21 +40,108 @@ enum CollisionCategory {
     CATEGORY_3 = 0x0004
 };
 
-// TODO : Add test for concave shape collision here
+// Contact point collision data
+struct CollisionPointData {
+
+	Vector3 localPointBody1;
+	Vector3 localPointBody2;
+	decimal penetrationDepth;
+
+	CollisionPointData(const Vector3& point1, const Vector3& point2, decimal penDepth) {
+		localPointBody1 = point1;
+		localPointBody2 = point2;
+		penetrationDepth = penDepth;
+	}
+
+	bool isContactPointSimilarTo(const Vector3& pointBody1, const Vector3& pointBody2, decimal penDepth, decimal epsilon = 0.001) {
+
+		return approxEqual(pointBody1, localPointBody1, epsilon) &&
+			   approxEqual(pointBody2, localPointBody2, epsilon) &&
+			   approxEqual(penetrationDepth, penDepth, epsilon);
+	}
+};
+
+// Contact manifold collision data
+struct CollisionManifoldData {
+
+	std::vector<CollisionPointData> contactPoints;
+
+	int getNbContactPoints() const {
+		return contactPoints.size();
+	}
+
+	bool hasContactPointSimilarTo(const Vector3& localPointBody1, const Vector3& localPointBody2, decimal penetrationDepth, decimal epsilon = 0.001) {
+
+		std::vector<CollisionPointData>::iterator it;
+		for (it = contactPoints.begin(); it != contactPoints.end(); ++it) {
+
+			if (it->isContactPointSimilarTo(localPointBody1, localPointBody2, penetrationDepth)) {
+				return true;
+			}
+		}
+
+		return false;
+	}
+
+};
+
+// Collision data between two proxy shapes
+struct CollisionData {
+
+	std::pair<const ProxyShape*, const ProxyShape*> proxyShapes;
+	std::pair<CollisionBody*, CollisionBody*> bodies;
+	std::vector<CollisionManifoldData> contactManifolds;
+
+	int getNbContactManifolds() const {
+		return contactManifolds.size();
+	}
+
+	int getTotalNbContactPoints() const {
+		
+		int nbPoints = 0;
+
+		std::vector<CollisionManifoldData>::const_iterator it;
+		for (it = contactManifolds.begin(); it != contactManifolds.end(); ++it) {
+
+			nbPoints += it->getNbContactPoints();
+		}
+
+		return nbPoints;
+	}
+
+	bool hasContactPointSimilarTo(const Vector3& localPointBody1, const Vector3& localPointBody2, decimal penetrationDepth, decimal epsilon = 0.001) {
+
+		std::vector<CollisionManifoldData>::iterator it;
+		for (it = contactManifolds.begin(); it != contactManifolds.end(); ++it) {
+
+			if (it->hasContactPointSimilarTo(localPointBody1, localPointBody2, penetrationDepth)) {
+				return true;
+			}
+		}
+
+		return false;
+	}
+
+};
 
 // Class
 class WorldCollisionCallback : public CollisionCallback
 {
+	private:
+
+		std::vector<std::pair<const ProxyShape*, const ProxyShape*>> mCollisionData;
+
+		std::pair<const ProxyShape*, const ProxyShape*> getCollisionKeyPair(std::pair<const ProxyShape*, const ProxyShape*> pair) const {
+			
+			if (pair.first > pair.second) {
+				return std::make_pair(pair.second, pair.first);
+			}
+
+			return pair;
+		}
+
     public:
 
-        bool boxCollideWithSphere1;
-        bool sphere1CollideWithSphere2;
-
-        CollisionBody* boxBody;
-        CollisionBody* sphere1Body;
-        CollisionBody* sphere2Body;
-        CollisionBody* cylinderBody;
-
         WorldCollisionCallback()
         {
             reset();
@@ -62,30 +149,79 @@ class WorldCollisionCallback : public CollisionCallback
 
         void reset()
         {
-            boxCollideWithSphere1 = false;
-            sphere1CollideWithSphere2 = false;
+			mCollisionData.clear();
         }
 
-        // This method will be called for contact
+		bool hasContacts() const {
+			return mCollisionData.size() > 0;
+		}
+
+		bool areProxyShapesColliding(const ProxyShape* proxyShape1, const ProxyShape* proxyShape2) {
+			return std::find(mCollisionData.begin(), mCollisionData.end(), getCollisionKeyPair(std::make_pair(proxyShape1, proxyShape2))) != mCollisionData.end();
+		}
+
+        // This method will be called for each contact
         virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo) override {
 
-            if (isContactBetweenBodies(boxBody, sphere1Body, collisionCallbackInfo)) {
-                boxCollideWithSphere1 = true;
-            }
-            else if (isContactBetweenBodies(sphere1Body, sphere2Body, collisionCallbackInfo)) {
-                sphere1CollideWithSphere2 = true;
-            }
-        }
+			CollisionData collisionData;
+			collisionData.bodies = std::make_pair(collisionCallbackInfo.body1, collisionCallbackInfo.body2);
+			collisionData.proxyShapes = std::make_pair(collisionCallbackInfo.proxyShape1, collisionCallbackInfo.proxyShape2);
 
-        bool isContactBetweenBodies(const CollisionBody* body1, const CollisionBody* body2,
-                                    const CollisionCallbackInfo& collisionCallbackInfo) {
-            return (collisionCallbackInfo.body1->getID() == body1->getID() &&
-                    collisionCallbackInfo.body2->getID() == body2->getID()) ||
-                   (collisionCallbackInfo.body2->getID() == body1->getID() &&
-                    collisionCallbackInfo.body1->getID() == body2->getID());
+			ContactManifoldListElement* element = collisionCallbackInfo.contactManifoldElements;
+			while (element != nullptr) {
+
+				ContactManifold* contactManifold = element->getContactManifold();
+
+				CollisionManifoldData collisionManifold;
+
+				ContactPoint* contactPoint = contactManifold->getContactPoints();
+				while (contactPoint != nullptr) {
+
+                    CollisionPointData collisionPoint(contactPoint->getLocalPointOnShape1(), contactPoint->getLocalPointOnShape2(), contactPoint->getPenetrationDepth());
+					collisionManifold.contactPoints.push_back(collisionPoint);
+
+					contactPoint = contactPoint->getNext();
+				}
+
+				collisionData.contactManifolds.push_back(collisionManifold);
+
+				element = element->getNext();
+			}
         }
 };
 
+/// Overlap callback
+class WorldOverlapCallback : public OverlapCallback {
+
+	private:
+
+		CollisionBody* mOverlapBody;
+
+	public:
+
+		/// Destructor
+		virtual ~WorldOverlapCallback() {
+			reset();
+		}
+
+		/// This method will be called for each reported overlapping bodies
+		virtual void notifyOverlap(CollisionBody* collisionBody) override {
+
+		}
+
+		void reset() {
+			mOverlapBody = nullptr;
+		}
+
+		bool hasOverlap() const {
+			return mOverlapBody != nullptr;
+		}
+
+		CollisionBody* getOverlapBody() {
+			return mOverlapBody;
+		}
+};
+
 // Class TestCollisionWorld
 /**
  * Unit test for the CollisionWorld class.
@@ -100,22 +236,29 @@ class TestCollisionWorld : public Test {
         CollisionWorld* mWorld;
 
         // Bodies
-        CollisionBody* mBoxBody;
-        CollisionBody* mSphere1Body;
-        CollisionBody* mSphere2Body;
+        CollisionBody* mBoxBody1;
+		CollisionBody* mBoxBody2;
+        CollisionBody* mSphereBody1;
+        CollisionBody* mSphereBody2;
 
         // Collision shapes
-        BoxShape* mBoxShape;
-        SphereShape* mSphereShape;
+        BoxShape* mBoxShape1;
+		BoxShape* mBoxShape2;
+        SphereShape* mSphereShape1;
+		SphereShape* mSphereShape2;
 
         // Proxy shapes
-        ProxyShape* mBoxProxyShape;
-        ProxyShape* mSphere1ProxyShape;
-        ProxyShape* mSphere2ProxyShape;
+        ProxyShape* mBoxProxyShape1;
+		ProxyShape* mBoxProxyShape2;
+        ProxyShape* mSphereProxyShape1;
+        ProxyShape* mSphereProxyShape2;
 
-        // Collision callback class
+        // Collision callback
         WorldCollisionCallback mCollisionCallback;
 
+		// Overlap callback
+		WorldOverlapCallback mOverlapCallback;
+
     public :
 
         // ---------- Methods ---------- //
@@ -123,147 +266,243 @@ class TestCollisionWorld : public Test {
         /// Constructor
         TestCollisionWorld(const std::string& name) : Test(name) {
 
-            // Create the world
+            // Create the collision world
             mWorld = new CollisionWorld();
 
-            // Create the bodies
-            Transform boxTransform(Vector3(10, 0, 0), Quaternion::identity());
-            mBoxBody = mWorld->createCollisionBody(boxTransform);
-            mBoxShape = new BoxShape(Vector3(3, 3, 3));
-            mBoxProxyShape = mBoxBody->addCollisionShape(mBoxShape, Transform::identity());
+            // ---------- Boxes ---------- //
+            Transform boxTransform1(Vector3(-20, 20, 0), Quaternion::identity());
+            mBoxBody1 = mWorld->createCollisionBody(boxTransform1);
+            mBoxShape1 = new BoxShape(Vector3(3, 3, 3));
+            mBoxProxyShape1 = mBoxBody1->addCollisionShape(mBoxShape1, Transform::identity());
 
-            mSphereShape = new SphereShape(3.0);
-            Transform sphere1Transform(Vector3(10,5, 0), Quaternion::identity());
-            mSphere1Body = mWorld->createCollisionBody(sphere1Transform);
-            mSphere1ProxyShape = mSphere1Body->addCollisionShape(mSphereShape, Transform::identity());
+			Transform boxTransform2(Vector3(-10, 20, 0), Quaternion::identity());
+			mBoxBody2 = mWorld->createCollisionBody(boxTransform2);
+			mBoxShape2 = new BoxShape(Vector3(2, 2, 2));
+			mBoxProxyShape2 = mBoxBody2->addCollisionShape(mBoxShape1, Transform::identity());
 
-            Transform sphere2Transform(Vector3(30, 10, 10), Quaternion::identity());
-            mSphere2Body = mWorld->createCollisionBody(sphere2Transform);
-            mSphere2ProxyShape = mSphere2Body->addCollisionShape(mSphereShape, Transform::identity());
+			// ---------- Spheres ---------- //
+            mSphereShape1 = new SphereShape(3.0);
+            Transform sphereTransform1(Vector3(10, 20, 0), Quaternion::identity());
+            mSphereBody1 = mWorld->createCollisionBody(sphereTransform1);
+            mSphereProxyShape1 = mSphereBody1->addCollisionShape(mSphereShape1, Transform::identity());
 
-            // Assign collision categories to proxy shapes
-            mBoxProxyShape->setCollisionCategoryBits(CATEGORY_1);
-            mSphere1ProxyShape->setCollisionCategoryBits(CATEGORY_1);
-            mSphere2ProxyShape->setCollisionCategoryBits(CATEGORY_2);
-
-            mCollisionCallback.boxBody = mBoxBody;
-            mCollisionCallback.sphere1Body = mSphere1Body;
-            mCollisionCallback.sphere2Body = mSphere2Body;
+			mSphereShape2 = new SphereShape(5.0);
+			Transform sphereTransform2(Vector3(20, 20, 0), Quaternion::identity());
+			mSphereBody2 = mWorld->createCollisionBody(sphereTransform2);
+			mSphereProxyShape2 = mSphereBody2->addCollisionShape(mSphereShape2, Transform::identity());
         }
 
         /// Destructor
         virtual ~TestCollisionWorld() {
-            delete mBoxShape;
-            delete mSphereShape;
+
+            delete mBoxShape1;
+            delete mBoxShape2;
+
+			delete mSphereShape1;
+			delete mSphereShape2;
+
+			delete mWorld;
         }
 
         /// Run the tests
         void run() {
 
-            testCollisions();
+			testNoCollisions();
+			testNoOverlap();
+			testNoAABBOverlap();
+
+			testAABBOverlap();
+
+			testSphereVsSphereCollision();
+			testSphereVsBoxCollision();
+
+			testMultipleCollisions();
         }
 
-        void testCollisions() {
+		void testNoCollisions() {
 
-            mCollisionCallback.reset();
-            mWorld->testCollision(&mCollisionCallback);
-            test(mCollisionCallback.boxCollideWithSphere1);
-            test(!mCollisionCallback.sphere1CollideWithSphere2);
+			// All the shapes of the world are not touching when they are created.
+			// Here we test that at the beginning, there is no collision at all.
 
-            test(mWorld->testAABBOverlap(mBoxBody, mSphere1Body));
-            test(!mWorld->testAABBOverlap(mSphere1Body, mSphere2Body));
+			// ---------- Global test ---------- //
 
-            test(mBoxProxyShape->testAABBOverlap(mSphere1ProxyShape->getWorldAABB()));
-            test(!mSphere1ProxyShape->testAABBOverlap(mSphere2ProxyShape->getWorldAABB()));
+			mCollisionCallback.reset();
+			mWorld->testCollision(&mCollisionCallback);
+			test(!mCollisionCallback.hasContacts());
 
-            mCollisionCallback.reset();
-            test(!mCollisionCallback.boxCollideWithSphere1);
-            test(!mCollisionCallback.sphere1CollideWithSphere2);
+			// ---------- Single body test ---------- //
 
-            mCollisionCallback.reset();
-            mWorld->testCollision(mBoxBody, mSphere1Body, &mCollisionCallback);
-            test(mCollisionCallback.boxCollideWithSphere1);
-            test(!mCollisionCallback.sphere1CollideWithSphere2);
+			mCollisionCallback.reset();
+			mWorld->testCollision(mBoxBody1, &mCollisionCallback);
+			test(!mCollisionCallback.hasContacts());
 
-            mCollisionCallback.reset();
-            test(!mCollisionCallback.boxCollideWithSphere1);
-            test(!mCollisionCallback.sphere1CollideWithSphere2);
+			mCollisionCallback.reset();
+			mWorld->testCollision(mBoxBody2, &mCollisionCallback);
+			test(!mCollisionCallback.hasContacts());
 
-            // Move sphere 1 to collide with sphere 2
-            mSphere1Body->setTransform(Transform(Vector3(30, 15, 10), Quaternion::identity()));
+			mCollisionCallback.reset();
+			mWorld->testCollision(mSphereBody1, &mCollisionCallback);
+			test(!mCollisionCallback.hasContacts());
 
-            mCollisionCallback.reset();
-            mWorld->testCollision(&mCollisionCallback);
-            test(!mCollisionCallback.boxCollideWithSphere1);
-            test(mCollisionCallback.sphere1CollideWithSphere2);
+			mCollisionCallback.reset();
+			mWorld->testCollision(mSphereBody2, &mCollisionCallback);
+			test(!mCollisionCallback.hasContacts());
 
-            mCollisionCallback.reset();
-            mWorld->testCollision(mBoxBody, mSphere1Body, &mCollisionCallback);
-            test(!mCollisionCallback.boxCollideWithSphere1);
-            test(!mCollisionCallback.sphere1CollideWithSphere2);
+			// Two bodies test
 
-            mCollisionCallback.reset();
-            test(!mCollisionCallback.boxCollideWithSphere1);
-            test(!mCollisionCallback.sphere1CollideWithSphere2);
+			mCollisionCallback.reset();
+			mWorld->testCollision(mBoxBody1, mBoxBody2, &mCollisionCallback);
+			test(!mCollisionCallback.hasContacts());
+
+			mCollisionCallback.reset();
+			mWorld->testCollision(mSphereBody1, mSphereBody2, &mCollisionCallback);
+			test(!mCollisionCallback.hasContacts());
+
+			mCollisionCallback.reset();
+			mWorld->testCollision(mBoxBody1, mSphereBody1, &mCollisionCallback);
+			test(!mCollisionCallback.hasContacts());
+
+			mCollisionCallback.reset();
+			mWorld->testCollision(mBoxBody1, mSphereBody2, &mCollisionCallback);
+			test(!mCollisionCallback.hasContacts());
+
+			mCollisionCallback.reset();
+			mWorld->testCollision(mBoxBody2, mSphereBody1, &mCollisionCallback);
+			test(!mCollisionCallback.hasContacts());
+
+			mCollisionCallback.reset();
+			mWorld->testCollision(mBoxBody2, mSphereBody2, &mCollisionCallback);
+			test(!mCollisionCallback.hasContacts());
+
+		}
+
+		void testNoOverlap() {
+
+			// All the shapes of the world are not touching when they are created.
+			// Here we test that at the beginning, there is no overlap at all.
+
+			// ---------- Single body test ---------- //
+
+			mOverlapCallback.reset();
+			mWorld->testOverlap(mBoxBody1, &mOverlapCallback);
+			test(!mOverlapCallback.hasOverlap());
+
+			mOverlapCallback.reset();
+			mWorld->testOverlap(mBoxBody2, &mOverlapCallback);
+			test(!mOverlapCallback.hasOverlap());
+
+			mOverlapCallback.reset();
+			mWorld->testOverlap(mSphereBody1, &mOverlapCallback);
+			test(!mOverlapCallback.hasOverlap());
+
+			mOverlapCallback.reset();
+			mWorld->testOverlap(mSphereBody2, &mOverlapCallback);
+			test(!mOverlapCallback.hasOverlap());
+
+			// Two bodies test
+
+			test(!mWorld->testOverlap(mBoxBody1, mBoxBody2));
+			test(!mWorld->testOverlap(mSphereBody1, mSphereBody2));
+			test(!mWorld->testOverlap(mBoxBody1, mSphereBody1));
+			test(!mWorld->testOverlap(mBoxBody1, mSphereBody2));
+			test(!mWorld->testOverlap(mBoxBody2, mSphereBody1));
+			test(!mWorld->testOverlap(mBoxBody2, mSphereBody2));
+		}
+
+		void testNoAABBOverlap() {
+
+			// All the shapes of the world are not touching when they are created.
+			// Here we test that at the beginning, there is no AABB overlap at all.
+
+			// Two bodies test
+
+			test(!mWorld->testAABBOverlap(mBoxBody1, mBoxBody2));
+			test(!mWorld->testAABBOverlap(mSphereBody1, mSphereBody2));
+			test(!mWorld->testAABBOverlap(mBoxBody1, mSphereBody1));
+			test(!mWorld->testAABBOverlap(mBoxBody1, mSphereBody2));
+			test(!mWorld->testAABBOverlap(mBoxBody2, mSphereBody1));
+			test(!mWorld->testAABBOverlap(mBoxBody2, mSphereBody2));
+		}
+
+		void testAABBOverlap() {
+
+			// TODO : Test the CollisionWorld::testAABBOverlap() method here
+		}
+
+		void testSphereVsSphereCollision() {
+
+
+
+			// Move sphere 1 to collide with sphere 2
+			mSphereBody1->setTransform(Transform(Vector3(30, 15, 10), Quaternion::identity()));
+
+		}
+
+        void testSphereVsBoxCollision() {
 
             // Move sphere 1 to collide with box
-            mSphere1Body->setTransform(Transform(Vector3(10, 5, 0), Quaternion::identity()));
+            mSphereBody1->setTransform(Transform(Vector3(10, 5, 0), Quaternion::identity()));
 
             // --------- Test collision with inactive bodies --------- //
 
             mCollisionCallback.reset();
-            mBoxBody->setIsActive(false);
-            mSphere1Body->setIsActive(false);
-            mSphere2Body->setIsActive(false);
+            mBoxBody1->setIsActive(false);
+            mSphereBody1->setIsActive(false);
+            mSphereBody2->setIsActive(false);
             mWorld->testCollision(&mCollisionCallback);
-            test(!mCollisionCallback.boxCollideWithSphere1);
-            test(!mCollisionCallback.sphere1CollideWithSphere2);
+          
 
-            test(!mWorld->testAABBOverlap(mBoxBody, mSphere1Body));
-            test(!mWorld->testAABBOverlap(mSphere1Body, mSphere2Body));
-
-            test(!mBoxProxyShape->testAABBOverlap(mSphere1ProxyShape->getWorldAABB()));
-            test(!mSphere1ProxyShape->testAABBOverlap(mSphere2ProxyShape->getWorldAABB()));
-
-            mBoxBody->setIsActive(true);
-            mSphere1Body->setIsActive(true);
-            mSphere2Body->setIsActive(true);
+            mBoxBody1->setIsActive(true);
+            mSphereBody1->setIsActive(true);
+            mSphereBody2->setIsActive(true);
 
             // --------- Test collision with collision filtering -------- //
 
-            mBoxProxyShape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_3);
-            mSphere1ProxyShape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_2);
-            mSphere2ProxyShape->setCollideWithMaskBits(CATEGORY_1);
+            //mBoxProxyShape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_3);
+            //mSphere1ProxyShape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_2);
+            //mSphere2ProxyShape->setCollideWithMaskBits(CATEGORY_1);
 
-            mCollisionCallback.reset();
-            mWorld->testCollision(&mCollisionCallback);
-            test(mCollisionCallback.boxCollideWithSphere1);
-            test(!mCollisionCallback.sphere1CollideWithSphere2);
+            //mCollisionCallback.reset();
+            //mWorld->testCollision(&mCollisionCallback);
+            //test(mCollisionCallback.boxCollideWithSphere1);
+            //test(!mCollisionCallback.sphere1CollideWithSphere2);
 
-            // Move sphere 1 to collide with sphere 2
-            mSphere1Body->setTransform(Transform(Vector3(30, 15, 10), Quaternion::identity()));
+            //// Move sphere 1 to collide with sphere 2
+            //mSphere1Body->setTransform(Transform(Vector3(30, 15, 10), Quaternion::identity()));
 
-            mCollisionCallback.reset();
-            mWorld->testCollision(&mCollisionCallback);
-            test(!mCollisionCallback.boxCollideWithSphere1);
-            test(mCollisionCallback.sphere1CollideWithSphere2);
+            //mCollisionCallback.reset();
+            //mWorld->testCollision(&mCollisionCallback);
+            //test(!mCollisionCallback.boxCollideWithSphere1);
+            //test(mCollisionCallback.sphere1CollideWithSphere2);
 
-            mBoxProxyShape->setCollideWithMaskBits(CATEGORY_2);
-            mSphere1ProxyShape->setCollideWithMaskBits(CATEGORY_2);
-            mSphere2ProxyShape->setCollideWithMaskBits(CATEGORY_3);
+            //mBoxProxyShape->setCollideWithMaskBits(CATEGORY_2);
+            //mSphere1ProxyShape->setCollideWithMaskBits(CATEGORY_2);
+            //mSphere2ProxyShape->setCollideWithMaskBits(CATEGORY_3);
 
-            mCollisionCallback.reset();
-            mWorld->testCollision(&mCollisionCallback);
-            test(!mCollisionCallback.boxCollideWithSphere1);
-            test(!mCollisionCallback.sphere1CollideWithSphere2);
+            //mCollisionCallback.reset();
+            //mWorld->testCollision(&mCollisionCallback);
+            //test(!mCollisionCallback.boxCollideWithSphere1);
+            //test(!mCollisionCallback.sphere1CollideWithSphere2);
 
-            // Move sphere 1 to collide with box
-            mSphere1Body->setTransform(Transform(Vector3(10, 5, 0), Quaternion::identity()));
+            //// Move sphere 1 to collide with box
+            //mSphere1Body->setTransform(Transform(Vector3(10, 5, 0), Quaternion::identity()));
 
-            mBoxProxyShape->setCollideWithMaskBits(0xFFFF);
-            mSphere1ProxyShape->setCollideWithMaskBits(0xFFFF);
-            mSphere2ProxyShape->setCollideWithMaskBits(0xFFFF);
+            //mBoxProxyShape->setCollideWithMaskBits(0xFFFF);
+            //mSphere1ProxyShape->setCollideWithMaskBits(0xFFFF);
+            //mSphere2ProxyShape->setCollideWithMaskBits(0xFFFF);
         }
+
+		void testMultipleCollisions() {
+
+			// TODO : Test collisions without categories set
+
+			// TODO : Test colliisons with categories set
+
+			// Assign collision categories to proxy shapes
+			//mBoxProxyShape->setCollisionCategoryBits(CATEGORY_1);
+			//mSphere1ProxyShape->setCollisionCategoryBits(CATEGORY_1);
+			//mSphere2ProxyShape->setCollisionCategoryBits(CATEGORY_2);
+		}
  };
 
 }

test/tests/collision/TestDynamicAABBTree.h

@@ -114,6 +114,12 @@ class TestDynamicAABBTree : public Test {
 
             // Dynamic AABB Tree
             DynamicAABBTree tree;
+			
+#ifdef IS_PROFILING_ACTIVE
+			/// Pointer to the profiler
+			Profiler* profiler = new Profiler();
+			tree.setProfiler(profiler);
+#endif
 
             int object1Data = 56;
             int object2Data = 23;
@@ -152,6 +158,10 @@ class TestDynamicAABBTree : public Test {
             test(*(int*)(tree.getNodeDataPointer(object2Id)) == object2Data);
             test(*(int*)(tree.getNodeDataPointer(object3Id)) == object3Data);
             test(*(int*)(tree.getNodeDataPointer(object4Id)) == object4Data);
+
+#ifdef IS_PROFILING_ACTIVE
+			delete profiler;
+#endif
         }
 
         void testOverlapping() {
@@ -161,6 +171,12 @@ class TestDynamicAABBTree : public Test {
             // Dynamic AABB Tree
             DynamicAABBTree tree;
 
+#ifdef IS_PROFILING_ACTIVE
+			/// Pointer to the profiler
+			Profiler* profiler = new Profiler();
+			tree.setProfiler(profiler);
+#endif
+
             int object1Data = 56;
             int object2Data = 23;
             int object3Data = 13;
@@ -342,6 +358,9 @@ class TestDynamicAABBTree : public Test {
             test(!mOverlapCallback.isOverlapping(object3Id));
             test(!mOverlapCallback.isOverlapping(object4Id));
 
+#ifdef IS_PROFILING_ACTIVE
+			delete profiler;
+#endif
         }
 
         void testRaycast() {
@@ -351,6 +370,12 @@ class TestDynamicAABBTree : public Test {
             // Dynamic AABB Tree
             DynamicAABBTree tree;
 
+#ifdef IS_PROFILING_ACTIVE
+			/// Pointer to the profiler
+			Profiler* profiler = new Profiler();
+			tree.setProfiler(profiler);
+#endif
+
             int object1Data = 56;
             int object2Data = 23;
             int object3Data = 13;
@@ -513,6 +538,10 @@ class TestDynamicAABBTree : public Test {
             test(!mRaycastCallback.isHit(object2Id));
             test(mRaycastCallback.isHit(object3Id));
             test(mRaycastCallback.isHit(object4Id));
+
+#ifdef IS_PROFILING_ACTIVE
+			delete profiler;
+#endif
         }
  };
 

test/tests/collision/TestHalfEdgeStructure.h

@@ -19,6 +19,9 @@ class TestHalfEdgeStructure : public Test {
 
         // ---------- Atributes ---------- //
 
+        /// Memory allocator
+        DefaultAllocator mAllocator;
+
 
     public :
 
@@ -43,7 +46,7 @@ class TestHalfEdgeStructure : public Test {
         void testCube() {
 
             // Create the half-edge structure for a cube
-            rp3d::HalfEdgeStructure cubeStructure;
+            rp3d::HalfEdgeStructure cubeStructure(mAllocator, 6, 8, 24);
 
             rp3d::Vector3 vertices[8] = {
                 rp3d::Vector3(-0.5, -0.5, 0.5),
@@ -67,18 +70,18 @@ class TestHalfEdgeStructure : public Test {
             cubeStructure.addVertex(7);
 
             // Faces
-            std::vector<uint> face0;
-            face0.push_back(0); face0.push_back(1); face0.push_back(2); face0.push_back(3);
-            std::vector<uint> face1;
-            face1.push_back(1); face1.push_back(5); face1.push_back(6); face1.push_back(2);
-            std::vector<uint> face2;
-            face2.push_back(5); face2.push_back(4); face2.push_back(7); face2.push_back(6);
-            std::vector<uint> face3;
-            face3.push_back(4); face3.push_back(0); face3.push_back(3); face3.push_back(7);
-            std::vector<uint> face4;
-            face4.push_back(0); face4.push_back(4); face4.push_back(5); face4.push_back(1);
-            std::vector<uint> face5;
-            face5.push_back(2); face5.push_back(6); face5.push_back(7); face5.push_back(3);
+            List<uint> face0(mAllocator, 4);
+            face0.add(0); face0.add(1); face0.add(2); face0.add(3);
+            List<uint> face1(mAllocator, 4);
+            face1.add(1); face1.add(5); face1.add(6); face1.add(2);
+            List<uint> face2(mAllocator, 4);
+            face2.add(5); face2.add(4); face2.add(7); face2.add(6);
+            List<uint> face3(mAllocator, 4);
+            face3.add(4); face3.add(0); face3.add(3); face3.add(7);
+            List<uint> face4(mAllocator, 4);
+            face4.add(0); face4.add(4); face4.add(5); face4.add(1);
+            List<uint> face5(mAllocator, 4);
+            face5.add(2); face5.add(6); face5.add(7); face5.add(3);
 
             cubeStructure.addFace(face0);
             cubeStructure.addFace(face1);
@@ -168,7 +171,7 @@ class TestHalfEdgeStructure : public Test {
 
             // Create the half-edge structure for a tetrahedron
             std::vector<std::vector<uint>> faces;
-            rp3d::HalfEdgeStructure tetrahedron;
+            rp3d::HalfEdgeStructure tetrahedron(mAllocator, 4, 4, 12);
 
             // Vertices
             rp3d::Vector3 vertices[4] = {
@@ -184,14 +187,14 @@ class TestHalfEdgeStructure : public Test {
             tetrahedron.addVertex(3);
 
             // Faces
-            std::vector<uint> face0;
-            face0.push_back(0); face0.push_back(1); face0.push_back(2);
-            std::vector<uint> face1;
-            face1.push_back(0); face1.push_back(3); face1.push_back(1);
-            std::vector<uint> face2;
-            face2.push_back(1); face2.push_back(3); face2.push_back(2);
-            std::vector<uint> face3;
-            face3.push_back(0); face3.push_back(2); face3.push_back(3);
+            List<uint> face0(mAllocator, 3);
+            face0.add(0); face0.add(1); face0.add(2);
+            List<uint> face1(mAllocator, 3);
+            face1.add(0); face1.add(3); face1.add(1);
+            List<uint> face2(mAllocator, 3);
+            face2.add(1); face2.add(3); face2.add(2);
+            List<uint> face3(mAllocator, 3);
+            face3.add(0); face3.add(2); face3.add(3);
 
             tetrahedron.addFace(face0);
             tetrahedron.addFace(face1);

test/tests/collision/TestRaycast.h

@@ -99,6 +99,8 @@ class TestRaycast : public Test {
         // Raycast callback class
         WorldRaycastCallback mCallback;
 
+        DefaultAllocator mAllocator;
+
         // Epsilon
         decimal epsilon;
 
@@ -203,7 +205,7 @@ class TestRaycast : public Test {
             triangleVertices[1] = Vector3(105, 100, 0);
             triangleVertices[2] = Vector3(100, 103, 0);
             Vector3 triangleVerticesNormals[3] = {Vector3(0, 0, 1), Vector3(0, 0, 1), Vector3(0, 0, 1)};
-            mTriangleShape = new TriangleShape(triangleVertices, triangleVerticesNormals, 0);
+            mTriangleShape = new TriangleShape(triangleVertices, triangleVerticesNormals, 0, mAllocator);
             mTriangleProxyShape = mTriangleBody->addCollisionShape(mTriangleShape, mShapeTransform);
 
             mCapsuleShape = new CapsuleShape(2, 5);

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 :
 
@@ -174,13 +176,13 @@ class TestMathematicsFunctions : public Test {
             segmentVertices.push_back(Vector3(-6, 3, 0));
             segmentVertices.push_back(Vector3(8, 3, 0));
 
-            std::vector<Vector3> planesNormals;
-            std::vector<Vector3> planesPoints;
-            planesNormals.push_back(Vector3(-1, 0, 0));
-            planesPoints.push_back(Vector3(4, 0, 0));
+            List<Vector3> planesNormals(mAllocator, 2);
+            List<Vector3> planesPoints(mAllocator, 2);
+            planesNormals.add(Vector3(-1, 0, 0));
+            planesPoints.add(Vector3(4, 0, 0));
 
-            std::vector<Vector3> clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1],
-                                                                             planesPoints, planesNormals);
+            List<Vector3> clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1],
+                                                                             planesPoints, planesNormals, mAllocator);
             test(clipSegmentVertices.size() == 2);
             test(approxEqual(clipSegmentVertices[0].x, -6, 0.000001));
             test(approxEqual(clipSegmentVertices[0].y, 3, 0.000001));
@@ -193,7 +195,7 @@ class TestMathematicsFunctions : public Test {
             segmentVertices.push_back(Vector3(8, 3, 0));
             segmentVertices.push_back(Vector3(-6, 3, 0));
 
-            clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals);
+            clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals, mAllocator);
             test(clipSegmentVertices.size() == 2);
             test(approxEqual(clipSegmentVertices[0].x, 4, 0.000001));
             test(approxEqual(clipSegmentVertices[0].y, 3, 0.000001));
@@ -206,7 +208,7 @@ class TestMathematicsFunctions : public Test {
             segmentVertices.push_back(Vector3(-6, 3, 0));
             segmentVertices.push_back(Vector3(3, 3, 0));
 
-            clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals);
+            clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals, mAllocator);
             test(clipSegmentVertices.size() == 2);
             test(approxEqual(clipSegmentVertices[0].x, -6, 0.000001));
             test(approxEqual(clipSegmentVertices[0].y, 3, 0.000001));
@@ -219,41 +221,41 @@ class TestMathematicsFunctions : public Test {
             segmentVertices.push_back(Vector3(5, 3, 0));
             segmentVertices.push_back(Vector3(8, 3, 0));
 
-            clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals);
+            clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals, mAllocator);
             test(clipSegmentVertices.size() == 0);
 
             // Test clipPolygonWithPlanes()
-            std::vector<Vector3> polygonVertices;
-            polygonVertices.push_back(Vector3(-4, 2, 0));
-            polygonVertices.push_back(Vector3(7, 2, 0));
-            polygonVertices.push_back(Vector3(7, 4, 0));
-            polygonVertices.push_back(Vector3(-4, 4, 0));
+            List<Vector3> polygonVertices(mAllocator);
+            polygonVertices.add(Vector3(-4, 2, 0));
+            polygonVertices.add(Vector3(7, 2, 0));
+            polygonVertices.add(Vector3(7, 4, 0));
+            polygonVertices.add(Vector3(-4, 4, 0));
 
-            planesNormals.clear();
-            planesPoints.clear();
-            planesNormals.push_back(Vector3(1, 0, 0));
-            planesPoints.push_back(Vector3(0, 0, 0));
-            planesNormals.push_back(Vector3(0, 1, 0));
-            planesPoints.push_back(Vector3(0, 0, 0));
-            planesNormals.push_back(Vector3(-1, 0, 0));
-            planesPoints.push_back(Vector3(10, 0, 0));
-            planesNormals.push_back(Vector3(0, -1, 0));
-            planesPoints.push_back(Vector3(10, 5, 0));
+            List<Vector3> polygonPlanesNormals(mAllocator);
+            List<Vector3> polygonPlanesPoints(mAllocator);
+            polygonPlanesNormals.add(Vector3(1, 0, 0));
+            polygonPlanesPoints.add(Vector3(0, 0, 0));
+            polygonPlanesNormals.add(Vector3(0, 1, 0));
+            polygonPlanesPoints.add(Vector3(0, 0, 0));
+            polygonPlanesNormals.add(Vector3(-1, 0, 0));
+            polygonPlanesPoints.add(Vector3(10, 0, 0));
+            polygonPlanesNormals.add(Vector3(0, -1, 0));
+            polygonPlanesPoints.add(Vector3(10, 5, 0));
 
-            clipSegmentVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals);
-            test(clipSegmentVertices.size() == 4);
-            test(approxEqual(clipSegmentVertices[0].x, 0, 0.000001));
-            test(approxEqual(clipSegmentVertices[0].y, 2, 0.000001));
-            test(approxEqual(clipSegmentVertices[0].z, 0, 0.000001));
-            test(approxEqual(clipSegmentVertices[1].x, 7, 0.000001));
-            test(approxEqual(clipSegmentVertices[1].y, 2, 0.000001));
-            test(approxEqual(clipSegmentVertices[1].z, 0, 0.000001));
-            test(approxEqual(clipSegmentVertices[2].x, 7, 0.000001));
-            test(approxEqual(clipSegmentVertices[2].y, 4, 0.000001));
-            test(approxEqual(clipSegmentVertices[2].z, 0, 0.000001));
-            test(approxEqual(clipSegmentVertices[3].x, 0, 0.000001));
-            test(approxEqual(clipSegmentVertices[3].y, 4, 0.000001));
-            test(approxEqual(clipSegmentVertices[3].z, 0, 0.000001));
+            List<Vector3> clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, polygonPlanesPoints, polygonPlanesNormals, mAllocator);
+            test(clipPolygonVertices.size() == 4);
+            test(approxEqual(clipPolygonVertices[0].x, 0, 0.000001));
+            test(approxEqual(clipPolygonVertices[0].y, 2, 0.000001));
+            test(approxEqual(clipPolygonVertices[0].z, 0, 0.000001));
+            test(approxEqual(clipPolygonVertices[1].x, 7, 0.000001));
+            test(approxEqual(clipPolygonVertices[1].y, 2, 0.000001));
+            test(approxEqual(clipPolygonVertices[1].z, 0, 0.000001));
+            test(approxEqual(clipPolygonVertices[2].x, 7, 0.000001));
+            test(approxEqual(clipPolygonVertices[2].y, 4, 0.000001));
+            test(approxEqual(clipPolygonVertices[2].z, 0, 0.000001));
+            test(approxEqual(clipPolygonVertices[3].x, 0, 0.000001));
+            test(approxEqual(clipPolygonVertices[3].y, 4, 0.000001));
+            test(approxEqual(clipPolygonVertices[3].z, 0, 0.000001));
 
         }