diff --git a/thirdparty/jolt_physics/Jolt/Core/STLLocalAllocator.h b/thirdparty/jolt_physics/Jolt/Core/STLLocalAllocator.h new file mode 100644 index 00000000000..92e16ef7e0a --- /dev/null +++ b/thirdparty/jolt_physics/Jolt/Core/STLLocalAllocator.h @@ -0,0 +1,169 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2025 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +#ifndef JPH_DISABLE_CUSTOM_ALLOCATOR + +/// STL allocator that keeps N elements in a local buffer before falling back to regular allocations +template +class STLLocalAllocator : private STLAllocator +{ + using Base = STLAllocator; + +public: + /// General properties + using value_type = T; + using pointer = T *; + using const_pointer = const T *; + using reference = T &; + using const_reference = const T &; + using size_type = size_t; + using difference_type = ptrdiff_t; + + /// The allocator is not stateless (has local buffer) + using is_always_equal = std::false_type; + + /// We cannot copy, move or swap allocators + using propagate_on_container_copy_assignment = std::false_type; + using propagate_on_container_move_assignment = std::false_type; + using propagate_on_container_swap = std::false_type; + + /// Constructor + STLLocalAllocator() = default; + STLLocalAllocator(const STLLocalAllocator &) = delete; // Can't copy an allocator as the buffer is local to the original + STLLocalAllocator(STLLocalAllocator &&) = delete; // Can't move an allocator as the buffer is local to the original + STLLocalAllocator & operator = (const STLLocalAllocator &) = delete; // Can't copy an allocator as the buffer is local to the original + + /// Constructor used when rebinding to another type. This expects the allocator to use the original memory pool from the first allocator, + /// but in our case we cannot use the local buffer of the original allocator as it has different size and alignment rules. + /// To solve this we make this allocator fall back to the heap immediately. + template STLLocalAllocator(const STLLocalAllocator &) : mNumElementsUsed(N) { } + + /// Check if inPointer is in the local buffer + inline bool is_local(const_pointer inPointer) const + { + ptrdiff_t diff = inPointer - reinterpret_cast(mElements); + return diff >= 0 && diff < ptrdiff_t(N); + } + + /// Allocate memory + inline pointer allocate(size_type inN) + { + // If we allocate more than we have, fall back to the heap + if (mNumElementsUsed + inN > N) + return Base::allocate(inN); + + // Allocate from our local buffer + pointer result = reinterpret_cast(mElements) + mNumElementsUsed; + mNumElementsUsed += inN; + return result; + } + + /// Always implements a reallocate function as we can often reallocate in place + static constexpr bool has_reallocate = true; + + /// Reallocate memory + inline pointer reallocate(pointer inOldPointer, size_type inOldSize, size_type inNewSize) + { + JPH_ASSERT(inNewSize > 0); // Reallocating to zero size is implementation dependent, so we don't allow it + + // If there was no previous allocation, we can go through the regular allocate function + if (inOldPointer == nullptr) + return allocate(inNewSize); + + // If the pointer is outside our local buffer, fall back to the heap + if (!is_local(inOldPointer)) + { + if constexpr (AllocatorHasReallocate::sValue) + return Base::reallocate(inOldPointer, inOldSize, inNewSize); + else + return ReallocateImpl(inOldPointer, inOldSize, inNewSize); + } + + // If we happen to have space left, we only need to update our bookkeeping + pointer base_ptr = reinterpret_cast(mElements) + mNumElementsUsed - inOldSize; + if (inOldPointer == base_ptr + && mNumElementsUsed - inOldSize + inNewSize <= N) + { + mNumElementsUsed += inNewSize - inOldSize; + return base_ptr; + } + + // We can't reallocate in place, fall back to the heap + return ReallocateImpl(inOldPointer, inOldSize, inNewSize); + } + + /// Free memory + inline void deallocate(pointer inPointer, size_type inN) + { + // If the pointer is not in our local buffer, fall back to the heap + if (!is_local(inPointer)) + return Base::deallocate(inPointer, inN); + + // Else we can only reclaim memory if it was the last allocation + if (inPointer == reinterpret_cast(mElements) + mNumElementsUsed - inN) + mNumElementsUsed -= inN; + } + + /// Allocators are not-stateless, assume if allocator address matches that the allocators are the same + inline bool operator == (const STLLocalAllocator &inRHS) const + { + return this == &inRHS; + } + + inline bool operator != (const STLLocalAllocator &inRHS) const + { + return this != &inRHS; + } + + /// Converting to allocator for other type + template + struct rebind + { + using other = STLLocalAllocator; + }; + +private: + /// Implements reallocate when the base class doesn't or when we go from local buffer to heap + inline pointer ReallocateImpl(pointer inOldPointer, size_type inOldSize, size_type inNewSize) + { + pointer new_pointer = Base::allocate(inNewSize); + size_type n = min(inOldSize, inNewSize); + if constexpr (std::is_trivially_copyable()) + { + // Can use mem copy + memcpy(new_pointer, inOldPointer, n * sizeof(T)); + } + else + { + // Need to actually move the elements + for (size_t i = 0; i < n; ++i) + { + new (new_pointer + i) T(std::move(inOldPointer[i])); + inOldPointer[i].~T(); + } + } + deallocate(inOldPointer, inOldSize); + return new_pointer; + } + + alignas(T) uint8 mElements[N * sizeof(T)]; + size_type mNumElementsUsed = 0; +}; + +/// The STLLocalAllocator always implements a reallocate function as it can often reallocate in place +template struct AllocatorHasReallocate> { static constexpr bool sValue = STLLocalAllocator::has_reallocate; }; + +#else + +template using STLLocalAllocator = std::allocator; + +#endif // !JPH_DISABLE_CUSTOM_ALLOCATOR + +JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/InternalEdgeRemovingCollector.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/InternalEdgeRemovingCollector.h index 0d67729125e..7cbbb226db1 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/InternalEdgeRemovingCollector.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/InternalEdgeRemovingCollector.h @@ -5,6 +5,7 @@ #pragma once #include +#include #include //#define JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG @@ -17,10 +18,13 @@ JPH_NAMESPACE_BEGIN /// Removes internal edges from collision results. Can be used to filter out 'ghost collisions'. /// Based on: Contact generation for meshes - Pierre Terdiman (https://www.codercorner.com/MeshContacts.pdf) +/// +/// Note that this class requires that CollideSettingsBase::mActiveEdgeMode == EActiveEdgeMode::CollideWithAll +/// and CollideSettingsBase::mCollectFacesMode == ECollectFacesMode::CollectFaces. class InternalEdgeRemovingCollector : public CollideShapeCollector { - static constexpr uint cMaxDelayedResults = 16; - static constexpr uint cMaxVoidedFeatures = 128; + static constexpr uint cMaxLocalDelayedResults = 32; + static constexpr uint cMaxLocalVoidedFeatures = 128; /// Check if a vertex is voided inline bool IsVoided(const SubShapeID &inSubShapeID, Vec3 inV) const @@ -35,17 +39,14 @@ class InternalEdgeRemovingCollector : public CollideShapeCollector /// Add all vertices of a face to the voided features inline void VoidFeatures(const CollideShapeResult &inResult) { - if (mVoidedFeatures.size() < cMaxVoidedFeatures) - for (const Vec3 &v : inResult.mShape2Face) - if (!IsVoided(inResult.mSubShapeID1, v)) - { - Voided vf; - v.StoreFloat3(&vf.mFeature); - vf.mSubShapeID = inResult.mSubShapeID1; - mVoidedFeatures.push_back(vf); - if (mVoidedFeatures.size() == cMaxVoidedFeatures) - break; - } + for (const Vec3 &v : inResult.mShape2Face) + if (!IsVoided(inResult.mSubShapeID1, v)) + { + Voided vf; + v.StoreFloat3(&vf.mFeature); + vf.mSubShapeID = inResult.mSubShapeID1; + mVoidedFeatures.push_back(vf); + } } /// Call the chained collector @@ -119,8 +120,6 @@ public: return ChainAndVoid(inResult); // Delayed processing - if (mDelayedResults.size() == cMaxDelayedResults) - return ChainAndVoid(inResult); mDelayedResults.push_back(inResult); } @@ -128,10 +127,11 @@ public: void Flush() { // Sort on biggest penetration depth first - uint sorted_indices[cMaxDelayedResults]; + Array> sorted_indices; + sorted_indices.resize(mDelayedResults.size()); for (uint i = 0; i < uint(mDelayedResults.size()); ++i) sorted_indices[i] = i; - QuickSort(sorted_indices, sorted_indices + mDelayedResults.size(), [this](uint inLHS, uint inRHS) { return mDelayedResults[inLHS].mPenetrationDepth > mDelayedResults[inRHS].mPenetrationDepth; }); + QuickSort(sorted_indices.begin(), sorted_indices.end(), [this](uint inLHS, uint inRHS) { return mDelayedResults[inLHS].mPenetrationDepth > mDelayedResults[inRHS].mPenetrationDepth; }); // Loop over all results for (uint i = 0; i < uint(mDelayedResults.size()); ++i) @@ -243,8 +243,8 @@ private: }; CollideShapeCollector & mChainedCollector; - StaticArray mVoidedFeatures; - StaticArray mDelayedResults; + Array> mVoidedFeatures; + Array> mDelayedResults; }; JPH_NAMESPACE_END