Allocate the ContactInfo into a memory pool for better efficiency

git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@465 92aac97c-a6ce-11dd-a772-7fcde58d38e6

src/collision/CollisionDetection.cpp

@@ -41,14 +41,12 @@ using namespace std;
 
 // Constructor
 CollisionDetection::CollisionDetection(PhysicsWorld* world)
-                   : world(world), narrowPhaseGJKAlgorithm(*this), narrowPhaseSphereVsSphereAlgorithm(*this),
+                   : world(world), memoryPoolContacts(NB_MAX_CONTACTS), memoryPoolOverlappingPairs(NB_MAX_COLLISION_PAIRS),
-                     memoryPoolContacts(NB_MAX_CONTACTS), memoryPoolOverlappingPairs(NB_MAX_COLLISION_PAIRS) {
+                     memoryPoolContactInfos(NB_MAX_CONTACTS), narrowPhaseGJKAlgorithm(*this, memoryPoolContactInfos),
+                     narrowPhaseSphereVsSphereAlgorithm(*this, memoryPoolContactInfos) {
 
     // Create the broad-phase algorithm that will be used (Sweep and Prune with AABB)
     broadPhaseAlgorithm = new SweepAndPruneAlgorithm(*this);
-
-    // Create the narrow-phase algorithm that will be used
-    //narrowPhaseAlgorithm = new GJKAlgorithm(*this);
 }
 
 // Destructor
@@ -133,8 +131,9 @@ bool CollisionDetection::computeNarrowPhase() {
             // Create a new contact
             Contact* contact = new(memoryPoolContacts.allocateObject()) Contact(body1, body2, contactInfo);
             
-            // Free the contact info memory
+            // Delete and remove the contact info from the memory pool
-            delete contactInfo;
+            contactInfo->ContactInfo::~ContactInfo();
+            memoryPoolContactInfos.freeObject(contactInfo);
             
             // Add the contact to the contact cache of the corresponding overlapping pair
             (*it).second->addContact(contact);

src/collision/CollisionDetection.h

@@ -65,6 +65,7 @@ class CollisionDetection {
         SphereVsSphereAlgorithm narrowPhaseSphereVsSphereAlgorithm;                     // Narrow-phase Sphere vs Sphere algorithm
         MemoryPool<Contact> memoryPoolContacts;                                         // Memory pool for the contacts
         MemoryPool<OverlappingPair> memoryPoolOverlappingPairs;                         // Memory pool for the overlapping pairs
+        MemoryPool<ContactInfo> memoryPoolContactInfos;                                 // Memory pool for the contact info
         
         void computeBroadPhase();                                                       // Compute the broad-phase collision detection
         bool computeNarrowPhase();                                                      // Compute the narrow-phase collision detection

src/collision/ContactInfo.cpp

@@ -31,9 +31,7 @@ using namespace reactphysics3d;
 
 // Constructor
 ContactInfo::ContactInfo(const Vector3& normal, decimal penetrationDepth,
-                         const Vector3& localPoint1, const Vector3& localPoint2,
+                         const Vector3& localPoint1, const Vector3& localPoint2)
-                         const Transform& transform1, const Transform& transform2)
+            : normal(normal), penetrationDepth(penetrationDepth), localPoint1(localPoint1), localPoint2(localPoint2) {
-            : normal(normal), penetrationDepth(penetrationDepth), localPoint1(localPoint1), localPoint2(localPoint2),
-              worldPoint1(transform1 * localPoint1), worldPoint2(transform2 * localPoint2) {
 
 }

src/collision/ContactInfo.h

@@ -47,12 +47,9 @@ struct ContactInfo {
         const decimal penetrationDepth; // Penetration depth of the contact
         const Vector3 localPoint1;      // Contact point of body 1 in local space of body 1
         const Vector3 localPoint2;      // Contact point of body 2 in local space of body 2
-        const Vector3 worldPoint1;      // Contact point of body 1 in world space
-        const Vector3 worldPoint2;      // Contact point of body 2 in world space
         
         ContactInfo(const Vector3& normal, decimal penetrationDepth,
-                    const Vector3& localPoint1, const Vector3& localPoint2,
+                    const Vector3& localPoint1, const Vector3& localPoint2);    // Constructor
-                    const Transform& transform1, const Transform& transform2);    // Constructor
 };
 
 } // End of the ReactPhysics3D namespace

src/collision/narrowphase/EPA/EPAAlgorithm.cpp

@@ -32,7 +32,8 @@
 using namespace reactphysics3d;
 
 // Constructor
-EPAAlgorithm::EPAAlgorithm() {
+EPAAlgorithm::EPAAlgorithm(MemoryPool<ContactInfo>& memoryPoolContactInfos)
+             : memoryPoolContactInfos(memoryPoolContactInfos) {
 
 }
 
@@ -374,7 +375,9 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
     Vector3 normal = v.getUnit();
     decimal penetrationDepth = v.length();
     assert(penetrationDepth > 0.0);
-    contactInfo = new ContactInfo(normal, penetrationDepth, pALocal, pBLocal, transform1, transform2);
+    
+    // Create the contact info object
+    contactInfo = new (memoryPoolContactInfos.allocateObject()) ContactInfo(normal, penetrationDepth, pALocal, pBLocal);
     
     return true;
 }

src/collision/narrowphase/EPA/EPAAlgorithm.h

@@ -32,6 +32,7 @@
 #include "../../ContactInfo.h"
 #include "../../../mathematics/mathematics.h"
 #include "TriangleEPA.h"
+#include "../../../memory/MemoryPool.h"
 #include <algorithm>
 
 // ReactPhysics3D namespace
@@ -72,16 +73,17 @@ class TriangleComparison {
 */
 class EPAAlgorithm {
     private:
+        MemoryPool<ContactInfo>& memoryPoolContactInfos; // Reference to the memory pool for contact infos
         TriangleComparison triangleComparison;           // Triangle comparison operator
-
+        
         void addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap,
                               uint& nbTriangles, decimal upperBoundSquarePenDepth);      // Add a triangle face in the candidate triangle heap
         int isOriginInTetrahedron(const Vector3& p1, const Vector3& p2,
                                   const Vector3& p3, const Vector3& p4) const;        // Decide if the origin is in the tetrahedron
 
     public:
-        EPAAlgorithm();         // Constructor
+        EPAAlgorithm(MemoryPool<ContactInfo>& memoryPoolContactInfos);          // Constructor
-        ~EPAAlgorithm();        // Destructor
+        ~EPAAlgorithm();                                                        // Destructor
 
         bool computePenetrationDepthAndContactPoints(Simplex simplex, const Collider* collider1, const Transform& transform1,
                                                      const Collider* collider2, const Transform& transform2,

src/collision/narrowphase/GJK/GJKAlgorithm.cpp

@@ -41,8 +41,8 @@
 using namespace reactphysics3d;
 
 // Constructor
-GJKAlgorithm::GJKAlgorithm(CollisionDetection& collisionDetection)
+GJKAlgorithm::GJKAlgorithm(CollisionDetection& collisionDetection, MemoryPool<ContactInfo>& memoryPoolContactInfos)
-             :NarrowPhaseAlgorithm(collisionDetection) {
+             :NarrowPhaseAlgorithm(collisionDetection, memoryPoolContactInfos), algoEPA(memoryPoolContactInfos) {
     
 }
 
@@ -133,7 +133,8 @@ bool GJKAlgorithm::testCollision(const Collider* collider1, const Transform& tra
 			// Reject the contact if the penetration depth is negative (due too numerical errors)
 			if (penetrationDepth <= 0.0) return false;
 			
-            contactInfo = new ContactInfo(normal, penetrationDepth, pA, pB, transform1, transform2);
+            // Create the contact info object
+            contactInfo = new (memoryPoolContactInfos.allocateObject()) ContactInfo(normal, penetrationDepth, pA, pB);
 
             // There is an intersection, therefore we return true
             return true;
@@ -161,7 +162,8 @@ bool GJKAlgorithm::testCollision(const Collider* collider1, const Transform& tra
 			// Reject the contact if the penetration depth is negative (due too numerical errors)
 			if (penetrationDepth <= 0.0) return false;
 			
-            contactInfo = new ContactInfo(normal, penetrationDepth, pA, pB, transform1, transform2);
+            // Create the contact info object
+            contactInfo = new (memoryPoolContactInfos.allocateObject()) ContactInfo(normal, penetrationDepth, pA, pB);
 
             // There is an intersection, therefore we return true
             return true;
@@ -187,7 +189,8 @@ bool GJKAlgorithm::testCollision(const Collider* collider1, const Transform& tra
 			// Reject the contact if the penetration depth is negative (due too numerical errors)
 			if (penetrationDepth <= 0.0) return false;
 			
-            contactInfo = new ContactInfo(normal, penetrationDepth, pA, pB, transform1, transform2);
+            // Create the contact info object
+            contactInfo = new (memoryPoolContactInfos.allocateObject()) ContactInfo(normal, penetrationDepth, pA, pB);
 
             // There is an intersection, therefore we return true
             return true;
@@ -221,7 +224,8 @@ bool GJKAlgorithm::testCollision(const Collider* collider1, const Transform& tra
 			// Reject the contact if the penetration depth is negative (due too numerical errors)
 			if (penetrationDepth <= 0.0) return false;
 			
-            contactInfo = new ContactInfo(normal, penetrationDepth, pA, pB, transform1, transform2);
+            // Create the contact info object
+            contactInfo = new (memoryPoolContactInfos.allocateObject()) ContactInfo(normal, penetrationDepth, pA, pB);
 
             // There is an intersection, therefore we return true
             return true;

src/collision/narrowphase/GJK/GJKAlgorithm.h

@@ -66,8 +66,9 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
                                                        ContactInfo*& contactInfo, Vector3& v);             // Compute the penetration depth for enlarged objects
 
     public :
-        GJKAlgorithm(CollisionDetection& collisionDetection);       // Constructor
+        GJKAlgorithm(CollisionDetection& collisionDetection,
-        ~GJKAlgorithm();                                            // Destructor
+                     MemoryPool<ContactInfo>& memoryPoolContactInfos);  // Constructor
+        ~GJKAlgorithm();                                                // Destructor
 
         virtual bool testCollision(const Collider* collider1, const Transform& transform1,
                                    const Collider* collider2,  const Transform& transform2,

src/collision/narrowphase/NarrowPhaseAlgorithm.cpp

@@ -30,8 +30,8 @@
 using namespace reactphysics3d;
 
 // Constructor
-NarrowPhaseAlgorithm::NarrowPhaseAlgorithm(CollisionDetection& collisionDetection)
+NarrowPhaseAlgorithm::NarrowPhaseAlgorithm(CollisionDetection& collisionDetection, MemoryPool<ContactInfo>& memoryPool)
-                     :collisionDetection(collisionDetection), currentOverlappingPair(0) {
+                     :collisionDetection(collisionDetection), memoryPoolContactInfos(memoryPool), currentOverlappingPair(0) {
 
 }
 

src/collision/narrowphase/NarrowPhaseAlgorithm.h

@@ -47,12 +47,13 @@ class CollisionDetection;
 */
 class NarrowPhaseAlgorithm {
     protected :
-        CollisionDetection& collisionDetection;  // Reference to the collision detection object
+        CollisionDetection& collisionDetection;                 // Reference to the collision detection object
-        OverlappingPair* currentOverlappingPair; // Overlapping pair of the bodies currently tested for collision
+        MemoryPool<ContactInfo>& memoryPoolContactInfos;        // Reference to the memory pool for contact infos
+        OverlappingPair* currentOverlappingPair;                // Overlapping pair of the bodies currently tested for collision
         
     public :
-        NarrowPhaseAlgorithm(CollisionDetection& collisionDetection);   // Constructor
+        NarrowPhaseAlgorithm(CollisionDetection& collisionDetection, MemoryPool<ContactInfo>& memoryPool);      // Constructor
-        virtual ~NarrowPhaseAlgorithm();                                // Destructor
+        virtual ~NarrowPhaseAlgorithm();                                                                        // Destructor
         
         void setCurrentOverlappingPair(OverlappingPair* overlappingPair);       // Set the current overlapping pair of bodies
         virtual bool testCollision(const Collider* collider1, const Transform& transform1,

src/collision/narrowphase/SphereVsSphereAlgorithm.cpp

@@ -31,8 +31,8 @@
 using namespace reactphysics3d;
 
 // Constructor
-SphereVsSphereAlgorithm::SphereVsSphereAlgorithm(CollisionDetection& collisionDetection)
+SphereVsSphereAlgorithm::SphereVsSphereAlgorithm(CollisionDetection& collisionDetection, MemoryPool<ContactInfo>& memoryPoolContactInfos)
-                        :NarrowPhaseAlgorithm(collisionDetection) {
+                        :NarrowPhaseAlgorithm(collisionDetection, memoryPoolContactInfos) {
     
 }
 
@@ -62,8 +62,10 @@ bool SphereVsSphereAlgorithm::testCollision(const Collider* collider1, const Tra
         Vector3 intersectionOnBody1 = sphereCollider1->getRadius() * centerSphere2InBody1LocalSpace.getUnit();
         Vector3 intersectionOnBody2 = sphereCollider2->getRadius() * centerSphere1InBody2LocalSpace.getUnit();
         decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
-        contactInfo = new ContactInfo(vectorBetweenCenters.getUnit(), penetrationDepth, intersectionOnBody1,
+        
-                                      intersectionOnBody2, transform1, transform2);
+        // Create the contact info object
+        contactInfo = new (memoryPoolContactInfos.allocateObject()) ContactInfo(vectorBetweenCenters.getUnit(), penetrationDepth,
+                                                                                intersectionOnBody1, intersectionOnBody2);
     
         return true;
     }

src/collision/narrowphase/SphereVsSphereAlgorithm.h

@@ -45,8 +45,9 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
     protected :
         
     public :
-        SphereVsSphereAlgorithm(CollisionDetection& collisionDetection);   // Constructor
+        SphereVsSphereAlgorithm(CollisionDetection& collisionDetection,
-        virtual ~SphereVsSphereAlgorithm();                                // Destructor
+                                MemoryPool<ContactInfo>& memoryPoolContactInfos);       // Constructor
+        virtual ~SphereVsSphereAlgorithm();                                             // Destructor
 
         virtual bool testCollision(const Collider* collider1, const Transform& transform1,
                                    const Collider* collider2, const Transform& transform2,

src/constraint/Contact.cpp

@@ -33,7 +33,7 @@ using namespace std;
 Contact::Contact(Body* const body1, Body* const body2, const ContactInfo* contactInfo)
         : Constraint(body1, body2, 3, true, CONTACT), normal(contactInfo->normal), penetrationDepth(contactInfo->penetrationDepth),
           localPointOnBody1(contactInfo->localPoint1), localPointOnBody2(contactInfo->localPoint2),
-          worldPointOnBody1(contactInfo->worldPoint1), worldPointOnBody2(contactInfo->worldPoint2) {
+          worldPointOnBody1(body1->getTransform() * contactInfo->localPoint1), worldPointOnBody2(body2->getTransform() * contactInfo->localPoint2) {
     assert(penetrationDepth > 0.0);
     
     // Compute the auxiliary lower and upper bounds