Make possible to use a single collider for multiple rigid bodies

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

src/body/RigidBody.cpp

@@ -39,7 +39,6 @@ using namespace reactphysics3d;
 
     // Set the body pointer of the AABB and the collider
     aabb->setBodyPointer(this);
-    collider->setBodyPointer(this);
 
     assert(collider);
     assert(aabb);

src/colliders/Collider.h

@@ -49,32 +49,18 @@ class Body;
 class Collider {
         
     protected :
-        Body* bodyPointer;              // Pointer to the owner body (not the abstract class Body but its derivative which is instanciable)
         ColliderType type;              // Type of the collider
         
     public :
         Collider(ColliderType type);    // Constructor
         virtual ~Collider();            // Destructor
 
-        Body* getBodyPointer() const;                                                                   // Return the body pointer
-        void setBodyPointer(Body* bodyPointer);                                                         // Set the body pointer
         ColliderType getType() const;                                                                   // Return the type of the collider
         virtual Vector3 getLocalSupportPoint(const Vector3& direction, decimal margin=0.0) const=0;     // Return a local support point in a given direction
         virtual Vector3 getLocalExtents(decimal margin=0.0) const=0;                                    // Return the local extents in x,y and z direction
         virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const=0;                // Return the local inertia tensor of the collider
 };
 
-// Return the body pointer
-inline Body* Collider::getBodyPointer() const {
-    assert(bodyPointer != 0);
-    return bodyPointer;
-}
-
-// Set the body pointer
-inline void Collider::setBodyPointer(Body* bodyPointer) {
-    this->bodyPointer = bodyPointer;
-}
-
 // Return the type of the collider
 inline ColliderType Collider::getType() const {
     return type;

src/collision/CollisionDetection.cpp

@@ -121,14 +121,17 @@ bool CollisionDetection::computeNarrowPhase() {
         // Select the narrow phase algorithm to use according to the two colliders
         NarrowPhaseAlgorithm& narrowPhaseAlgorithm = SelectNarrowPhaseAlgorithm(body1->getCollider(), body2->getCollider());
         
-        // Use the narrow-phase collision detection algorithm to check if there really are a contact
+        // Notify the narrow-phase algorithm about the overlapping pair we are going to test
+        narrowPhaseAlgorithm.setCurrentOverlappingPair((*it).second);
+        
+        // Use the narrow-phase collision detection algorithm to check if there really is a collision
         if (narrowPhaseAlgorithm.testCollision(body1->getCollider(), body1->getTransform(),
                                                body2->getCollider(), body2->getTransform(), contactInfo)) {
             assert(contactInfo);
             collisionExists = true;
 
             // Create a new contact
-            Contact* contact = new(memoryPoolContacts.allocateObject()) Contact(contactInfo);
+            Contact* contact = new(memoryPoolContacts.allocateObject()) Contact(body1, body2, contactInfo);
             
             // Free the contact info memory
             delete contactInfo;

src/collision/ContactInfo.cpp

@@ -29,11 +29,11 @@
 using namespace reactphysics3d;
 
 
-// Constructor for GJK
+// Constructor
-ContactInfo::ContactInfo(Body* body1, Body* body2, const Vector3& normal, decimal penetrationDepth,
+ContactInfo::ContactInfo(const Vector3& normal, decimal penetrationDepth,
                          const Vector3& localPoint1, const Vector3& localPoint2,
                          const Transform& transform1, const Transform& transform2)
-            : body1(body1), body2(body2), normal(normal), penetrationDepth(penetrationDepth),
+            : normal(normal), penetrationDepth(penetrationDepth), localPoint1(localPoint1), localPoint2(localPoint2),
-              localPoint1(localPoint1), localPoint2(localPoint2), worldPoint1(transform1 * localPoint1), worldPoint2(transform2 * localPoint2) {
+              worldPoint1(transform1 * localPoint1), worldPoint2(transform2 * localPoint2) {
 
 }

src/collision/ContactInfo.h

@@ -43,8 +43,6 @@ namespace reactphysics3d {
 */
 struct ContactInfo {
     public:
-        Body* const body1;              // Pointer to the first body of the contact
-        Body* const body2;              // Pointer to the second body of the contact
         const Vector3 normal;           // Normal vector the the collision contact in world space
         const decimal penetrationDepth; // Penetration depth of the contact
         const Vector3 localPoint1;      // Contact point of body 1 in local space of body 1
@@ -52,7 +50,7 @@ struct ContactInfo {
         const Vector3 worldPoint1;      // Contact point of body 1 in world space
         const Vector3 worldPoint2;      // Contact point of body 2 in world space
         
-        ContactInfo(Body* body1, Body* body2, const Vector3& normal, decimal penetrationDepth,
+        ContactInfo(const Vector3& normal, decimal penetrationDepth,
                     const Vector3& localPoint1, const Vector3& localPoint2,
                     const Transform& transform1, const Transform& transform2);    // Constructor
 };

src/collision/OverlappingPair.h

@@ -52,13 +52,14 @@ class OverlappingPair {
         OverlappingPair(Body* body1, Body* body2, MemoryPool<Contact>& memoryPoolContacts);     // Constructor
         ~OverlappingPair();                                                                     // Destructor
         
-        Body* const getBody1() const;                 // Return the pointer to first body
+        Body* const getBody1() const;                           // Return the pointer to first body
-        Body* const getBody2() const;                 // Return the pointer to second body
+        Body* const getBody2() const;                           // Return the pointer to second body
-        void addContact(Contact* contact);            // Add a contact to the contact cache
+        void addContact(Contact* contact);                      // Add a contact to the contact cache
-        void update();                                // Update the contact cache
+        void update();                                          // Update the contact cache
-        Vector3 getCachedSeparatingAxis() const;      // Return the cached separating axis
+        Vector3 getCachedSeparatingAxis() const;                // Return the cached separating axis
-        uint getNbContacts() const;                   // Return the number of contacts in the cache
+        void setCachedSeparatingAxis(const Vector3& axis);      // Set the cached separating axis
-        Contact* getContact(uint index) const;        // Return a contact of the cache                           
+        uint getNbContacts() const;                             // Return the number of contacts in the cache
+        Contact* getContact(uint index) const;                  // Return a contact of the cache                           
 };
 
 // Return the pointer to first body
@@ -84,7 +85,12 @@ inline void OverlappingPair::update() {
 // Return the cached separating axis
 inline Vector3 OverlappingPair::getCachedSeparatingAxis() const {
     return cachedSeparatingAxis;
-}      
+} 
+
+// Set the cached separating axis
+inline void OverlappingPair::setCachedSeparatingAxis(const Vector3& axis) {
+    cachedSeparatingAxis = axis;
+}              
 
 
 // Return the number of contacts in the cache

src/collision/narrowphase/EPA/EPAAlgorithm.cpp

@@ -374,8 +374,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
     Vector3 normal = v.getUnit();
     decimal penetrationDepth = v.length();
     assert(penetrationDepth > 0.0);
-    contactInfo = new ContactInfo(collider1->getBodyPointer(), collider2->getBodyPointer(), normal,
+    contactInfo = new ContactInfo(normal, penetrationDepth, pALocal, pBLocal, transform1, transform2);
-                                  penetrationDepth, pALocal, pBLocal, transform1, transform2);
     
     return true;
 }

src/collision/narrowphase/GJK/GJKAlgorithm.cpp

@@ -34,6 +34,7 @@
 #include <cmath>
 #include <cfloat>
 #include <cassert>
+#include <iostream> // TODO : DELETE THIS
 
 
 // We want to use the ReactPhysics3D namespace
@@ -63,8 +64,6 @@ GJKAlgorithm::~GJKAlgorithm() {
 bool GJKAlgorithm::testCollision(const Collider* collider1, const Transform& transform1,
                                  const Collider* collider2,  const Transform& transform2,
 		                         ContactInfo*& contactInfo) {
-
-    assert(collider1 != collider2);
     
     Vector3 suppA;             // Support point of object A
     Vector3 suppB;             // Support point of object B
@@ -73,8 +72,6 @@ bool GJKAlgorithm::testCollision(const Collider* collider1, const Transform& tra
     Vector3 pB;                // Closest point of object B
     decimal vDotw;
     decimal prevDistSquare;
-    Body* const body1 = collider1->getBodyPointer();
-    Body* const body2 = collider2->getBodyPointer();
 
     // Transform a point from local space of body 2 to local space of body 1 (the GJK algorithm is done in local space of body 1)
     Transform body2Tobody1 = transform1.inverse() * transform2;
@@ -91,13 +88,13 @@ bool GJKAlgorithm::testCollision(const Collider* collider1, const Transform& tra
     Simplex simplex;
 
     // Get the previous point V (last cached separating axis)
-	OverlappingPair* overlappingPair = collisionDetection.getOverlappingPair(body1->getID(), body2->getID());
+	Vector3 v = currentOverlappingPair->getCachedSeparatingAxis();
-	Vector3 v = (overlappingPair) ? overlappingPair->getCachedSeparatingAxis() : Vector3(1.0, 1.0, 1.0);
+
-			
     // Initialize the upper bound for the square distance
     decimal distSquare = DECIMAL_MAX;
     
     do {
+              
         // Compute the support points for original objects (without margins) A and B
         suppA = collider1->getLocalSupportPoint(-v);
         suppB = body2Tobody1 * collider2->getLocalSupportPoint(rotateToBody2 * v);
@@ -109,6 +106,10 @@ bool GJKAlgorithm::testCollision(const Collider* collider1, const Transform& tra
         
         // If the enlarge objects (with margins) do not intersect
         if (vDotw > 0.0 && vDotw * vDotw > distSquare * marginSquare) {
+                        
+            // Cache the current separating axis for frame coherence
+            currentOverlappingPair->setCachedSeparatingAxis(v);
+            
             // No intersection, we return false
             return false;
         }
@@ -132,7 +133,7 @@ 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(body1, body2, normal, penetrationDepth, pA, pB, transform1, transform2);
+            contactInfo = new ContactInfo(normal, penetrationDepth, pA, pB, transform1, transform2);
 
             // There is an intersection, therefore we return true
             return true;
@@ -160,7 +161,7 @@ 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(body1, body2, normal, penetrationDepth, pA, pB, transform1, transform2);
+            contactInfo = new ContactInfo(normal, penetrationDepth, pA, pB, transform1, transform2);
 
             // There is an intersection, therefore we return true
             return true;
@@ -186,7 +187,7 @@ 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(body1, body2, normal, penetrationDepth, pA, pB, transform1, transform2);
+            contactInfo = new ContactInfo(normal, penetrationDepth, pA, pB, transform1, transform2);
 
             // There is an intersection, therefore we return true
             return true;
@@ -220,7 +221,7 @@ 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(body1, body2, normal, penetrationDepth, pA, pB, transform1, transform2);
+            contactInfo = new ContactInfo(normal, penetrationDepth, pA, pB, transform1, transform2);
 
             // There is an intersection, therefore we return true
             return true;

src/collision/narrowphase/NarrowPhaseAlgorithm.cpp

@@ -31,7 +31,7 @@ using namespace reactphysics3d;
 
 // Constructor
 NarrowPhaseAlgorithm::NarrowPhaseAlgorithm(CollisionDetection& collisionDetection)
-                     :collisionDetection(collisionDetection) {
+                     :collisionDetection(collisionDetection), currentOverlappingPair(0) {
 
 }
 

src/collision/narrowphase/NarrowPhaseAlgorithm.h

@@ -29,6 +29,7 @@
 // Libraries
 #include "../../body/Body.h"
 #include "../ContactInfo.h"
+#include "../OverlappingPair.h"
 
 // Namespace ReactPhysics3D
 namespace reactphysics3d {
@@ -39,7 +40,7 @@ class CollisionDetection;
 /*  -------------------------------------------------------------------
     Class NarrowPhaseAlgorithm :
         This class is an abstract class that represents an algorithm
-        used to perform the narrow phase of a collision detection. The
+        used to perform the narrow-phase of a collision detection. The
         goal of the narrow phase algorithm is to compute contact
         informations of a collision between two bodies.
     -------------------------------------------------------------------
@@ -47,16 +48,23 @@ class CollisionDetection;
 class NarrowPhaseAlgorithm {
     protected :
         CollisionDetection& collisionDetection;  // Reference to the collision detection object
+        OverlappingPair* currentOverlappingPair; // Overlapping pair of the bodies currently tested for collision
         
     public :
         NarrowPhaseAlgorithm(CollisionDetection& collisionDetection);   // Constructor
         virtual ~NarrowPhaseAlgorithm();                                // Destructor
-
+        
+        void setCurrentOverlappingPair(OverlappingPair* overlappingPair);       // Set the current overlapping pair of bodies
         virtual bool testCollision(const Collider* collider1, const Transform& transform1,
                                    const Collider* collider2, const Transform& transform2,
                                    ContactInfo*& contactInfo)=0;  // Return true and compute a contact info if the two bounding volume collide
 };
 
+// Set the current overlapping pair of bodies
+inline void NarrowPhaseAlgorithm::setCurrentOverlappingPair(OverlappingPair* overlappingPair) {
+    currentOverlappingPair = overlappingPair;
+}      
+
 } // End of reactphysics3d namespace
 
 #endif

src/collision/narrowphase/SphereVsSphereAlgorithm.cpp

@@ -43,8 +43,6 @@ SphereVsSphereAlgorithm::~SphereVsSphereAlgorithm() {
 
 bool SphereVsSphereAlgorithm::testCollision(const Collider* collider1, const Transform& transform1,
                                             const Collider* collider2, const Transform& transform2, ContactInfo*& contactInfo) {
-
-    assert(collider1 != collider2);
     
     // Get the sphere colliders
     const SphereCollider* sphereCollider1 = dynamic_cast<const SphereCollider*>(collider1);
@@ -64,8 +62,8 @@ 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(collider1->getBodyPointer(), collider2->getBodyPointer(), vectorBetweenCenters.getUnit(),
+        contactInfo = new ContactInfo(vectorBetweenCenters.getUnit(), penetrationDepth, intersectionOnBody1,
-                                      penetrationDepth, intersectionOnBody1, intersectionOnBody2, transform1, transform2);
+                                      intersectionOnBody2, transform1, transform2);
     
         return true;
     }

src/constraint/Contact.cpp

@@ -30,8 +30,8 @@ using namespace reactphysics3d;
 using namespace std;
 
 // Constructor
-Contact::Contact(const ContactInfo* contactInfo)
+Contact::Contact(Body* const body1, Body* const body2, const ContactInfo* contactInfo)
-        : Constraint(contactInfo->body1, contactInfo->body2, 3, true, CONTACT), normal(contactInfo->normal), penetrationDepth(contactInfo->penetrationDepth),
+        : Constraint(body1, body2, 3, true, CONTACT), normal(contactInfo->normal), penetrationDepth(contactInfo->penetrationDepth),
           localPointOnBody1(contactInfo->localPoint1), localPointOnBody2(contactInfo->localPoint2),
           worldPointOnBody1(contactInfo->worldPoint1), worldPointOnBody2(contactInfo->worldPoint2) {
     assert(penetrationDepth > 0.0);

src/constraint/Contact.h

@@ -75,11 +75,11 @@ class Contact : public Constraint {
         void computeFrictionVectors();                  // Compute the two friction vectors that span the tangential friction plane
 
     public :
-        Contact(const ContactInfo* contactInfo);        // Constructor
+        Contact(Body* const body1, Body* const body2, const ContactInfo* contactInfo); // Constructor
         virtual ~Contact();                             // Destructor
 
         Vector3 getNormal() const;                                                     // Return the normal vector of the contact
-        void setPenetrationDepth(decimal penetrationDepth);                             // Set the penetration depth of the contact
+        void setPenetrationDepth(decimal penetrationDepth);                            // Set the penetration depth of the contact
         Vector3 getLocalPointOnBody1() const;                                          // Return the contact local point on body 1
         Vector3 getLocalPointOnBody2() const;                                          // Return the contact local point on body 2
         Vector3 getWorldPointOnBody1() const;                                          // Return the contact world point on body 1