diff --git a/src/collision/broadphase/PairManager.h b/src/collision/broadphase/PairManager.h
index 860ab19a..336df403 100644
--- a/src/collision/broadphase/PairManager.h
+++ b/src/collision/broadphase/PairManager.h
@@ -127,9 +127,6 @@ class PairManager {
         /// This method returns an hash value for a 32 bits key.
         int computeHash32Bits(int key) const;
 
-        /// Return the next power of two
-        luint computeNextPowerOfTwo(luint number) const;
-
         /// Reallocate memory for more pairs
         void reallocatePairs();
 
@@ -171,6 +168,9 @@ class PairManager {
         /// Find a pair given two body IDs
         BodyPair* findPair(bodyindex id1, bodyindex id2) const;
 
+        /// Return the next power of two
+        static luint computeNextPowerOfTwo(luint number);
+
         /// Return a pointer to the first overlapping pair (used to
         /// iterate over the active pairs).
         BodyPair* beginOverlappingPairsPointer() const;
@@ -215,7 +215,7 @@ inline bool PairManager::isDifferentPair(const BodyPair& pair1, bodyindex pair2I
 }
 
 // Return the next power of two of a 32bits integer using a SWAR algorithm
-inline luint PairManager::computeNextPowerOfTwo(luint number) const {
+inline luint PairManager::computeNextPowerOfTwo(luint number) {
     number |= (number >> 1);
     number |= (number >> 2);
     number |= (number >> 4);
diff --git a/src/collision/broadphase/SweepAndPruneAlgorithm.cpp b/src/collision/broadphase/SweepAndPruneAlgorithm.cpp
index 1c363d7a..142ec56f 100644
--- a/src/collision/broadphase/SweepAndPruneAlgorithm.cpp
+++ b/src/collision/broadphase/SweepAndPruneAlgorithm.cpp
@@ -183,6 +183,12 @@ void SweepAndPruneAlgorithm::removeObject(CollisionBody* body) {
 
     mMapBodyToBoxIndex.erase(body);
     mNbBoxes--;
+
+    // Check if we need to shrink the allocated memory
+    const luint nextPowerOf2 = PairManager::computeNextPowerOfTwo((mNbBoxes-1) / 100 );
+    if (nextPowerOf2 * 100 < mNbMaxBoxes) {
+        shrinkArrays();
+    }
 } 
         
 // Notify the broad-phase that the AABB of an object has changed.
@@ -492,3 +498,99 @@ void SweepAndPruneAlgorithm::resizeArrays() {
     
     mNbMaxBoxes = newNbMaxBoxes;
 }
+
+// Shrink the boxes and end-points arrays when too much memory is allocated
+void SweepAndPruneAlgorithm::shrinkArrays() {
+
+    // New number of boxes and end-points in the array
+    const luint nextPowerOf2 = PairManager::computeNextPowerOfTwo((mNbBoxes-1) / 100 );
+    const luint newNbMaxBoxes = (mNbBoxes > 100) ? nextPowerOf2 * 100 : 100;
+    const luint nbEndPoints = mNbBoxes * 2 + NB_SENTINELS;
+    const luint newNbEndPoints = newNbMaxBoxes * 2 + NB_SENTINELS;
+
+    assert(newNbMaxBoxes < mNbMaxBoxes);
+
+    // Sort the list of the free boxes indices in ascending order
+    mFreeBoxIndices.sort();
+
+    // Reorganize the boxes inside the boxes array so that all the boxes are at the
+    // beginning of the array
+    std::map<CollisionBody*, bodyindex> newMapBodyToBoxIndex;
+    std::map<CollisionBody*,bodyindex>::const_iterator it;
+    for (it = mMapBodyToBoxIndex.begin(); it != mMapBodyToBoxIndex.end(); ++it) {
+
+        CollisionBody* body = it->first;
+        bodyindex boxIndex = it->second;
+
+        // If the box index is outside the range of the current number of boxes
+        if (boxIndex >= mNbBoxes) {
+
+            assert(!mFreeBoxIndices.empty());
+
+            // Get a new box index for that body (from the list of free box indices)
+            bodyindex newBoxIndex = mFreeBoxIndices.front();
+            mFreeBoxIndices.pop_front();
+            assert(newBoxIndex < mNbBoxes);
+
+            // Copy the box to its new location in the boxes array
+            BoxAABB* oldBox = &mBoxes[boxIndex];
+            BoxAABB* newBox = &mBoxes[newBoxIndex];
+            assert(oldBox->body->getID() == body->getID());
+            newBox->body = oldBox->body;
+            for (uint axis=0; axis<3; axis++) {
+
+                // Copy the minimum and maximum end-points indices
+                newBox->min[axis] = oldBox->min[axis];
+                newBox->max[axis] = oldBox->max[axis];
+
+                // Update the box index of the end-points
+                EndPoint* minimumEndPoint = &mEndPoints[axis][newBox->min[axis]];
+                EndPoint* maximumEndPoint = &mEndPoints[axis][newBox->max[axis]];
+                assert(minimumEndPoint->boxID == boxIndex);
+                assert(maximumEndPoint->boxID == boxIndex);
+                minimumEndPoint->boxID = newBoxIndex;
+                maximumEndPoint->boxID = newBoxIndex;
+            }
+
+            newMapBodyToBoxIndex.insert(pair<CollisionBody*, bodyindex>(body, newBoxIndex));
+        }
+        else {
+            newMapBodyToBoxIndex.insert(pair<CollisionBody*, bodyindex>(body, boxIndex));
+        }
+    }
+
+    assert(newMapBodyToBoxIndex.size() == mMapBodyToBoxIndex.size());
+    mMapBodyToBoxIndex = newMapBodyToBoxIndex;
+
+    // Allocate memory for the new boxes and end-points arrays
+    BoxAABB* newBoxesArray = new BoxAABB[newNbMaxBoxes];
+    EndPoint* newEndPointsXArray = new EndPoint[newNbEndPoints];
+    EndPoint* newEndPointsYArray = new EndPoint[newNbEndPoints];
+    EndPoint* newEndPointsZArray = new EndPoint[newNbEndPoints];
+
+    assert(newBoxesArray != NULL);
+    assert(newEndPointsXArray != NULL);
+    assert(newEndPointsYArray != NULL);
+    assert(newEndPointsZArray != NULL);
+
+    // Copy the data from the old arrays into the new one
+    memcpy(newBoxesArray, mBoxes, sizeof(BoxAABB) * mNbBoxes);
+    const size_t nbBytesNewEndPoints = sizeof(EndPoint) * nbEndPoints;
+    memcpy(newEndPointsXArray, mEndPoints[0], nbBytesNewEndPoints);
+    memcpy(newEndPointsYArray, mEndPoints[1], nbBytesNewEndPoints);
+    memcpy(newEndPointsZArray, mEndPoints[2], nbBytesNewEndPoints);
+
+    // Delete the old arrays
+    delete[] mBoxes;
+    delete[] mEndPoints[0];
+    delete[] mEndPoints[1];
+    delete[] mEndPoints[2];
+
+    // Assign the pointer to the new arrays
+    mBoxes = newBoxesArray;
+    mEndPoints[0] = newEndPointsXArray;
+    mEndPoints[1] = newEndPointsYArray;
+    mEndPoints[2] = newEndPointsZArray;
+
+    mNbMaxBoxes = newNbMaxBoxes;
+}
diff --git a/src/collision/broadphase/SweepAndPruneAlgorithm.h b/src/collision/broadphase/SweepAndPruneAlgorithm.h
index 2c72152b..a4e2594b 100644
--- a/src/collision/broadphase/SweepAndPruneAlgorithm.h
+++ b/src/collision/broadphase/SweepAndPruneAlgorithm.h
@@ -31,6 +31,7 @@
 #include "../../collision/shapes/AABB.h"
 #include <map>
 #include <vector>
+#include <list>
 
 
 /// Namespace ReactPhysics3D
@@ -135,7 +136,7 @@ class SweepAndPruneAlgorithm : public BroadPhaseAlgorithm {
         bodyindex mNbMaxBoxes;
 
         /// Indices that are not used by any boxes
-        std::vector<bodyindex> mFreeBoxIndices;
+        std::list<bodyindex> mFreeBoxIndices;
 
         /// Map a body pointer to a box index
         std::map<CollisionBody*,bodyindex> mMapBodyToBoxIndex;
@@ -151,6 +152,9 @@ class SweepAndPruneAlgorithm : public BroadPhaseAlgorithm {
         /// Resize the boxes and end-points arrays when it's full
         void resizeArrays();
 
+        /// Shrink the boxes and end-points arrays when too much memory is allocated
+        void shrinkArrays();
+
         /// Add an overlapping pair of AABBS
         void addPair(CollisionBody* body1, CollisionBody* body2);