Allocate memory for the collision shapes inside the physics engine

examples/common/Box.cpp

@@ -76,11 +76,13 @@ Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &p
     translateWorld(position);
 
     // Create the collision shape for the rigid body (box shape)
-    mCollisionShape = new rp3d::BoxShape(rp3d::Vector3(mSize[0], mSize[1], mSize[2]));
+    // ReactPhysics3D will clone this object to create an internal one. Therefore,
+    // it is OK if this object is destroy right after calling Dynamics::createRigidBody()
+    const rp3d::BoxShape collisionShape(rp3d::Vector3(mSize[0], mSize[1], mSize[2]));
 
     // Compute the inertia tensor of the body using its collision shape
     rp3d::Matrix3x3 inertiaTensor;
-    mCollisionShape->computeLocalInertiaTensor(inertiaTensor, mass);
+    collisionShape.computeLocalInertiaTensor(inertiaTensor, mass);
 
     // Initial position and orientation of the rigid body
     rp3d::Vector3 initPosition(position.x, position.y, position.z);
@@ -88,7 +90,7 @@ Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &p
     rp3d::Transform transform(initPosition, initOrientation);
 
     // Create a rigid body corresponding to the cube in the dynamics world
-    mRigidBody = dynamicsWorld->createRigidBody(transform, mass, inertiaTensor, mCollisionShape);
+    mRigidBody = dynamicsWorld->createRigidBody(transform, mass, inertiaTensor, collisionShape);
 
     // If the Vertex Buffer object has not been created yet
     if (!areVBOsCreated) {
@@ -100,8 +102,6 @@ Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &p
 // Destructor
 Box::~Box() {
 
-    // Destroy the collision shape
-    delete mCollisionShape;
 }
 
 // Render the cube at the correct position and with the correct orientation

examples/common/Box.h

@@ -56,9 +56,6 @@ class Box : public openglframework::Object3D {
         /// Rigid body used to simulate the dynamics of the box
         rp3d::RigidBody* mRigidBody;
 
-        /// Collision shape of the rigid body
-        rp3d::BoxShape* mCollisionShape;
-
         /// Scaling matrix (applied to a cube to obtain the correct box dimensions)
         openglframework::Matrix4 mScalingMatrix;
 

src/collision/shapes/BoxShape.cpp

@@ -50,6 +50,11 @@ BoxShape::BoxShape(const Vector3& extent) : CollisionShape(BOX), mExtent(extent)
 
 }
 
+// Private copy-constructor
+BoxShape::BoxShape(const BoxShape& shape) : CollisionShape(shape), mExtent(shape.mExtent) {
+
+}
+
 // Destructor
 BoxShape::~BoxShape() {
 

src/collision/shapes/BoxShape.h

@@ -69,6 +69,9 @@ class BoxShape : public CollisionShape {
         /// Destructor
         virtual ~BoxShape();
 
+        /// Allocate and return a copy of the object
+        virtual BoxShape* clone(void* allocatedMemory) const;
+
         /// Return the extents of the box
         const Vector3& getExtent() const;
 
@@ -81,6 +84,9 @@ class BoxShape : public CollisionShape {
         /// Return the margin distance around the shape
         virtual decimal getMargin() const;
 
+        /// Return the number of bytes used by the collision shape
+        virtual size_t getSizeInBytes() const;
+
         /// Return a local support point in a given direction with the object margin
         virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction) const;
 
@@ -90,12 +96,20 @@ class BoxShape : public CollisionShape {
         /// Return the local inertia tensor of the collision shape
         virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
 
+        /// Test equality between two box shapes
+        virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const;
+
 #ifdef VISUAL_DEBUG
         /// Draw the Box (only for testing purpose)
         virtual void draw() const;
 #endif
 };
 
+// Allocate and return a copy of the object
+inline BoxShape* BoxShape::clone(void* allocatedMemory) const {
+    return new (allocatedMemory) BoxShape(*this);
+}
+
 // Return the extents of the box
 inline const Vector3& BoxShape::getExtent() const {
     return mExtent;
@@ -117,6 +131,11 @@ inline decimal BoxShape::getMargin() const {
     return OBJECT_MARGIN;
 }
 
+// Return the number of bytes used by the collision shape
+inline size_t BoxShape::getSizeInBytes() const {
+    return sizeof(BoxShape);
+}
+
 // Return a local support point in a given direction with the object margin
 inline Vector3 BoxShape::getLocalSupportPointWithMargin(const Vector3& direction) const {
 
@@ -136,6 +155,12 @@ inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direct
                    direction.z < 0.0 ? -mExtent.z : mExtent.z);
 }
 
+// Test equality between two box shapes
+inline bool BoxShape::isEqualTo(const CollisionShape& otherCollisionShape) const {
+    const BoxShape& otherShape = dynamic_cast<const BoxShape&>(otherCollisionShape);
+    return (mExtent == otherShape.mExtent);
+}
+
 }
 
 #endif

src/collision/shapes/CollisionShape.cpp

@@ -30,13 +30,20 @@
 using namespace reactphysics3d;
 
 // Constructor
-CollisionShape::CollisionShape(CollisionShapeType type) : mType(type) {
+CollisionShape::CollisionShape(CollisionShapeType type)
+               : mType(type), mNbSimilarCreatedShapes(0) {
     
 }
 
+// Private copy-constructor
+CollisionShape::CollisionShape(const CollisionShape& shape)
+               : mType(shape.mType), mNbSimilarCreatedShapes(shape.mNbSimilarCreatedShapes){
+
+}
+
 // Destructor
 CollisionShape::~CollisionShape() {
-
+    assert(mNbSimilarCreatedShapes == 0);
 }
 
 // Update the AABB of a body using its collision shape

src/collision/shapes/CollisionShape.h

@@ -54,6 +54,9 @@ class CollisionShape {
 
         /// Type of the collision shape
         CollisionShapeType mType;
+
+        /// Current number of similar created shapes
+        uint mNbSimilarCreatedShapes;
         
         // -------------------- Methods -------------------- //
 
@@ -73,9 +76,18 @@ class CollisionShape {
         /// Destructor
         virtual ~CollisionShape();
 
+        /// Allocate and return a copy of the object
+        virtual CollisionShape* clone(void* allocatedMemory) const=0;
+
         /// Return the type of the collision shapes
         CollisionShapeType getType() const;
 
+        /// Return the number of similar created shapes
+        uint getNbSimilarCreatedShapes() const;
+
+        /// Return the number of bytes used by the collision shape
+        virtual size_t getSizeInBytes() const = 0;
+
         /// Return a local support point in a given direction with the object margin
         virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction) const=0;
 
@@ -93,6 +105,18 @@ class CollisionShape {
 
         /// Update the AABB of a body using its collision shape
         virtual void updateAABB(AABB& aabb, const Transform& transform);
+
+        /// Increment the number of similar allocated collision shapes
+        void incrementNbSimilarCreatedShapes();
+
+        /// Decrement the number of similar allocated collision shapes
+        void decrementNbSimilarCreatedShapes();
+
+        /// Equality operator between two collision shapes.
+        bool operator==(const CollisionShape& otherCollisionShape) const;
+
+        /// Test equality between two collision shapes of the same type (same derived classes).
+        virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const=0;
 };
 
 // Return the type of the collision shape
@@ -100,6 +124,36 @@ inline CollisionShapeType CollisionShape::getType() const {
     return mType;
 }
 
+// Return the number of similar created shapes
+inline uint CollisionShape::getNbSimilarCreatedShapes() const {
+    return mNbSimilarCreatedShapes;
+}
+
+// Increment the number of similar allocated collision shapes
+inline void CollisionShape::incrementNbSimilarCreatedShapes() {
+    mNbSimilarCreatedShapes++;
+}
+
+// Decrement the number of similar allocated collision shapes
+inline void CollisionShape::decrementNbSimilarCreatedShapes() {
+    mNbSimilarCreatedShapes--;
+}
+
+// Equality operator between two collision shapes.
+/// This methods returns true only if the two collision shapes are of the same type and
+/// of the same dimensions.
+inline bool CollisionShape::operator==(const CollisionShape& otherCollisionShape) const {
+
+    // If the two collisions shapes are not of the same type (same derived classes)
+    // we return false
+    if (mType != otherCollisionShape.mType) return false;
+
+    assert(typeid(*this) == typeid(otherCollisionShape));
+
+    // Check if the two shapes are equal
+    return otherCollisionShape.isEqualTo(*this);
+}
+
 }
 
 #endif

src/collision/shapes/ConeShape.cpp

@@ -53,6 +53,13 @@ ConeShape::ConeShape(decimal radius, decimal height)
     mSinTheta = radius / (sqrt(radius * radius + height * height));
 }
 
+// Private copy-constructor
+ConeShape::ConeShape(const ConeShape& shape)
+          : CollisionShape(shape), mRadius(shape.mRadius), mHalfHeight(shape.mHalfHeight),
+            mSinTheta(shape.mSinTheta){
+
+}
+
 // Destructor
 ConeShape::~ConeShape() {
 

src/collision/shapes/ConeShape.h

@@ -75,6 +75,9 @@ class ConeShape : public CollisionShape {
         /// Destructor
         virtual ~ConeShape();
 
+        /// Allocate and return a copy of the object
+        virtual ConeShape* clone(void* allocatedMemory) const;
+
         /// Return the radius
         decimal getRadius() const;
 
@@ -87,6 +90,9 @@ class ConeShape : public CollisionShape {
         /// Set the height
         void setHeight(decimal height);
 
+        /// Return the number of bytes used by the collision shape
+        virtual size_t getSizeInBytes() const;
+
         /// Return a local support point in a given direction with the object margin
         virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction) const;
 
@@ -102,12 +108,20 @@ class ConeShape : public CollisionShape {
         /// Return the margin distance around the shape
         virtual decimal getMargin() const;
 
+        /// Test equality between two cone shapes
+        virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const;
+
 #ifdef VISUAL_DEBUG
         /// Draw the sphere (only for testing purpose)
         virtual void draw() const;
 #endif
 };
 
+// Allocate and return a copy of the object
+inline ConeShape* ConeShape::clone(void* allocatedMemory) const {
+    return new (allocatedMemory) ConeShape(*this);
+}
+
 // Return the radius
 inline decimal ConeShape::getRadius() const {
     return mRadius;
@@ -134,6 +148,11 @@ inline void ConeShape::setHeight(decimal height) {
     mSinTheta = mRadius / (sqrt(mRadius * mRadius + height * height));
 }
 
+// Return the number of bytes used by the collision shape
+inline size_t ConeShape::getSizeInBytes() const {
+    return sizeof(ConeShape);
+}
+
 // Return the local extents in x,y and z direction
 inline Vector3 ConeShape::getLocalExtents(decimal margin) const {
     return Vector3(mRadius + margin, mHalfHeight + margin, mRadius + margin);
@@ -153,6 +172,12 @@ inline decimal ConeShape::getMargin() const {
     return OBJECT_MARGIN;
 }
 
+// Test equality between two cone shapes
+inline bool ConeShape::isEqualTo(const CollisionShape& otherCollisionShape) const {
+    const ConeShape& otherShape = dynamic_cast<const ConeShape&>(otherCollisionShape);
+    return (mRadius == otherShape.mRadius && mHalfHeight == otherShape.mHalfHeight);
+}
+
 }
 
 #endif

src/collision/shapes/CylinderShape.cpp

@@ -48,6 +48,12 @@ CylinderShape::CylinderShape(decimal radius, decimal height)
 
 }
 
+// Private copy-constructor
+CylinderShape::CylinderShape(const CylinderShape& shape)
+              : CollisionShape(shape), mRadius(shape.mRadius), mHalfHeight(shape.mHalfHeight) {
+
+}
+
 // Destructor
 CylinderShape::~CylinderShape() {
 

src/collision/shapes/CylinderShape.h

@@ -71,6 +71,9 @@ class CylinderShape : public CollisionShape {
         /// Destructor
         virtual ~CylinderShape();
 
+        /// Allocate and return a copy of the object
+        virtual CylinderShape* clone(void* allocatedMemory) const;
+
         /// Return the radius
         decimal getRadius() const;
 
@@ -83,6 +86,9 @@ class CylinderShape : public CollisionShape {
         /// Set the height
         void setHeight(decimal height);
 
+        /// Return the number of bytes used by the collision shape
+        virtual size_t getSizeInBytes() const;
+
         /// Return a local support point in a given direction with the object margin
         virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction) const;
 
@@ -98,12 +104,20 @@ class CylinderShape : public CollisionShape {
         /// Return the margin distance around the shape
         virtual decimal getMargin() const;
 
+        /// Test equality between two cylinder shapes
+        virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const;
+
 #ifdef VISUAL_DEBUG
         /// Draw the sphere (only for testing purpose)
         virtual void draw() const;
 #endif
 };
 
+/// Allocate and return a copy of the object
+inline CylinderShape* CylinderShape::clone(void* allocatedMemory) const {
+    return new (allocatedMemory) CylinderShape(*this);
+}
+
 // Return the radius
 inline decimal CylinderShape::getRadius() const {
     return mRadius;
@@ -124,6 +138,11 @@ inline void CylinderShape::setHeight(decimal height) {
     mHalfHeight = height * decimal(0.5);
 }
 
+// Return the number of bytes used by the collision shape
+inline size_t CylinderShape::getSizeInBytes() const {
+    return sizeof(CylinderShape);
+}
+
 // Return the local extents in x,y and z direction
 inline Vector3 CylinderShape::getLocalExtents(decimal margin) const {
     return Vector3(mRadius + margin, mHalfHeight + margin, mRadius + margin);
@@ -143,6 +162,12 @@ inline decimal CylinderShape::getMargin() const {
    return OBJECT_MARGIN;
 }
 
+// Test equality between two cylinder shapes
+inline bool CylinderShape::isEqualTo(const CollisionShape& otherCollisionShape) const {
+    const CylinderShape& otherShape = dynamic_cast<const CylinderShape&>(otherCollisionShape);
+    return (mRadius == otherShape.mRadius && mHalfHeight == otherShape.mHalfHeight);
+}
+
 }
 
 #endif

src/collision/shapes/SphereShape.cpp

@@ -45,7 +45,13 @@ using namespace reactphysics3d;
 using namespace std;
 
 // Constructor
-SphereShape::SphereShape(decimal radius) : CollisionShape(SPHERE), mRadius(radius) {
+SphereShape::SphereShape(decimal radius): CollisionShape(SPHERE), mRadius(radius) {
+
+}
+
+// Private copy-constructor
+SphereShape::SphereShape(const SphereShape& shape)
+            : CollisionShape(shape), mRadius(shape.mRadius) {
 
 }
 

src/collision/shapes/SphereShape.h

@@ -65,12 +65,18 @@ class SphereShape : public CollisionShape {
         /// Destructor
         virtual ~SphereShape();
 
+        /// Allocate and return a copy of the object
+        virtual SphereShape* clone(void* allocatedMemory) const;
+
         /// Return the radius of the sphere
         decimal getRadius() const;
 
         /// Set the radius of the sphere
         void setRadius(decimal radius);
 
+        /// Return the number of bytes used by the collision shape
+        virtual size_t getSizeInBytes() const;
+
         /// Return a local support point in a given direction with the object margin
         virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction) const;
 
@@ -89,12 +95,20 @@ class SphereShape : public CollisionShape {
         /// Update the AABB of a body using its collision shape
         virtual void updateAABB(AABB& aabb, const Transform& transform);
 
+        /// Test equality between two sphere shapes
+        virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const;
+
 #ifdef VISUAL_DEBUG
         /// Draw the sphere (only for testing purpose)
         virtual void draw() const;
 #endif
 };
 
+/// Allocate and return a copy of the object
+inline SphereShape* SphereShape::clone(void* allocatedMemory) const {
+    return new (allocatedMemory) SphereShape(*this);
+}
+
 // Get the radius of the sphere
 inline decimal SphereShape::getRadius() const {
     return mRadius;
@@ -105,6 +119,11 @@ inline void SphereShape::setRadius(decimal radius) {
     mRadius = radius;
 }
 
+// Return the number of bytes used by the collision shape
+inline size_t SphereShape::getSizeInBytes() const {
+    return sizeof(SphereShape);
+}
+
 // Return a local support point in a given direction with the object margin
 inline Vector3 SphereShape::getLocalSupportPointWithMargin(const Vector3& direction) const {
 
@@ -163,6 +182,12 @@ inline void SphereShape::updateAABB(AABB& aabb, const Transform& transform) {
     aabb.setMax(maxCoordinates);
 }
 
+// Test equality between two sphere shapes
+inline bool SphereShape::isEqualTo(const CollisionShape& otherCollisionShape) const {
+    const SphereShape& otherShape = dynamic_cast<const SphereShape&>(otherCollisionShape);
+    return (mRadius == otherShape.mRadius);
+}
+
 }
 
 #endif

src/engine/CollisionWorld.cpp

@@ -38,7 +38,8 @@ CollisionWorld::CollisionWorld()
 
 // Destructor
 CollisionWorld::~CollisionWorld() {
-
+    assert(mCollisionShapes.empty());
+    assert(mBodies.empty());
 }
 
 // Notify the world about a new broad-phase overlapping pair
@@ -104,7 +105,7 @@ void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) {
     collisionBody->CollisionBody::~CollisionBody();
 
     // Remove the collision body from the list of bodies
-    mBodies.erase(collisionBody);           // TODO : Maybe use a set to make this faster
+    mBodies.erase(collisionBody);
 
     // Free the object from the memory allocator
     mMemoryAllocator.release(collisionBody, sizeof(CollisionBody));
@@ -127,4 +128,62 @@ bodyindex CollisionWorld::computeNextAvailableBodyID() {
     return bodyID;
 }
 
+// Create a new collision shape.
+/// First, this methods checks that the new collision shape does not exist yet in the
+/// world. If it already exists, we do not allocate memory for a new one but instead
+/// we reuse the existing one. The goal is to only allocate memory for a single
+/// collision shape if this one is used for several bodies in the world. To allocate
+/// memory for a new collision shape, we use the memory allocator.
+CollisionShape* CollisionWorld::createCollisionShape(const CollisionShape& collisionShape) {
+
+    // Check if there is already a similar collision shape in the world
+    std::list<CollisionShape*>::iterator it;
+    for (it = mCollisionShapes.begin(); it != mCollisionShapes.end(); ++it) {
+
+        if (collisionShape == (*(*it))) {
+
+            // Increment the number of similar created shapes
+            (*it)->incrementNbSimilarCreatedShapes();
+
+            // A similar collision shape already exists in the world, so we do not
+            // create a new one but we simply return a pointer to the existing one
+            return (*it);
+        }
+    }
+
+    // A similar collision shape does not already exist in the world, so we create a
+    // new one and add it to the world
+    void* allocatedMemory = mMemoryAllocator.allocate(collisionShape.getSizeInBytes());
+    CollisionShape* newCollisionShape = collisionShape.clone(allocatedMemory);
+    mCollisionShapes.push_back(newCollisionShape);
+
+    newCollisionShape->incrementNbSimilarCreatedShapes();
+
+    // Return a pointer to the new collision shape
+    return newCollisionShape;
+}
+
+
+// Remove a collision shape.
+/// First, we check if another body is still using the same collision shape. If so,
+/// we keep the allocated collision shape. If it is not the case, we can deallocate
+/// the memory associated with the collision shape.
+void CollisionWorld::removeCollisionShape(CollisionShape* collisionShape) {
+
+    assert(collisionShape->getNbSimilarCreatedShapes() != 0);
+
+    // Decrement the number of bodies using the same collision shape
+    collisionShape->decrementNbSimilarCreatedShapes();
+
+    // If no other body is using the collision shape in the world
+    if (collisionShape->getNbSimilarCreatedShapes() == 0) {
+
+        // Remove the shape from the set of shapes in the world
+        mCollisionShapes.remove(collisionShape);
+
+        // Deallocate the memory used by the collision shape
+        mMemoryAllocator.release(collisionShape, collisionShape->getSizeInBytes());
+    }
+}
+
 

src/engine/CollisionWorld.h

@@ -29,6 +29,7 @@
 // Libraries
 #include <vector>
 #include <set>
+#include <list>
 #include <algorithm>
 #include "../mathematics/mathematics.h"
 #include "Profiler.h"
@@ -60,6 +61,9 @@ class CollisionWorld {
         /// All the bodies (rigid and soft) of the world
         std::set<CollisionBody*> mBodies;
 
+        /// All the collision shapes of the world
+        std::list<CollisionShape*> mCollisionShapes;
+
         /// Broad-phase overlapping pairs of bodies
         std::map<bodyindexpair, OverlappingPair*>  mOverlappingPairs;
 
@@ -95,9 +99,15 @@ class CollisionWorld {
         /// Return the next available body ID
         bodyindex computeNextAvailableBodyID();
 
+        /// Create a new collision shape.
+        CollisionShape* createCollisionShape(const CollisionShape& collisionShape);
+
+        /// Remove a collision shape.
+        void removeCollisionShape(CollisionShape* collisionShape);
+
     public :
 
-        // ----- Methods ----- //
+        // -------------------- Methods -------------------- //
 
         /// Constructor
         CollisionWorld();
@@ -118,7 +128,7 @@ class CollisionWorld {
         /// Destroy a collision body
         void destroyCollisionBody(CollisionBody* collisionBody);
 
-        // ----- Friends ----- //
+        // -------------------- Friends -------------------- //
 
         friend class CollisionDetection;
 };

src/engine/DynamicsWorld.cpp

@@ -258,7 +258,7 @@ void DynamicsWorld::applyGravity() {
 // Create a rigid body into the physics world
 RigidBody* DynamicsWorld::createRigidBody(const Transform& transform, decimal mass,
                                           const Matrix3x3& inertiaTensorLocal,
-                                          CollisionShape* collisionShape) {
+                                          const CollisionShape& collisionShape) {
 
     // Compute the body ID
     bodyindex bodyID = computeNextAvailableBodyID();
@@ -266,11 +266,14 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform, decimal ma
     // Largest index cannot be used (it is used for invalid index)
     assert(bodyID < std::numeric_limits<reactphysics3d::bodyindex>::max());
 
+    // Create a collision shape for the rigid body into the world
+    CollisionShape* newCollisionShape = createCollisionShape(collisionShape);
+
     // Create the rigid body
     RigidBody* rigidBody = new (mMemoryAllocator.allocate(sizeof(RigidBody))) RigidBody(transform,
                                                                                 mass,
                                                                                 inertiaTensorLocal,
-                                                                                collisionShape,
+                                                                                newCollisionShape,
                                                                                 bodyID);
     assert(rigidBody != NULL);
 
@@ -294,6 +297,9 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
     // Add the body ID to the list of free IDs
     mFreeBodiesIDs.push_back(rigidBody->getID());
 
+    // Remove the collision shape from the world
+    removeCollisionShape(rigidBody->getCollisionShape());
+
     // Call the constructor of the rigid body
     rigidBody->RigidBody::~RigidBody();
 

src/engine/DynamicsWorld.h

@@ -158,10 +158,10 @@ public :
         /// Set the isErrorCorrectionActive value
         void setIsErrorCorrectionActive(bool isErrorCorrectionActive);
 
-        /// Create a rigid body into the physics world
+        /// Create a rigid body into the physics world.
         RigidBody* createRigidBody(const Transform& transform, decimal mass,
                                    const Matrix3x3& inertiaTensorLocal,
-                                   CollisionShape* collisionShape);
+                                   const CollisionShape& collisionShape);
 
         /// Destroy a rigid body
         void destroyRigidBody(RigidBody* rigidBody);