Continue the implementation of the constraint solver

src/collision/CollisionDetection.cpp

@@ -99,7 +99,7 @@ void CollisionDetection::computeNarrowPhase() {
     
     // For each possible collision pair of bodies
     for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); it++) {
-        ContactInfo* contactInfo = NULL;
+        ContactPointInfo* contactInfo = NULL;
 
         BroadPhasePair* pair = (*it).second;
         assert(pair != NULL);
@@ -125,12 +125,18 @@ void CollisionDetection::computeNarrowPhase() {
                                                contactInfo)) {
             assert(contactInfo != NULL);
 
+            // Set the bodies of the contact
+            contactInfo->body1 = dynamic_cast<RigidBody*>(body1);
+            contactInfo->body2 = dynamic_cast<RigidBody*>(body2);
+            assert(contactInfo->body1 != NULL);
+            assert(contactInfo->body2 != NULL);
+
             // Notify the world about the new narrow-phase contact
             mWorld->notifyNewContact(pair, contactInfo);
 
             // Delete and remove the contact info from the memory allocator
-            contactInfo->ContactInfo::~ContactInfo();
+            contactInfo->ContactPointInfo::~ContactPointInfo();
-            mMemoryAllocator.release(contactInfo, sizeof(ContactInfo));
+            mMemoryAllocator.release(contactInfo, sizeof(ContactPointInfo));
         }
     }
 }

src/collision/CollisionDetection.h

@@ -33,7 +33,7 @@
 #include "narrowphase/GJK/GJKAlgorithm.h"
 #include "narrowphase/SphereVsSphereAlgorithm.h"
 #include "../memory/MemoryAllocator.h"
-#include "ContactInfo.h"
+#include "../constraint/ContactPoint.h"
 #include <vector>
 #include <map>
 #include <set>

src/collision/ContactInfo.cpp

@@ -1,38 +0,0 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
-* Copyright (c) 2010-2013 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 "ContactInfo.h"
-
-using namespace reactphysics3d;
-
-
-// Constructor
-ContactInfo::ContactInfo(const Vector3& normal, decimal penetrationDepth,
-                         const Vector3& localPoint1, const Vector3& localPoint2)
-            : normal(normal), penetrationDepth(penetrationDepth), localPoint1(localPoint1),
-              localPoint2(localPoint2) {
-
-}

src/collision/ContactInfo.h

@@ -1,81 +0,0 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
-* Copyright (c) 2010-2013 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_CONTACT_INFO_H
-#define	REACTPHYSICS3D_CONTACT_INFO_H
-
-// Libraries
-#include "../collision/shapes/BoxShape.h"
-#include "../mathematics/mathematics.h"
-
-// ReactPhysics3D namespace
-namespace reactphysics3d {
-
-// Structure ContactInfo
-/**
- * This structure contains informations about a collision contact
- * computed during the narrow-phase collision detection. Those
- * informations are used to compute the contact set for a contact
- * between two bodies.
- */
-struct ContactInfo {
-
-    private:
-
-        // -------------------- Methods -------------------- //
-
-        /// Private copy-constructor
-        ContactInfo(const ContactInfo& contactInfo);
-
-        /// Private assignment operator
-        ContactInfo& operator=(const ContactInfo& contactInfo);
-
-    public:
-
-        // -------------------- Attributes -------------------- //
-
-        /// Normal vector the the collision contact in world space
-        const Vector3 normal;
-
-        /// Penetration depth of the contact
-        const decimal penetrationDepth;
-
-        /// Contact point of body 1 in local space of body 1
-        const Vector3 localPoint1;
-
-        /// Contact point of body 2 in local space of body 2
-        const Vector3 localPoint2;
-
-        // -------------------- Methods -------------------- //
-
-        /// Constructor
-        ContactInfo(const Vector3& normal, decimal penetrationDepth,
-                    const Vector3& localPoint1, const Vector3& localPoint2);
-};
-
-}
-
-#endif
-

src/collision/narrowphase/EPA/EPAAlgorithm.cpp

@@ -87,7 +87,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
                                                            const Transform& transform1,
                                                            const CollisionShape* collisionShape2,
                                                            const Transform& transform2,
-                                                           Vector3& v, ContactInfo*& contactInfo) {
+                                                           Vector3& v, ContactPointInfo*& contactInfo) {
 
     Vector3 suppPointsA[MAX_SUPPORT_POINTS];  // Support points of object A in local coordinates
     Vector3 suppPointsB[MAX_SUPPORT_POINTS];  // Support points of object B in local coordinates
@@ -400,7 +400,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
     assert(penetrationDepth > 0.0);
     
     // Create the contact info object
-    contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactInfo))) ContactInfo(normal,
+    contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(normal,
                                                                              penetrationDepth,
                                                                              pALocal, pBLocal);
     

src/collision/narrowphase/EPA/EPAAlgorithm.h

@@ -29,7 +29,7 @@
 // Libraries
 #include "../GJK/Simplex.h"
 #include "../../shapes/CollisionShape.h"
-#include "../../ContactInfo.h"
+#include "../../../constraint/ContactPoint.h"
 #include "../../../mathematics/mathematics.h"
 #include "TriangleEPA.h"
 #include "../../../memory/MemoryAllocator.h"
@@ -123,7 +123,7 @@ class EPAAlgorithm {
                                                      const Transform& transform1,
                                                      const CollisionShape* collisionShape2,
                                                      const Transform& transform2,
-                                                     Vector3& v, ContactInfo*& contactInfo);
+                                                     Vector3& v, ContactPointInfo*& contactInfo);
 };
 
 // Add a triangle face in the candidate triangle heap in the EPA algorithm

src/collision/narrowphase/GJK/GJKAlgorithm.cpp

@@ -61,7 +61,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1,
                                  const Transform& transform1,
                                  const CollisionShape* collisionShape2,
                                  const Transform& transform2,
-		                         ContactInfo*& contactInfo) {
+		                         ContactPointInfo*& contactInfo) {
     
     Vector3 suppA;             // Support point of object A
     Vector3 suppB;             // Support point of object B
@@ -137,7 +137,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1,
 			if (penetrationDepth <= 0.0) return false;
 			
             // Create the contact info object
-            contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactInfo))) ContactInfo(normal,
+            contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(normal,
                                                                                    penetrationDepth,
                                                                                     pA, pB);
 
@@ -169,7 +169,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1,
 			if (penetrationDepth <= 0.0) return false;
 			
             // Create the contact info object
-            contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactInfo))) ContactInfo(normal,
+            contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(normal,
                                                                                    penetrationDepth,
                                                                                    pA, pB);
 
@@ -199,7 +199,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1,
 			if (penetrationDepth <= 0.0) return false;
 			
             // Create the contact info object
-            contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactInfo))) ContactInfo(normal,
+            contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(normal,
                                                                                    penetrationDepth,
                                                                                     pA, pB);
 
@@ -236,7 +236,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1,
 			if (penetrationDepth <= 0.0) return false;
 			
             // Create the contact info object
-            contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactInfo))) ContactInfo(normal,
+            contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(normal,
                                                                                    penetrationDepth,
                                                                                     pA, pB);
 
@@ -263,7 +263,7 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap
                                                              const Transform& transform1,
                                                              const CollisionShape* collisionShape2,
                                                              const Transform& transform2,
-                                                             ContactInfo*& contactInfo,
+                                                             ContactPointInfo*& contactInfo,
                                                              Vector3& v) {
     Simplex simplex;
     Vector3 suppA;

src/collision/narrowphase/GJK/GJKAlgorithm.h

@@ -28,7 +28,7 @@
 
 // Libraries
 #include "../NarrowPhaseAlgorithm.h"
-#include "../../ContactInfo.h"
+#include "../../../constraint/ContactPoint.h"
 #include "../../../collision/shapes/CollisionShape.h"
 #include "../EPA/EPAAlgorithm.h"
 
@@ -78,7 +78,7 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
                                                        const Transform& transform1,
                                                        const CollisionShape* collisionShape2,
                                                        const Transform& transform2,
-                                                       ContactInfo*& contactInfo, Vector3& v);
+                                                       ContactPointInfo*& contactInfo, Vector3& v);
 
     public :
 
@@ -95,7 +95,7 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
                                    const Transform& transform1,
                                    const CollisionShape* collisionShape2,
                                    const Transform& transform2,
-                                   ContactInfo*& contactInfo);
+                                   ContactPointInfo*& contactInfo);
 };
 
 }

src/collision/narrowphase/NarrowPhaseAlgorithm.h

@@ -28,7 +28,7 @@
 
 // Libraries
 #include "../../body/Body.h"
-#include "../ContactInfo.h"
+#include "../../constraint/ContactPoint.h"
 #include "../broadphase/PairManager.h"
 #include "../../memory/MemoryAllocator.h"
 #include "../BroadPhasePair.h"
@@ -36,7 +36,7 @@
 
 /// Namespace ReactPhysics3D
 namespace reactphysics3d {
-    
+
 // Class NarrowPhaseAlgorithm
 /**
  * This class is an abstract class that represents an algorithm
@@ -82,7 +82,7 @@ class NarrowPhaseAlgorithm {
                                    const Transform& transform1,
                                    const CollisionShape* collisionShape2,
                                    const Transform& transform2,
-                                   ContactInfo*& contactInfo)=0;
+                                   ContactPointInfo*& contactInfo)=0;
 };
 
 // Set the current overlapping pair of bodies

src/collision/narrowphase/SphereVsSphereAlgorithm.cpp

@@ -45,7 +45,7 @@ bool SphereVsSphereAlgorithm::testCollision(const CollisionShape* collisionShape
                                             const Transform& transform1,
                                             const CollisionShape* collisionShape2,
                                             const Transform& transform2,
-                                            ContactInfo*& contactInfo) {
+                                            ContactPointInfo*& contactInfo) {
     
     // Get the sphere collision shapes
     const SphereShape* sphereShape1 = dynamic_cast<const SphereShape*>(collisionShape1);
@@ -69,7 +69,7 @@ bool SphereVsSphereAlgorithm::testCollision(const CollisionShape* collisionShape
         decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
         
         // Create the contact info object
-        contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactInfo))) ContactInfo(
+        contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(
                            vectorBetweenCenters.getUnit(), penetrationDepth,
                            intersectionOnBody1, intersectionOnBody2);
     

src/collision/narrowphase/SphereVsSphereAlgorithm.cpp.autosave

@@ -0,0 +1,81 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
+* Copyright (c) 2010-2013 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 "SphereVsSphereAlgorithm.h"
+#include "../../collision/shapes/SphereShape.h"
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+
+// Constructor
+SphereVsSphereAlgorithm::SphereVsSphereAlgorithm(MemoryAllocator& memoryAllocator)
+                        :NarrowPhaseAlgorithm(memoryAllocator) {
+    
+}
+
+// Destructor
+SphereVsSphereAlgorithm::~SphereVsSphereAlgorithm() {
+    
+}   
+
+bool SphereVsSphereAlgorithm::testCollision(const CollisionShape* collisionShape1,
+                                            const Transform& transform1,
+                                            const CollisionShape* collisionShape2,
+                                            const Transform& transform2,
+                                            ContactPointInfo*& contactInfo) {
+    
+    // Get the sphere collision shapes
+    const SphereShape* sphereShape1 = dynamic_cast<const SphereShape*>(collisionShape1);
+    const SphereShape* sphereShape2 = dynamic_cast<const SphereShape*>(collisionShape2);
+    
+    // Compute the distance between the centers
+    Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition();
+    decimal squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare();
+
+    // Compute the sum of the radius
+    decimal sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius();
+    
+    // If the sphere collision shapes intersect
+    if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) {
+        Vector3 centerSphere2InBody1LocalSpace = transform1.inverse() * transform2.getPosition();
+        Vector3 centerSphere1InBody2LocalSpace = transform2.inverse() * transform1.getPosition();
+        Vector3 intersectionOnBody1 = sphereShape1->getRadius() *
+                                      centerSphere2InBody1LocalSpace.getUnit();
+        Vector3 intersectionOnBody2 = sphereShape2->getRadius() *
+                                      centerSphere1InBody2LocalSpace.getUnit();
+        decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
+        
+        // Create the contact info object
+        contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(
+                           vectorBetweenCenters.getUnit(), penetrationDepth,
+                           intersectionOnBody1, intersectionOnBody2);
+    
+        return true;
+    }
+    
+    return false;
+}

src/collision/narrowphase/SphereVsSphereAlgorithm.h

@@ -28,7 +28,7 @@
 
 // Libraries
 #include "../../body/Body.h"
-#include "../ContactInfo.h"
+#include "../../constraint/ContactPoint.h"
 #include "NarrowPhaseAlgorithm.h"
 
 
@@ -67,7 +67,7 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
                                    const Transform& transform1,
                                    const CollisionShape* collisionShape2,
                                    const Transform& transform2,
-                                   ContactInfo*& contactInfo);
+                                   ContactPointInfo*& contactInfo);
 };
 
 }

src/constraint/BallAndSocketJoint.cpp

@@ -29,9 +29,8 @@
 using namespace reactphysics3d;
 
 // Constructor
-BallAndSocketJoint::BallAndSocketJoint(RigidBody* const body1, RigidBody* const body2,
+BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo &jointInfo)
-                                       bool active, ConstraintType type)
+                   : Constraint(jointInfo){
-                   : Constraint(body1, body2, active, type){
 
 }
 

src/constraint/BallAndSocketJoint.h

@@ -28,9 +28,28 @@
 
 // Libraries
 #include "Constraint.h"
+#include "../mathematics/mathematics.h"
 
 namespace reactphysics3d {
 
+// Structure BallAndSocketJointInfo
+/**
+ * This structure is used to gather the information needed to create a ball-and-socket
+ * joint. This structure will be used to create the actual ball-and-socket joint.
+ */
+struct BallAndSocketJointInfo : public ConstraintInfo {
+
+    public :
+
+        // -------------------- Attributes -------------------- //
+
+        /// Anchor point (in world space coordinates)
+        Vector3 anchorPoint;
+
+        /// Constructor
+        BallAndSocketJointInfo() : ConstraintInfo(BALLSOCKETJOINT) {}
+};
+
 // Class BallAndSocketJoint
 /**
  * This class represents a ball-and-socket joint that allows arbitrary rotation
@@ -42,18 +61,31 @@ class BallAndSocketJoint : public Constraint {
 
         // -------------------- Attributes -------------------- //
 
+        /// Anchor point of body 1 (in local space coordinates)
+        Vector3 mLocalAnchorPoint1;
+
+        /// Anchor point of body 2 (in local space coordinates)
+        Vector3 mLocalAnchorPoint2;
+
     public :
 
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        BallAndSocketJoint(RigidBody* const body1, RigidBody* const body2,
+        BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo);
-                           bool active, ConstraintType type);
 
         /// Destructor
         virtual ~BallAndSocketJoint();
+
+        /// Return the number of bytes used by the joint
+        virtual size_t getSizeInBytes() const;
 };
 
+// Return the number of bytes used by the joint
+inline size_t BallAndSocketJoint::getSizeInBytes() const {
+    return sizeof(BallAndSocketJoint);
+}
+
 }
 
 #endif

src/constraint/Constraint.cpp

@@ -29,10 +29,12 @@
 using namespace reactphysics3d;
 
 // Constructor
-Constraint::Constraint(RigidBody* const body1, RigidBody* const body2,
+Constraint::Constraint(const ConstraintInfo& constraintInfo)
-                       bool active, ConstraintType type)
+           :mBody1(constraintInfo.body1), mBody2(constraintInfo.body2), mActive(true),
-           :mBody1(body1), mBody2(body2), mActive(active), mType(type) {
+            mType(constraintInfo.type) {
 
+    assert(mBody1 != NULL);
+    assert(mBody2 != NULL);
 }
 
 // Destructor

src/constraint/Constraint.h

@@ -36,6 +36,39 @@ namespace reactphysics3d {
 // Enumeration for the type of a constraint
 enum ConstraintType {CONTACT, BALLSOCKETJOINT};
 
+// Structure ConstraintInfo
+/**
+ * This structure is used to gather the information needed to create a constraint.
+ */
+struct ConstraintInfo {
+
+    public :
+
+        // -------------------- Attributes -------------------- //
+
+        /// First rigid body of the constraint
+        RigidBody* body1;
+
+        /// Second rigid body of the constraint
+        RigidBody* body2;
+
+        /// Type of the constraint
+        ConstraintType type;
+
+        /// Constructor
+        ConstraintInfo(ConstraintType constraintType)
+                      : body1(NULL), body2(NULL), type(constraintType) {}
+
+        /// Constructor
+        ConstraintInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, ConstraintType constraintType)
+                      : body1(rigidBody1), body2(rigidBody2), type(constraintType) {
+        }
+
+        /// Destructor
+        virtual ~ConstraintInfo() {}
+
+};
+
 // Class Constraint
 /**
  * This abstract class represents a constraint in the physics engine.
@@ -74,8 +107,7 @@ class Constraint {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        Constraint(RigidBody* const body1, RigidBody* const body2,
+        Constraint(const ConstraintInfo& constraintInfo);
-                   bool active, ConstraintType type);
 
         /// Destructor
         virtual ~Constraint();
@@ -91,6 +123,9 @@ class Constraint {
 
         /// Return the type of the constraint
         ConstraintType getType() const;
+
+        /// Return the number of bytes used by the constraint
+        virtual size_t getSizeInBytes() const = 0;
 };
 
 // Return the reference to the body 1

src/constraint/ContactPoint.cpp

@@ -30,14 +30,13 @@ using namespace reactphysics3d;
 using namespace std;
 
 // Constructor
-ContactPoint::ContactPoint(RigidBody* const body1, RigidBody* const body2,
+ContactPoint::ContactPoint(const ContactPointInfo& contactInfo)
-                           const ContactInfo* contactInfo)
+             : Constraint(contactInfo), mNormal(contactInfo.normal),
-             : Constraint(body1, body2, true, CONTACT), mNormal(contactInfo->normal),
+               mPenetrationDepth(contactInfo.penetrationDepth),
-               mPenetrationDepth(contactInfo->penetrationDepth),
+               mLocalPointOnBody1(contactInfo.localPoint1),
-               mLocalPointOnBody1(contactInfo->localPoint1),
+               mLocalPointOnBody2(contactInfo.localPoint2),
-               mLocalPointOnBody2(contactInfo->localPoint2),
+               mWorldPointOnBody1(contactInfo.body1->getTransform() * contactInfo.localPoint1),
-               mWorldPointOnBody1(body1->getTransform() * contactInfo->localPoint1),
+               mWorldPointOnBody2(contactInfo.body2->getTransform() * contactInfo.localPoint2),
-               mWorldPointOnBody2(body2->getTransform() * contactInfo->localPoint2),
                mIsRestingContact(false) {
 
     mFrictionVectors[0] = Vector3(0, 0, 0);

src/constraint/ContactPoint.h

@@ -28,7 +28,6 @@
 
 // Libraries
 #include "Constraint.h"
-#include "../collision/ContactInfo.h"
 #include "../body/RigidBody.h"
 #include "../configuration.h"
 #include "../mathematics/mathematics.h"
@@ -50,6 +49,52 @@
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
 
+// Structure ContactPointInfo
+/**
+ * This structure contains informations about a collision contact
+ * computed during the narrow-phase collision detection. Those
+ * informations are used to compute the contact set for a contact
+ * between two bodies.
+ */
+struct ContactPointInfo : public ConstraintInfo {
+
+    private:
+
+        // -------------------- Methods -------------------- //
+
+        /// Private copy-constructor
+        ContactPointInfo(const ContactPointInfo& contactInfo);
+
+        /// Private assignment operator
+        ContactPointInfo& operator=(const ContactPointInfo& contactInfo);
+
+    public:
+
+        // -------------------- Attributes -------------------- //
+
+        /// Normal vector the the collision contact in world space
+        const Vector3 normal;
+
+        /// Penetration depth of the contact
+        const decimal penetrationDepth;
+
+        /// Contact point of body 1 in local space of body 1
+        const Vector3 localPoint1;
+
+        /// Contact point of body 2 in local space of body 2
+        const Vector3 localPoint2;
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        ContactPointInfo(const Vector3& normal, decimal penetrationDepth,
+                         const Vector3& localPoint1, const Vector3& localPoint2)
+            : ConstraintInfo(CONTACT), normal(normal), penetrationDepth(penetrationDepth),
+              localPoint1(localPoint1), localPoint2(localPoint2) {
+
+        }
+};
+
 // Class ContactPoint
 /**
  * This class represents a collision contact point between two
@@ -108,7 +153,7 @@ class ContactPoint : public Constraint {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ContactPoint(RigidBody* const body1, RigidBody* const body2, const ContactInfo* contactInfo);
+        ContactPoint(const ContactPointInfo& contactInfo);
 
         /// Destructor
         virtual ~ContactPoint();
@@ -176,6 +221,9 @@ class ContactPoint : public Constraint {
         /// Return the penetration depth
         decimal getPenetrationDepth() const;
 
+        /// Return the number of bytes used by the contact point
+        virtual size_t getSizeInBytes() const;
+
         #ifdef VISUAL_DEBUG
             /// Draw the contact (for debugging)
            void draw() const;
@@ -287,6 +335,10 @@ inline decimal ContactPoint::getPenetrationDepth() const {
     return mPenetrationDepth;
 }
 
+// Return the number of bytes used by the contact point
+inline size_t ContactPoint::getSizeInBytes() const {
+    return sizeof(ContactPoint);
+}
 
 #ifdef VISUAL_DEBUG
 inline void ContactPoint::draw() const {

src/engine/CollisionWorld.cpp

@@ -56,7 +56,7 @@ void CollisionWorld::notifyRemovedOverlappingPair(const BroadPhasePair* removedP
 
 // Notify the world about a new narrow-phase contact
 void CollisionWorld::notifyNewContact(const BroadPhasePair* broadPhasePair,
-                                      const ContactInfo* contactInfo) {
+                                      const ContactPointInfo* contactInfo) {
 
     // TODO : Implement this method
 }
@@ -101,7 +101,7 @@ void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) {
     // Add the body ID to the list of free IDs
     mFreeBodiesIDs.push_back(collisionBody->getID());
 
-    // Call the constructor of the collision body
+    // Call the destructor of the collision body
     collisionBody->CollisionBody::~CollisionBody();
 
     // Remove the collision body from the list of bodies
@@ -181,6 +181,9 @@ void CollisionWorld::removeCollisionShape(CollisionShape* collisionShape) {
         // Remove the shape from the set of shapes in the world
         mCollisionShapes.remove(collisionShape);
 
+        // Call the destructor of the collision shape
+        collisionShape->CollisionShape::~CollisionShape();
+
         // Deallocate the memory used by the collision shape
         mMemoryAllocator.release(collisionShape, collisionShape->getSizeInBytes());
     }

src/engine/CollisionWorld.h

@@ -91,7 +91,7 @@ class CollisionWorld {
         virtual void notifyRemovedOverlappingPair(const BroadPhasePair* removedPair);
 
         /// Notify the world about a new narrow-phase contact
-        virtual void notifyNewContact(const BroadPhasePair* pair, const ContactInfo* contactInfo);
+        virtual void notifyNewContact(const BroadPhasePair* pair, const ContactPointInfo* contactInfo);
 
         /// Update the overlapping pair
         virtual void updateOverlappingPair(const BroadPhasePair* pair);

src/engine/ConstraintSolver.cpp

@@ -29,7 +29,13 @@
 using namespace reactphysics3d;
 
 // Constructor
-ConstraintSolver::ConstraintSolver() {
+ConstraintSolver::ConstraintSolver(std::set<Constraint*>& joints,
+                                   std::vector<Vector3>& constrainedLinearVelocities,
+                                   std::vector<Vector3>& constrainedAngularVelocities,
+                                   const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
+                 : mJoints(joints), mConstrainedLinearVelocities(constrainedLinearVelocities),
+                   mConstrainedAngularVelocities(constrainedAngularVelocities),
+                   mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex) {
 
 }
 

src/engine/ConstraintSolver.h

@@ -27,6 +27,11 @@
 #define REACTPHYSICS3D_CONSTRAINT_SOLVER_H
 
 // Libraries
+#include "../configuration.h"
+#include "mathematics/mathematics.h"
+#include "../constraint/Constraint.h"
+#include <map>
+#include <set>
 
 namespace reactphysics3d {
 
@@ -105,6 +110,20 @@ class ConstraintSolver {
 
         // -------------------- Attributes -------------------- //
 
+        /// Reference to all the joints of the world
+        std::set<Constraint*>& mJoints;
+
+        /// Reference to the array of constrained linear velocities (state of the linear velocities
+        /// after solving the constraints)
+        std::vector<Vector3>& mConstrainedLinearVelocities;
+
+        /// Reference to the array of constrained angular velocities (state of the angular velocities
+        /// after solving the constraints)
+        std::vector<Vector3>& mConstrainedAngularVelocities;
+
+        /// Reference to the map of rigid body to their index in the constrained velocities array
+        const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex;
+
         /// Number of iterations of the contact solver
         uint mNbIterations;
 
@@ -116,11 +135,13 @@ class ConstraintSolver {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ConstraintSolver();
+        ConstraintSolver(std::set<Constraint*>& joints,
+                         std::vector<Vector3>& constrainedLinearVelocities,
+                         std::vector<Vector3>& constrainedAngularVelocities,
+                         const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
 
         /// Destructor
         ~ConstraintSolver();
-
 };
 
 }

src/engine/ContactSolver.cpp

@@ -39,10 +39,12 @@ const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2);
 const decimal ContactSolver::SLOP = decimal(0.01);
 
 // Constructor
-ContactSolver::ContactSolver(DynamicsWorld& world,std::vector<Vector3>& constrainedLinearVelocities,
+ContactSolver::ContactSolver(std::vector<ContactManifold*>& contactManifolds,
+                             std::vector<Vector3>& constrainedLinearVelocities,
                              std::vector<Vector3>& constrainedAngularVelocities,
                              const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
-              :mWorld(world), mNbIterations(DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS),
+              :mContactManifolds(contactManifolds),
+               mNbIterations(DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS),
                mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL),
                mContactConstraints(NULL),
                mConstrainedLinearVelocities(constrainedLinearVelocities),
@@ -62,14 +64,13 @@ ContactSolver::~ContactSolver() {
 void ContactSolver::initialize() {
 
     // TODO : Use better memory allocation here
-    mContactConstraints = new ContactManifoldSolver[mWorld.getNbContactManifolds()];
+    mContactConstraints = new ContactManifoldSolver[mContactManifolds.size()];
 
     mNbContactManifolds = 0;
 
     // For each contact manifold of the world
     vector<ContactManifold*>::iterator it;
-    for (it = mWorld.getContactManifoldsBeginIterator();
+    for (it = mContactManifolds.begin(); it != mContactManifolds.end(); ++it) {
-         it != mWorld.getContactManifoldsEndIterator(); ++it) {
 
         ContactManifold* externalManifold = *it;
 
@@ -174,8 +175,8 @@ void ContactSolver::initialize() {
 
     // Allocated memory for split impulse velocities
     // TODO : Use better memory allocation here
-    mSplitLinearVelocities = new Vector3[mWorld.getNbRigidBodies()];
+    mSplitLinearVelocities = new Vector3[mMapBodyToConstrainedVelocityIndex.size()];
-    mSplitAngularVelocities = new Vector3[mWorld.getNbRigidBodies()];
+    mSplitAngularVelocities = new Vector3[mMapBodyToConstrainedVelocityIndex.size()];
     assert(mSplitLinearVelocities != NULL);
     assert(mSplitAngularVelocities != NULL);
 

src/engine/ContactSolver.h

@@ -342,8 +342,8 @@ class ContactSolver {
 
         // -------------------- Attributes -------------------- //
 
-        /// Reference to the world
+        /// Reference to all the contact manifold of the world
-        DynamicsWorld& mWorld;
+        std::vector<ContactManifold*>& mContactManifolds;
 
         /// Number of iterations of the contact solver
         uint mNbIterations;
@@ -366,11 +366,11 @@ class ContactSolver {
         /// Constrained bodies
         std::set<RigidBody*> mConstraintBodies;
 
-        /// Pointer to the array of constrained linear velocities (state of the linear velocities
+        /// Reference to the array of constrained linear velocities (state of the linear velocities
         /// after solving the constraints)
         std::vector<Vector3>& mConstrainedLinearVelocities;
 
-        /// Pointer to the array of constrained angular velocities (state of the angular velocities
+        /// Reference to the array of constrained angular velocities (state of the angular velocities
         /// after solving the constraints)
         std::vector<Vector3>& mConstrainedAngularVelocities;
 
@@ -452,7 +452,8 @@ class ContactSolver {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ContactSolver(DynamicsWorld& mWorld, std::vector<Vector3>& constrainedLinearVelocities,
+        ContactSolver(std::vector<ContactManifold*>& contactManifolds,
+                      std::vector<Vector3>& constrainedLinearVelocities,
                       std::vector<Vector3>& constrainedAngularVelocities,
                       const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
 

src/engine/DynamicsWorld.cpp

@@ -25,6 +25,7 @@
 
 // Libraries
 #include "DynamicsWorld.h"
+#include "constraint/BallAndSocketJoint.h"
 
 // Namespaces
 using namespace reactphysics3d;
@@ -33,8 +34,10 @@ using namespace std;
 // Constructor
 DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_TIMESTEP)
               : CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityOn(true),
-                mContactSolver(*this, mConstrainedLinearVelocities, mConstrainedAngularVelocities,
+                mContactSolver(mContactManifolds, mConstrainedLinearVelocities, mConstrainedAngularVelocities,
                                mMapBodyToConstrainedVelocityIndex),
+                mConstraintSolver(mJoints, mConstrainedLinearVelocities, mConstrainedAngularVelocities,
+                                  mMapBodyToConstrainedVelocityIndex),
                 mIsDeactivationActive(DEACTIVATION_ENABLED) {
 
 }
@@ -221,6 +224,8 @@ void DynamicsWorld::initConstrainedVelocitiesArray() {
 
         i++;
     }
+
+    assert(mMapBodyToConstrainedVelocityIndex.size() == mRigidBodies.size());
 }
 
 // Cleanup the constrained velocities array at each step
@@ -230,6 +235,9 @@ void DynamicsWorld::cleanupConstrainedVelocitiesArray() {
     mConstrainedLinearVelocities.clear();
     mConstrainedAngularVelocities.clear();
 
+    // Clear the constrained bodies
+    mConstrainedBodies.clear();
+
     // Clear the rigid body to velocities array index mapping
     mMapBodyToConstrainedVelocityIndex.clear();
 }
@@ -300,7 +308,7 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
     // Remove the collision shape from the world
     removeCollisionShape(rigidBody->getCollisionShape());
 
-    // Call the constructor of the rigid body
+    // Call the destructor of the rigid body
     rigidBody->RigidBody::~RigidBody();
 
     // Remove the rigid body from the list of rigid bodies
@@ -311,9 +319,51 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
     mMemoryAllocator.release(rigidBody, sizeof(RigidBody));
 }
 
-// Remove all constraints in the physics world
+// Create a joint between two bodies in the world and return a pointer to the new joint
-void DynamicsWorld::removeAllConstraints() {
+Constraint* DynamicsWorld::createJoint(const ConstraintInfo& jointInfo) {
-    mConstraints.clear();
+
+    Constraint* newJoint = NULL;
+
+    // Allocate memory to create the new joint
+    switch(jointInfo.type) {
+
+        // Ball-and-Socket joint
+        case BALLSOCKETJOINT:
+        {
+            void* allocatedMemory = mMemoryAllocator.allocate(sizeof(BallAndSocketJoint));
+            const BallAndSocketJointInfo& info = dynamic_cast<const BallAndSocketJointInfo&>(
+                                                                                        jointInfo);
+            newJoint = new (allocatedMemory) BallAndSocketJoint(info);
+            break;
+        }
+
+        default:
+        {
+            assert(false);
+            return NULL;
+        }
+    }
+
+    // Add the joint into the world
+    mJoints.insert(newJoint);
+
+    // Return the pointer to the created joint
+    return newJoint;
+}
+
+// Destroy a joint
+void DynamicsWorld::destroyJoint(Constraint* joint) {
+
+    assert(joint != NULL);
+
+    // Remove the joint from the world
+    mJoints.erase(joint);
+
+    // Call the destructor of the joint
+    joint->Constraint::~Constraint();
+
+    // Release the allocated memory
+    mMemoryAllocator.release(joint, joint->getSizeInBytes());
 }
 
 // Notify the world about a new broad-phase overlapping pair
@@ -345,19 +395,11 @@ void DynamicsWorld::notifyRemovedOverlappingPair(const BroadPhasePair* removedPa
 
 // Notify the world about a new narrow-phase contact
 void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair,
-                                     const ContactInfo* contactInfo) {
+                                     const ContactPointInfo* contactInfo) {
-
-    RigidBody* const rigidBody1 = dynamic_cast<RigidBody* const>(broadPhasePair->body1);
-    RigidBody* const rigidBody2 = dynamic_cast<RigidBody* const>(broadPhasePair->body2);
-
-    assert(rigidBody1 != NULL);
-    assert(rigidBody2 != NULL);
 
     // Create a new contact
     ContactPoint* contact = new (mMemoryAllocator.allocate(sizeof(ContactPoint))) ContactPoint(
-                                                                                    rigidBody1,
+                                                                                    *contactInfo);
-                                                                                    rigidBody2,
-                                                                                    contactInfo);
     assert(contact != NULL);
 
     // Get the corresponding overlapping pair

src/engine/DynamicsWorld.h

@@ -30,6 +30,7 @@
 #include "CollisionWorld.h"
 #include "../collision/CollisionDetection.h"
 #include "ContactSolver.h"
+#include "ConstraintSolver.h"
 #include "../body/RigidBody.h"
 #include "Timer.h"
 #include "../configuration.h"
@@ -55,6 +56,9 @@ class DynamicsWorld : public CollisionWorld {
         /// Contact solver
         ContactSolver mContactSolver;
 
+        /// Constraint solver
+        ConstraintSolver mConstraintSolver;
+
         /// True if the deactivation (sleeping) of inactive bodies is enabled
         bool mIsDeactivationActive;
 
@@ -64,8 +68,11 @@ class DynamicsWorld : public CollisionWorld {
         /// All the contact constraints
         std::vector<ContactManifold*> mContactManifolds;
 
-        /// All the constraints (except contact constraints)
+        /// All the joints of the world
-        std::vector<Constraint*> mConstraints;
+        std::set<Constraint*> mJoints;
+
+        /// All the bodies that are part of contacts or constraints
+        std::set<RigidBody*> mConstrainedBodies;
 
         /// Gravity vector of the world
         Vector3 mGravity;
@@ -124,7 +131,7 @@ class DynamicsWorld : public CollisionWorld {
         virtual void notifyRemovedOverlappingPair(const BroadPhasePair* removedPair);
 
         /// Notify the world about a new narrow-phase contact
-        virtual void notifyNewContact(const BroadPhasePair* pair, const ContactInfo* contactInfo);
+        virtual void notifyNewContact(const BroadPhasePair* pair, const ContactPointInfo* contactInfo);
 
 public :
 
@@ -166,6 +173,12 @@ public :
         /// Destroy a rigid body
         void destroyRigidBody(RigidBody* rigidBody);
 
+        /// Create a joint between two bodies in the world and return a pointer to the new joint
+        Constraint* createJoint(const ConstraintInfo& jointInfo);
+
+        /// Destroy a joint
+        void destroyJoint(Constraint* joint);
+
         /// Return the gravity vector of the world
         Vector3 getGravity() const;
 
@@ -178,30 +191,9 @@ public :
         /// Return the number of rigid bodies in the world
         uint getNbRigidBodies() const;
 
-        /// Add a constraint
-        void addConstraint(Constraint* constraint);
-
-        /// Remove a constraint
-        void removeConstraint(Constraint* constraint);
-
-        /// Remove all constraints and delete them (free their memory)
-        void removeAllConstraints();
-
         /// Return the number of contact constraints in the world
         uint getNbContactManifolds() const;
 
-        /// Return a start iterator on the constraint list
-        std::vector<Constraint*>::iterator getConstraintsBeginIterator();
-
-        /// Return a end iterator on the constraint list
-        std::vector<Constraint*>::iterator getConstraintsEndIterator();
-
-        /// Return a start iterator on the contact manifolds list
-        std::vector<ContactManifold*>::iterator getContactManifoldsBeginIterator();
-
-        /// Return a end iterator on the contact manifolds list
-        std::vector<ContactManifold*>::iterator getContactManifoldsEndIterator();
-
         /// Return an iterator to the beginning of the rigid bodies of the physics world
         std::set<RigidBody*>::iterator getRigidBodiesBeginIterator();
 
@@ -259,24 +251,6 @@ inline void DynamicsWorld::updateOverlappingPair(const BroadPhasePair* pair) {
     overlappingPair->update();
 }
 
-
-// Add a constraint into the physics world
-inline void DynamicsWorld::addConstraint(Constraint* constraint) {
-    assert(constraint != 0);
-    mConstraints.push_back(constraint);
-}
-
-// Remove a constraint and free its memory
-inline void DynamicsWorld::removeConstraint(Constraint* constraint) {
-    std::vector<Constraint*>::iterator it;
-
-    assert(constraint != NULL);
-    it = std::find(mConstraints.begin(), mConstraints.end(), constraint);
-    assert(*it == constraint);
-    delete *it;
-    mConstraints.erase(it);
-}
-
 // Return the gravity vector of the world
 inline Vector3 DynamicsWorld::getGravity() const {
     return mGravity;
@@ -312,26 +286,6 @@ inline uint DynamicsWorld::getNbContactManifolds() const {
     return mContactManifolds.size();
 }
 
-// Return a start iterator on the constraint list
-inline std::vector<Constraint*>::iterator DynamicsWorld::getConstraintsBeginIterator() {
-    return mConstraints.begin();
-}
-
-// Return a end iterator on the constraint list
-inline std::vector<Constraint*>::iterator DynamicsWorld::getConstraintsEndIterator() {
-    return mConstraints.end();
-}
-
-// Return a start iterator on the contact manifolds list
-inline std::vector<ContactManifold*>::iterator DynamicsWorld::getContactManifoldsBeginIterator() {
-    return mContactManifolds.begin();
-}
-
-// Return a end iterator on the contact manifolds list
-inline std::vector<ContactManifold*>::iterator DynamicsWorld::getContactManifoldsEndIterator() {
-    return mContactManifolds.end();
-}
-
 }
 
 #endif