Modifications of allocators

include/reactphysics3d/memory/HeapAllocator.h

@@ -57,26 +57,33 @@ class HeapAllocator : public MemoryAllocator {
 
                 // -------------------- Attributes -------------------- //
 
-                /// Size in bytes of the allocated memory unit
-                size_t size;
-
-                /// True if the memory unit is currently allocated
-                bool isAllocated;
-
                 /// Pointer to the previous memory unit
                 MemoryUnitHeader* previousUnit;
 
                 /// Pointer to the next memory unit
                 MemoryUnitHeader* nextUnit;
 
+                /// Pointer to the previous free (not allocated) memory unit
+                MemoryUnitHeader* previousFreeUnit;
+
+                /// Pointer to the next free (not allocated) memory unit
+                MemoryUnitHeader* nextFreeUnit;
+
+                /// Size in bytes of the allocated memory unit
+                size_t size;
+
                 /// True if the next memory unit has been allocated with the same call to malloc()
                 bool isNextContiguousMemory;
 
+                /// True if the memory unit is currently allocated
+                bool isAllocated = false;
+
                 // -------------------- Methods -------------------- //
 
-                MemoryUnitHeader(size_t size, MemoryUnitHeader* previousUnit, MemoryUnitHeader* nextUnit, bool isNextContiguousMemory)
+                MemoryUnitHeader(size_t size, MemoryUnitHeader* previousUnit, MemoryUnitHeader* nextUnit,
-                    : size(size), isAllocated(false), previousUnit(previousUnit),
+                                MemoryUnitHeader* previousFreeUnit,  MemoryUnitHeader* nextFreeUnit, bool isNextContiguousMemory)
-                      nextUnit(nextUnit), isNextContiguousMemory(isNextContiguousMemory) {
+                    : previousUnit(previousUnit), nextUnit(nextUnit), previousFreeUnit(previousFreeUnit), nextFreeUnit(nextFreeUnit), size(size),
+                      isNextContiguousMemory(isNextContiguousMemory) {
 
                     assert(size > 0);
                 }
@@ -101,8 +108,8 @@ class HeapAllocator : public MemoryAllocator {
         /// Pointer to the first memory unit of the linked-list
         MemoryUnitHeader* mMemoryUnits;
 
-        /// Pointer to a cached free memory unit
+        /// Pointer to the first item of the linked-list of free units
-        MemoryUnitHeader* mCachedFreeUnit;
+        MemoryUnitHeader* mFreeUnits;
 
 #ifndef NDEBUG
         /// This variable is incremented by one when the allocate() method has been
@@ -118,12 +125,21 @@ class HeapAllocator : public MemoryAllocator {
         /// left over space. The second unit is put into the free memory units
         void splitMemoryUnit(MemoryUnitHeader* unit, size_t size);
 
-        // Merge two contiguous memory units that are not allocated.
+        /// Add the unit from the linked-list of free units
+        void addToFreeUnits(MemoryUnitHeader* unit);
+
+        /// Remove the unit from the linked-list of free units
+        void removeFromFreeUnits(MemoryUnitHeader* unit);
+
+        /// Merge two contiguous memory units that are not allocated.
         void mergeUnits(MemoryUnitHeader* unit1, MemoryUnitHeader* unit2);
 
         /// Reserve more memory for the allocator
         void reserve(size_t sizeToAllocate);
 
+        /// Return the next aligned memory address
+        void* computeAlignedAddress(void* unalignedAddress);
+
     public :
 
         // -------------------- Methods -------------------- //

include/reactphysics3d/memory/SingleFrameAllocator.h

@@ -65,6 +65,9 @@ class SingleFrameAllocator : public MemoryAllocator {
         /// Pointer to the next available memory location in the buffer
         size_t mCurrentOffset;
 
+        /// True if we need to allocate more memory in the next reset() call
+        bool mNeedToAllocatedMore;
+
     public :
 
         // -------------------- Methods -------------------- //

src/memory/HeapAllocator.cpp

@@ -36,7 +36,7 @@ size_t HeapAllocator::INIT_ALLOCATED_SIZE = 5 * 1048576;    // 5 Mb
 
 // Constructor
 HeapAllocator::HeapAllocator(MemoryAllocator& baseAllocator, size_t initAllocatedMemory)
-              : mBaseAllocator(baseAllocator), mAllocatedMemory(0), mMemoryUnits(nullptr), mCachedFreeUnit(nullptr) {
+              : mBaseAllocator(baseAllocator), mAllocatedMemory(0), mMemoryUnits(nullptr), mFreeUnits(nullptr) {
 
 #ifndef NDEBUG
         mNbTimesAllocateMethodCalled = 0;
@@ -55,12 +55,12 @@ HeapAllocator::~HeapAllocator() {
 #endif
 
     // Release the memory allocated for memory unit
-    MemoryUnitHeader* unit = mMemoryUnits;
+    MemoryUnitHeader* unit = mFreeUnits;
     while (unit != nullptr) {
 
         assert(!unit->isAllocated);
 
-        MemoryUnitHeader* nextUnit = unit->nextUnit;
+        MemoryUnitHeader* nextUnit = unit->nextFreeUnit;
 
         const size_t unitSize = unit->size;
 
@@ -76,23 +76,24 @@ HeapAllocator::~HeapAllocator() {
 /// left over space. The second unit is put into the free memory units
 void HeapAllocator::splitMemoryUnit(MemoryUnitHeader* unit, size_t size) {
 
-    assert(size <= unit->size);
     assert(!unit->isAllocated);
 
-    // Split the free memory unit in two memory units, one with the requested memory size
+    // If the size of the unit is large enough to be slit
-    // and a second one with the left over space
     if (size + sizeof(MemoryUnitHeader) < unit->size) {
 
-        assert(unit->size - size > 0);
-
         // Create a new memory unit with left over space
         unsigned char* newUnitLocation = (reinterpret_cast<unsigned char*>(unit)) + sizeof(MemoryUnitHeader) + size;
-        MemoryUnitHeader* newUnit = new (static_cast<void*>(newUnitLocation)) MemoryUnitHeader(unit->size - sizeof(MemoryUnitHeader) - size, unit, unit->nextUnit, unit->isNextContiguousMemory);
+        MemoryUnitHeader* newUnit = new (static_cast<void*>(newUnitLocation)) MemoryUnitHeader(unit->size - sizeof(MemoryUnitHeader) - size, unit, unit->nextUnit, unit, unit->nextFreeUnit, unit->isNextContiguousMemory);
         assert(newUnit->nextUnit != newUnit);
         unit->nextUnit = newUnit;
+        unit->nextFreeUnit = newUnit;
         if (newUnit->nextUnit != nullptr) {
             newUnit->nextUnit->previousUnit = newUnit;
         }
+        if (newUnit->nextFreeUnit != nullptr) {
+            newUnit->nextFreeUnit->previousFreeUnit = newUnit;
+        }
+
         assert(unit->nextUnit != unit);
         unit->isNextContiguousMemory = true;
         unit->size = size;
@@ -100,9 +101,19 @@ void HeapAllocator::splitMemoryUnit(MemoryUnitHeader* unit, size_t size) {
        assert(unit->previousUnit == nullptr || unit->previousUnit->nextUnit == unit);
        assert(unit->nextUnit == nullptr || unit->nextUnit->previousUnit == unit);
 
+       assert(unit->previousFreeUnit == nullptr || unit->previousFreeUnit->nextFreeUnit == unit);
+       assert(unit->nextFreeUnit == nullptr || unit->nextFreeUnit->previousFreeUnit == unit);
+
        assert(newUnit->previousUnit == nullptr || newUnit->previousUnit->nextUnit == newUnit);
        assert(newUnit->nextUnit == nullptr || newUnit->nextUnit->previousUnit == newUnit);
-    }
+
+       assert(newUnit->previousFreeUnit->nextFreeUnit == newUnit);
+       assert(newUnit->nextFreeUnit == nullptr || newUnit->nextFreeUnit->previousFreeUnit == newUnit);
+
+       assert(unit->nextFreeUnit == newUnit);
+       assert(newUnit->previousFreeUnit == unit);
+       assert(!newUnit->isAllocated);
+   }
 }
 
 // Allocate memory of a given size (in bytes) and return a pointer to the
@@ -117,72 +128,92 @@ void* HeapAllocator::allocate(size_t size) {
     // We cannot allocate zero bytes
     if (size == 0) return nullptr;
 
+    // Allocate a little bit more memory to make sure we can return an aligned address
+    const size_t totalSize = size + GLOBAL_ALIGNMENT;
+
 #ifndef NDEBUG
         mNbTimesAllocateMethodCalled++;
 #endif
 
-    MemoryUnitHeader* currentUnit = mMemoryUnits;
+    MemoryUnitHeader* currentUnit = mFreeUnits;
-    assert(mMemoryUnits->previousUnit == nullptr);
 
-    // If there is a cached free memory unit
+    // For each free memory unit
-    if (mCachedFreeUnit != nullptr) {
-        assert(!mCachedFreeUnit->isAllocated);
-
-        // If the cached free memory unit matches the request
-        if (size <= mCachedFreeUnit->size) {
-            currentUnit = mCachedFreeUnit;
-            mCachedFreeUnit = nullptr;
-        }
-    }
-
-    // For each memory unit
     while (currentUnit != nullptr) {
 
-        // If we have found a free memory unit with size large enough for the allocation request
+        assert(!currentUnit->isAllocated);
-        if (!currentUnit->isAllocated && size <= currentUnit->size) {
 
-            // Split the free memory unit in two memory units, one with the requested memory size
+        // If we have found a free memory unit with size large enough for the allocation request
-            // and a second one with the left over space
+        if (totalSize <= currentUnit->size) {
-            splitMemoryUnit(currentUnit, size);
 
             break;
         }
 
-        currentUnit = currentUnit->nextUnit;
+        assert(currentUnit->nextFreeUnit == nullptr || currentUnit->nextFreeUnit->previousFreeUnit == currentUnit);
+
+        currentUnit = currentUnit->nextFreeUnit;
     }
 
-    // If we have not found a large enough memory unit we need to allocate more memory
+    // If we have not found a large enough free memory unit
     if (currentUnit == nullptr) {
 
-        reserve((mAllocatedMemory + size) * 2);
+        // We need to allocate more memory
+        reserve((mAllocatedMemory + totalSize) * 2);
 
-        assert(mCachedFreeUnit != nullptr);
+        assert(mFreeUnits != nullptr);
-        assert(!mCachedFreeUnit->isAllocated);
 
         // The cached free memory unit is large enough at this point
-        currentUnit = mCachedFreeUnit;
+        currentUnit = mFreeUnits;
-
-        assert(currentUnit->size >= size);
-
-        splitMemoryUnit(currentUnit, size);
     }
 
+    // Split the free memory unit in two memory units, one with the requested memory size
+    // and a second one with the left over space
+    splitMemoryUnit(currentUnit, totalSize);
+
+    assert(currentUnit->size >= totalSize);
+    assert(!currentUnit->isAllocated);
+
     currentUnit->isAllocated = true;
 
-    // Cache the next memory unit if it is not allocated
+    removeFromFreeUnits(currentUnit);
-    if (currentUnit->nextUnit != nullptr && !currentUnit->nextUnit->isAllocated) {
-        mCachedFreeUnit = currentUnit->nextUnit;
-    }
 
     // Return a pointer to the memory area inside the unit
     void* allocatedMemory = static_cast<void*>(reinterpret_cast<unsigned char*>(currentUnit) + sizeof(MemoryUnitHeader));
 
+    // Offset the allocated address such that it is properly aligned
+    allocatedMemory = computeAlignedAddress(allocatedMemory);
+
     // Check that allocated memory is 16-bytes aligned
     assert(reinterpret_cast<uintptr_t>(allocatedMemory) % GLOBAL_ALIGNMENT == 0);
 
     return allocatedMemory;
 }
 
+// Return the next aligned memory address
+void* HeapAllocator::computeAlignedAddress(void* unalignedAddress) {
+
+    // Take care of alignment to make sure that we always return an address to the
+    // enforce the global alignment of the library
+
+    const uintptr_t currentAdress = reinterpret_cast<uintptr_t>(unalignedAddress);
+
+    // Calculate the adjustment by masking off the lower bits of the address, to determine how "misaligned" it is.
+    size_t mask = GLOBAL_ALIGNMENT - 1;
+    uintptr_t misalignment = currentAdress & mask;
+    ptrdiff_t alignmentOffset = GLOBAL_ALIGNMENT - misalignment;
+
+    // Compute the aligned address
+    uintptr_t alignedAddress = currentAdress + alignmentOffset;
+
+    // Store the adjustment in the byte immediately preceding the adjusted address.
+    // This way we can find again the original allocated memory address returned by malloc
+    // when this memory unit is released.
+    assert(alignmentOffset < 256);
+    uint8* pAlignedMemory = reinterpret_cast<uint8*>(alignedAddress);
+    pAlignedMemory[-1] = static_cast<uint8>(alignmentOffset);
+
+    return reinterpret_cast<void*>(alignedAddress);
+}
+
 // Release previously allocated memory.
 void HeapAllocator::release(void* pointer, size_t size) {
 
@@ -198,9 +229,19 @@ void HeapAllocator::release(void* pointer, size_t size) {
         mNbTimesAllocateMethodCalled--;
 #endif
 
-    unsigned char* unitLocation = static_cast<unsigned char*>(pointer) - sizeof(MemoryUnitHeader);
+    // Read the alignment offset in order to compute the initial allocated
+    // raw address (instead of the aligned address)
+    const uint8* pAlignedMemory = reinterpret_cast<const uint8*>(pointer);
+    const uintptr_t alignedAddress = reinterpret_cast<uintptr_t>(pAlignedMemory);
+    const ptrdiff_t alignmentOffset = static_cast<ptrdiff_t>(pAlignedMemory[-1]);
+    const uintptr_t initialAddress = alignedAddress - alignmentOffset;
+    void* pInitialAddress = reinterpret_cast<void*>(initialAddress);
+
+    unsigned char* unitLocation = static_cast<unsigned char*>(pInitialAddress) - sizeof(MemoryUnitHeader);
     MemoryUnitHeader* unit = reinterpret_cast<MemoryUnitHeader*>(unitLocation);
     assert(unit->isAllocated);
+    assert(unit->nextFreeUnit == nullptr);
+    assert(unit->previousFreeUnit == nullptr);
     unit->isAllocated = false;
 
     MemoryUnitHeader* currentUnit = unit;
@@ -208,6 +249,8 @@ void HeapAllocator::release(void* pointer, size_t size) {
     // If the previous unit is not allocated and memory is contiguous to the current unit
     if (unit->previousUnit != nullptr && !unit->previousUnit->isAllocated && unit->previousUnit->isNextContiguousMemory) {
 
+        removeFromFreeUnits(unit->previousUnit);
+
         currentUnit = unit->previousUnit;
 
         // Merge the two contiguous memory units
@@ -217,11 +260,40 @@ void HeapAllocator::release(void* pointer, size_t size) {
     // If the next unit is not allocated and memory is contiguous to the current unit
     if (currentUnit->nextUnit != nullptr && !currentUnit->nextUnit->isAllocated && currentUnit->isNextContiguousMemory) {
 
+        removeFromFreeUnits(unit->nextUnit);
+
         // Merge the two contiguous memory units
         mergeUnits(currentUnit, currentUnit->nextUnit);
     }
 
-    mCachedFreeUnit = currentUnit;
+    addToFreeUnits(currentUnit);
+}
+
+// Add the unit from the linked-list of free units
+void HeapAllocator::addToFreeUnits(MemoryUnitHeader* unit) {
+
+    if (mFreeUnits != nullptr) {
+        assert(mFreeUnits->previousFreeUnit == nullptr);
+        mFreeUnits->previousFreeUnit = unit;
+    }
+    unit->nextFreeUnit = mFreeUnits;
+    mFreeUnits = unit;
+}
+
+// Remove the unit from the linked-list of free units
+void HeapAllocator::removeFromFreeUnits(MemoryUnitHeader* unit) {
+
+    if (unit->previousFreeUnit != nullptr) {
+        unit->previousFreeUnit->nextFreeUnit = unit->nextFreeUnit;
+    }
+    if (unit->nextFreeUnit != nullptr) {
+        unit->nextFreeUnit->previousFreeUnit = unit->previousFreeUnit;
+    }
+    if (unit == mFreeUnits) {
+        mFreeUnits = unit->nextFreeUnit;
+    }
+    unit->nextFreeUnit = nullptr;
+    unit->previousFreeUnit = nullptr;
 }
 
 // Merge two contiguous memory units that are not allocated.
@@ -260,7 +332,13 @@ void HeapAllocator::reserve(size_t sizeToAllocate) {
     assert(reinterpret_cast<uintptr_t>(memory) % GLOBAL_ALIGNMENT == 0);
 
     // Create a new memory unit for the allocated memory
-    MemoryUnitHeader* memoryUnit = new (memory) MemoryUnitHeader(sizeToAllocate, nullptr, mMemoryUnits, false);
+    MemoryUnitHeader* memoryUnit = new (memory) MemoryUnitHeader(sizeToAllocate, nullptr, mMemoryUnits, nullptr, mFreeUnits, false);
+
+    if (mFreeUnits != nullptr) {
+
+        assert(mFreeUnits->previousFreeUnit == nullptr);
+        mFreeUnits->previousFreeUnit = memoryUnit;
+    }
 
     if (mMemoryUnits != nullptr) {
         mMemoryUnits->previousUnit = memoryUnit;
@@ -268,8 +346,7 @@ void HeapAllocator::reserve(size_t sizeToAllocate) {
 
     // Add the memory unit at the beginning of the linked-list of memory units
     mMemoryUnits = memoryUnit;
-
+    mFreeUnits = mMemoryUnits;
-    mCachedFreeUnit = mMemoryUnits;
 
     mAllocatedMemory += sizeToAllocate;
 }

src/memory/SingleFrameAllocator.cpp

@@ -34,11 +34,17 @@ using namespace reactphysics3d;
 // Constructor
 SingleFrameAllocator::SingleFrameAllocator(MemoryAllocator& baseAllocator) : mBaseAllocator(baseAllocator),
                                            mTotalSizeBytes(INIT_SINGLE_FRAME_ALLOCATOR_NB_BYTES),
-                                           mCurrentOffset(0) {
+                                           mCurrentOffset(0), mNeedToAllocatedMore(false) {
 
     // Allocate a whole block of memory at the beginning
-    mMemoryBufferStart = static_cast<char*>(mBaseAllocator.allocate(mTotalSizeBytes));
+    void* allocatedMemory = mBaseAllocator.allocate(mTotalSizeBytes);
-    assert(mMemoryBufferStart != nullptr);
+
+    assert(allocatedMemory != nullptr);
+
+    // Check that allocated memory is 16-bytes aligned
+    assert(reinterpret_cast<uintptr_t>(allocatedMemory) % GLOBAL_ALIGNMENT == 0);
+
+    mMemoryBufferStart = static_cast<char*>(allocatedMemory);
 }
 
 // Destructor
@@ -61,27 +67,11 @@ void* SingleFrameAllocator::allocate(size_t size) {
     // Check that there is enough remaining memory in the buffer
     if (mCurrentOffset + totalSize > mTotalSizeBytes) {
 
-        const size_t previousTotalSizeBytes = mTotalSizeBytes;
+        // We need to allocate more memory next time reset() is called
+       mNeedToAllocatedMore = true;
 
-        // Multiply the total memory to allocate by two
+       // Return default memory allocation
-        mTotalSizeBytes *= 2;
+       return mBaseAllocator.allocate(size);
-
-        // Allocate a whole block of memory
-        void* allocatedMemory = mBaseAllocator.allocate(mTotalSizeBytes);
-        assert(allocatedMemory != nullptr);
-
-        // Check that allocated memory is 16-bytes aligned
-        assert(reinterpret_cast<uintptr_t>(allocatedMemory) % GLOBAL_ALIGNMENT == 0);
-
-        char* newMemoryBufferStart = static_cast<char*>(allocatedMemory);
-
-        // Copy the previous memory bloc to new new location
-        memcpy(newMemoryBufferStart, mMemoryBufferStart, previousTotalSizeBytes);
-
-        // Release the memory allocated at the beginning
-        mBaseAllocator.release(mMemoryBufferStart, previousTotalSizeBytes);
-
-        mMemoryBufferStart = newMemoryBufferStart;
     }
 
     // Next available memory location
@@ -92,12 +82,13 @@ void* SingleFrameAllocator::allocate(size_t size) {
     uintptr_t currentAdress = reinterpret_cast<uintptr_t>(nextAvailableMemory);
 
     // Calculate the adjustment by masking off the lower bits of the address, to determine how "misaligned" it is.
-    size_t mask = GLOBAL_ALIGNMENT - 1;
+    const size_t mask = GLOBAL_ALIGNMENT - 1;
-    uintptr_t misalignment = currentAdress & mask;
+    const uintptr_t misalignment = currentAdress & mask;
-    ptrdiff_t alignmentOffset = GLOBAL_ALIGNMENT - misalignment;
+    const ptrdiff_t alignmentOffset = GLOBAL_ALIGNMENT - misalignment;
+    assert(alignmentOffset <= GLOBAL_ALIGNMENT);
 
     // Compute the aligned address
-    uintptr_t alignedAdress = currentAdress + alignmentOffset;
+    const uintptr_t alignedAdress = currentAdress + alignmentOffset;
     nextAvailableMemory = reinterpret_cast<void*>(alignedAdress);
 
     // Increment the offset
@@ -111,8 +102,18 @@ void* SingleFrameAllocator::allocate(size_t size) {
 }
 
 // Release previously allocated memory.
-void SingleFrameAllocator::release(void* /*pointer*/, size_t /*size*/) {
+void SingleFrameAllocator::release(void* pointer, size_t size) {
 
+    // Lock the method with a mutex
+    std::lock_guard<std::mutex> lock(mMutex);
+
+    // If allocated memory is not within the single frame allocation range
+    char* p = static_cast<char*>(pointer);
+    if (p < mMemoryBufferStart || p > mMemoryBufferStart + mTotalSizeBytes) {
+
+        // Use default deallocation
+        mBaseAllocator.release(pointer, size);
+    }
 }
 
 // Reset the marker of the current allocated memory
@@ -121,6 +122,22 @@ void SingleFrameAllocator::reset() {
     // Lock the method with a mutex
     std::lock_guard<std::mutex> lock(mMutex);
 
+    // If we need to allocate more memory
+    if (mNeedToAllocatedMore) {
+
+        // Release the memory allocated at the beginning
+        mBaseAllocator.release(mMemoryBufferStart, mTotalSizeBytes);
+
+        // Multiply the total memory to allocate by two
+        mTotalSizeBytes *= 2;
+
+        // Allocate a whole block of memory at the beginning
+        mMemoryBufferStart = static_cast<char*>(mBaseAllocator.allocate(mTotalSizeBytes));
+        assert(mMemoryBufferStart != nullptr);
+
+        mNeedToAllocatedMore = false;
+    }
+
     // Reset the current offset at the beginning of the block
     mCurrentOffset = 0;
 }