Improved contact cache scaling across multiple cores (#26)

* Removed LockFreeHashMap::mNumKeyValues in non-debug builds to avoid contention on the atomic
* Fixed contention on LockFreeHashMap::mWriteOffset by allocating larger blocks and putting multiple contact points in a single block

Jolt/Core/LockFreeHashMap.h

@@ -8,6 +8,61 @@
 
 namespace JPH {
 
+/// Allocator for a lock free hash map
+class LFHMAllocator : public NonCopyable
+{
+public:
+	/// Destructor
+	inline					~LFHMAllocator();
+
+	/// Initialize the allocator
+	/// @param inObjectStoreSizeBytes Number of bytes to reserve for all key value pairs
+	inline void				Init(uint inObjectStoreSizeBytes);
+
+	/// Clear all allocations
+	inline void				Clear();
+
+	/// Allocate a new block of data
+	/// @param inBlockSize Size of block to allocate (will potentially return a smaller block if memory is full)
+	/// @param ioBegin Should be start of first free byte in current memory block on input, will contain start of first free byte in allocated block on return
+	/// @param ioEnd Should be the byte beyond the current memory block on input, will contain the byte beyond the allocated block on return
+	inline void				Allocate(uint32 inBlockSize, uint32 &ioBegin, uint32 &ioEnd);
+
+	/// Convert a pointer to an offset
+	template <class T>
+	inline uint32			ToOffset(const T *inData) const;
+
+	/// Convert an offset to a pointer
+	template <class T>
+	inline T *				FromOffset(uint32 inOffset) const;
+
+private:
+	uint8 *					mObjectStore = nullptr;			///< This contains a contigous list of objects (possibly of varying size)
+	uint32					mObjectStoreSizeBytes = 0;		///< The size of mObjectStore in bytes
+	atomic<uint32>			mWriteOffset { 0 };				///< Next offset to write to in mObjectStore
+};
+
+/// Allocator context object for a lock free hash map that allocates a larger memory block at once and hands it out in smaller portions.
+/// This avoids contention on the atomic LFHMAllocator::mWriteOffset.
+class LFHMAllocatorContext : public NonCopyable
+{
+public:
+	/// Construct a new allocator context
+	inline					LFHMAllocatorContext(LFHMAllocator &inAllocator, uint32 inBlockSize);
+
+	/// @brief Allocate data block
+	/// @param inSize Size of block to allocae
+	/// @param outWriteOffset Offset in buffer where block is located
+	/// @return True if allocation succeeded
+	inline bool				Allocate(uint32 inSize, uint32 &outWriteOffset);
+
+private:
+	LFHMAllocator &			mAllocator;
+	uint32					mBlockSize;
+	uint32					mBegin = 0;
+	uint32					mEnd = 0;
+};
+
 /// Very simple lock free hash map that only allows insertion, retrieval and provides a fixed amount of buckets and fixed storage.
 /// Note: This class currently assumes key and value are simple types that need no calls to the destructor.
 template <class Key, class Value>
@@ -17,12 +72,12 @@ public:
 	using MapType = LockFreeHashMap<Key, Value>;
 
 	/// Destructor
+	explicit				LockFreeHashMap(LFHMAllocator &inAllocator) : mAllocator(inAllocator) { }
 							~LockFreeHashMap();
 
 	/// Initialization
-	/// @param inObjectStoreSizeBytes Number of bytes to reserve for all key value pairs
 	/// @param inMaxBuckets Max amount of buckets to use in the hashmap. Must be power of 2.
-	void					Init(uint32 inObjectStoreSizeBytes, uint32 inMaxBuckets);
+	void					Init(uint32 inMaxBuckets);
 
 	/// Remove all elements.
 	/// Note that this cannot happen simultaneously with adding new elements.
@@ -57,7 +112,7 @@ public:
 	/// Insert a new element, returns null if map full.
 	/// Multiple threads can be inserting in the map at the same time.
 	template <class... Params>
-	inline KeyValue *		Create(const Key &inKey, size_t inKeyHash, int inExtraBytes, Params &&... inConstructorParams);
+	inline KeyValue *		Create(LFHMAllocatorContext &ioContext, const Key &inKey, size_t inKeyHash, int inExtraBytes, Params &&... inConstructorParams);
 	
 	/// Find an element, returns null if not found
 	inline const KeyValue *	Find(const Key &inKey, size_t inKeyHash) const;
@@ -71,8 +126,11 @@ public:
 	/// Convert uint32 handle back to key and value
 	inline const KeyValue *	FromHandle(uint32 inHandle) const;
 
-	/// Get the number of key value pairs that this map currently contains
-	inline uint32			GetNumKeyValues() const		{ return mNumKeyValues; }
+#ifdef JPH_ENABLE_ASSERTS
+	/// Get the number of key value pairs that this map currently contains.
+	/// Available only when asserts are enabled because adding elements creates contention on this atomic and negatively affects performance.
+	inline uint32			GetNumKeyValues() const			{ return mNumKeyValues; }
+#endif // JPH_ENABLE_ASSERTS
 
 	/// Get all key/value pairs
 	inline void				GetAllKeyValues(vector<const KeyValue *> &outAll) const;
@@ -106,10 +164,11 @@ public:
 #endif
 
 private:
-	uint8 *					mObjectStore = nullptr;			///< This contains a contigous list of objects (possibly of varying size)
-	uint32					mObjectStoreSizeBytes = 0;		///< The size of mObjectStore in bytes
-	atomic<uint32>			mWriteOffset { 0 };				///< Next offset to write to in mObjectStore
+	LFHMAllocator &			mAllocator;						///< Allocator used to allocate key value pairs
+
+#ifdef JPH_ENABLE_ASSERTS
 	atomic<uint32>			mNumKeyValues = 0;				///< Number of key value pairs in the store
+#endif // JPH_ENABLE_ASSERTS
 
 	atomic<uint32> *		mBuckets = nullptr;				///< This contains the offset in mObjectStore of the first object with a particular hash
 	uint32					mNumBuckets = 0;				///< Current number of buckets

Jolt/Core/LockFreeHashMap.inl

@@ -5,18 +5,107 @@
 
 namespace JPH {
 
+///////////////////////////////////////////////////////////////////////////////////
+// LFHMAllocator
+///////////////////////////////////////////////////////////////////////////////////
+
+inline LFHMAllocator::~LFHMAllocator()
+{
+	delete [] mObjectStore;
+}
+
+inline void LFHMAllocator::Init(uint inObjectStoreSizeBytes)
+{
+	JPH_ASSERT(mObjectStore == nullptr);
+
+	mObjectStoreSizeBytes = inObjectStoreSizeBytes;
+	mObjectStore = new uint8 [inObjectStoreSizeBytes];
+}
+
+inline void LFHMAllocator::Clear()
+{
+	mWriteOffset = 0;
+}
+
+inline void LFHMAllocator::Allocate(uint32 inBlockSize, uint32 &ioBegin, uint32 &ioEnd)
+{
+	// Atomically fetch a block from the pool
+	uint32 begin = mWriteOffset.fetch_add(inBlockSize, memory_order_relaxed);
+	uint32 end = min(begin + inBlockSize, mObjectStoreSizeBytes);
+
+	if (ioEnd == begin)
+	{
+		// Block is allocated straight after our previous block
+		begin = ioBegin;
+	}
+	else
+	{
+		// Block is a new block
+		begin = min(begin, mObjectStoreSizeBytes);
+	}
+
+	// Store the begin and end of the resulting block
+	ioBegin = begin;
+	ioEnd = end;
+}
+
+template <class T>
+inline uint32 LFHMAllocator::ToOffset(const T *inData) const
+{
+	const uint8 *data = reinterpret_cast<const uint8 *>(inData);
+	JPH_ASSERT(data >= mObjectStore && data < mObjectStore + mObjectStoreSizeBytes);
+	return uint32(data - mObjectStore);
+}
+
+template <class T>
+inline T *LFHMAllocator::FromOffset(uint32 inOffset) const
+{
+	JPH_ASSERT(inOffset < mObjectStoreSizeBytes);
+	return reinterpret_cast<T *>(mObjectStore + inOffset);
+}
+
+///////////////////////////////////////////////////////////////////////////////////
+// LFHMAllocatorContext
+///////////////////////////////////////////////////////////////////////////////////
+
+inline LFHMAllocatorContext::LFHMAllocatorContext(LFHMAllocator &inAllocator, uint32 inBlockSize) : 
+	mAllocator(inAllocator), 
+	mBlockSize(inBlockSize) 
+{ 
+}
+
+inline bool LFHMAllocatorContext::Allocate(uint32 inSize, uint32 &outWriteOffset)
+{
+	// Check if we have space
+	if (mEnd - mBegin < inSize)
+	{
+		// Allocate a new block
+		mAllocator.Allocate(mBlockSize, mBegin, mEnd);
+
+		// Check if we have space again
+		if (mEnd - mBegin < inSize)
+			return false;
+	}
+
+	// Make the allocation
+	outWriteOffset = mBegin;
+	mBegin += inSize;
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////
+// LockFreeHashMap
+///////////////////////////////////////////////////////////////////////////////////
+
 template <class Key, class Value>
-void LockFreeHashMap<Key, Value>::Init(uint inObjectStoreSizeBytes, uint32 inMaxBuckets)
+void LockFreeHashMap<Key, Value>::Init(uint32 inMaxBuckets)
 {
 	JPH_ASSERT(inMaxBuckets >= 4 && IsPowerOf2(inMaxBuckets));
-	JPH_ASSERT(mObjectStore == nullptr);
 	JPH_ASSERT(mBuckets == nullptr);
 
-	mObjectStoreSizeBytes = inObjectStoreSizeBytes;
 	mNumBuckets = inMaxBuckets;
 	mMaxBuckets = inMaxBuckets;
 
-	mObjectStore = new uint8 [inObjectStoreSizeBytes];
 	mBuckets = new atomic<uint32> [inMaxBuckets];
 
 	Clear();
@@ -25,16 +114,16 @@ void LockFreeHashMap<Key, Value>::Init(uint inObjectStoreSizeBytes, uint32 inMax
 template <class Key, class Value>
 LockFreeHashMap<Key, Value>::~LockFreeHashMap()
 {
-	delete [] mObjectStore;
 	delete [] mBuckets;
 }
 
 template <class Key, class Value>
 void LockFreeHashMap<Key, Value>::Clear()
 {
-	// Reset write offset and number of key value pairs
-	mWriteOffset = 0;
+#ifdef JPH_ENABLE_ASSERTS
+	// Reset number of key value pairs
 	mNumKeyValues = 0;
+#endif // JPH_ENABLE_ASSERTS
 
 	// Reset buckets 4 at a time
 	static_assert(sizeof(atomic<uint32>) == sizeof(uint32));
@@ -60,7 +149,7 @@ void LockFreeHashMap<Key, Value>::SetNumBuckets(uint32 inNumBuckets)
 
 template <class Key, class Value>
 template <class... Params>
-inline typename LockFreeHashMap<Key, Value>::KeyValue *LockFreeHashMap<Key, Value>::Create(const Key &inKey, size_t inKeyHash, int inExtraBytes, Params &&... inConstructorParams)
+inline typename LockFreeHashMap<Key, Value>::KeyValue *LockFreeHashMap<Key, Value>::Create(LFHMAllocatorContext &ioContext, const Key &inKey, size_t inKeyHash, int inExtraBytes, Params &&... inConstructorParams)
 {
 	// This is not a multi map, test the key hasn't been inserted yet
 	JPH_ASSERT(Find(inKey, inKeyHash) == nullptr);
@@ -68,14 +157,19 @@ inline typename LockFreeHashMap<Key, Value>::KeyValue *LockFreeHashMap<Key, Valu
 	// Calculate total size
 	uint size = sizeof(KeyValue) + inExtraBytes;
 
-	// Allocate entry in the cache
-	uint32 write_offset = mWriteOffset.fetch_add(size);
-	if (write_offset + size > mObjectStoreSizeBytes)
+	// Get the write offset for this key value pair
+	uint32 write_offset;
+	if (!ioContext.Allocate(size, write_offset))
 		return nullptr;
-	++mNumKeyValues;
+
+#ifdef JPH_ENABLE_ASSERTS
+	// Increment amount of entries in map
+	mNumKeyValues.fetch_add(1, memory_order_relaxed);
+#endif // JPH_ENABLE_ASSERTS
 
 	// Construct the key/value pair
-	KeyValue *kv = reinterpret_cast<KeyValue *>(mObjectStore + write_offset);
+	KeyValue *kv = mAllocator.template FromOffset<KeyValue>(write_offset);
+	JPH_ASSERT(intptr_t(kv) % alignof(KeyValue) == 0);
 #ifdef _DEBUG
 	memset(kv, 0xcd, size);
 #endif
@@ -86,12 +180,11 @@ inline typename LockFreeHashMap<Key, Value>::KeyValue *LockFreeHashMap<Key, Valu
 	atomic<uint32> &offset = mBuckets[inKeyHash & (mNumBuckets - 1)];
 
 	// Add this entry as the first element in the linked list
-	uint32 new_offset = uint32(reinterpret_cast<uint8 *>(kv) - mObjectStore);
 	for (;;)
 	{
-		uint32 old_offset = offset;
+		uint32 old_offset = offset.load(memory_order_relaxed);
 		kv->mNextOffset = old_offset;
-		if (offset.compare_exchange_strong(old_offset, new_offset))
+		if (offset.compare_exchange_weak(old_offset, write_offset, memory_order_release))
 			break;
 	}
 
@@ -102,11 +195,11 @@ template <class Key, class Value>
 inline const typename LockFreeHashMap<Key, Value>::KeyValue *LockFreeHashMap<Key, Value>::Find(const Key &inKey, size_t inKeyHash) const
 {
 	// Get the offset to the keyvalue object from the bucket with corresponding hash
-	uint32 offset = mBuckets[inKeyHash & (mNumBuckets - 1)];
+	uint32 offset = mBuckets[inKeyHash & (mNumBuckets - 1)].load(memory_order_acquire);
 	while (offset != cInvalidHandle)
 	{
 		// Loop through linked list of values until the right one is found
-		const KeyValue *kv = reinterpret_cast<const KeyValue *>(mObjectStore + offset);
+		const KeyValue *kv = mAllocator.template FromOffset<const KeyValue>(offset);
 		if (kv->mKey == inKey)
 			return kv;
 		offset = kv->mNextOffset;
@@ -119,16 +212,13 @@ inline const typename LockFreeHashMap<Key, Value>::KeyValue *LockFreeHashMap<Key
 template <class Key, class Value>
 inline uint32 LockFreeHashMap<Key, Value>::ToHandle(const KeyValue *inKeyValue) const
 {
-	const uint8 *kv = reinterpret_cast<const uint8 *>(inKeyValue);
-	JPH_ASSERT(kv >= mObjectStore && kv < mObjectStore + mObjectStoreSizeBytes);
-	return uint32(kv - mObjectStore);
+	return mAllocator.ToOffset(inKeyValue);
 }
 
 template <class Key, class Value>
 inline const typename LockFreeHashMap<Key, Value>::KeyValue *LockFreeHashMap<Key, Value>::FromHandle(uint32 inHandle) const
 {
-	JPH_ASSERT(inHandle < mObjectStoreSizeBytes);
-	return reinterpret_cast<const KeyValue *>(mObjectStore + inHandle);
+	return mAllocator.template FromOffset<const KeyValue>(inHandle);
 }
 
 template <class Key, class Value>
@@ -139,7 +229,7 @@ inline void LockFreeHashMap<Key, Value>::GetAllKeyValues(vector<const KeyValue *
 		uint32 offset = *bucket;
 		while (offset != cInvalidHandle)
 		{
-			const KeyValue *kv = reinterpret_cast<const KeyValue *>(mObjectStore + offset);
+			const KeyValue *kv = mAllocator.template FromOffset<const KeyValue>(offset);
 			outAll.push_back(kv);
 			offset = kv->mNextOffset;
 		}
@@ -170,7 +260,7 @@ typename LockFreeHashMap<Key, Value>::KeyValue &LockFreeHashMap<Key, Value>::Ite
 {
 	JPH_ASSERT(mOffset != cInvalidHandle);
 
-	return *reinterpret_cast<KeyValue *>(mMap->mObjectStore + mOffset);
+	return *mMap->mAllocator.template FromOffset<KeyValue>(mOffset);
 }		
 
 template <class Key, class Value>
@@ -181,7 +271,7 @@ typename LockFreeHashMap<Key, Value>::Iterator &LockFreeHashMap<Key, Value>::Ite
 	// Find the next key value in this bucket
 	if (mOffset != cInvalidHandle)
 	{
-		const KeyValue *kv = reinterpret_cast<const KeyValue *>(mMap->mObjectStore + mOffset);
+		const KeyValue *kv = mMap->mAllocator.template FromOffset<const KeyValue>(mOffset);
 		mOffset = kv->mNextOffset;
 		if (mOffset != cInvalidHandle)
 			return *this;
@@ -221,7 +311,7 @@ void LockFreeHashMap<Key, Value>::TraceStats() const
 		uint32 offset = *bucket;
 		while (offset != cInvalidHandle)
 		{
-			const KeyValue *kv = reinterpret_cast<const KeyValue *>(mObjectStore + offset);
+			const KeyValue *kv = mAllocator.template FromOffset<const KeyValue>(offset);
 			offset = kv->mNextOffset;
 			++objects_in_bucket;
 			++num_objects;

Jolt/Physics/Constraints/ContactConstraintManager.cpp

@@ -200,8 +200,9 @@ void ContactConstraintManager::CachedBodyPair::RestoreState(StateRecorder &inStr
 
 void ContactConstraintManager::ManifoldCache::Init(uint inMaxBodyPairs, uint inMaxContactConstraints, uint inCachedManifoldsSize)
 {
-	mCachedManifolds.Init(inCachedManifoldsSize, GetNextPowerOf2(inMaxContactConstraints));
-	mCachedBodyPairs.Init(inMaxBodyPairs * sizeof(BodyPairMap::KeyValue), GetNextPowerOf2(inMaxBodyPairs));
+	mAllocator.Init(inMaxBodyPairs * sizeof(BodyPairMap::KeyValue) + inCachedManifoldsSize);
+	mCachedManifolds.Init(GetNextPowerOf2(inMaxContactConstraints));
+	mCachedBodyPairs.Init(GetNextPowerOf2(inMaxBodyPairs));
 }
 
 void ContactConstraintManager::ManifoldCache::Clear()
@@ -210,6 +211,7 @@ void ContactConstraintManager::ManifoldCache::Clear()
 
 	mCachedManifolds.Clear();
 	mCachedBodyPairs.Clear();
+	mAllocator.Clear();
 
 #ifdef JPH_ENABLE_ASSERTS
 	// Mark as incomplete
@@ -217,16 +219,14 @@ void ContactConstraintManager::ManifoldCache::Clear()
 #endif
 }
 
-void ContactConstraintManager::ManifoldCache::Prepare(const ManifoldCache &inReadCache)
+void ContactConstraintManager::ManifoldCache::Prepare(uint inExpectedNumBodyPairs, uint inExpectedNumManifolds)
 {
-	JPH_ASSERT(this != &inReadCache);
-
 	// Minimum amount of buckets to use in the hash map
 	constexpr uint32 cMinBuckets = 1024;
 
 	// Use the next higher power of 2 of amount of objects in the cache from last frame to determine the amount of buckets in this frame
-	mCachedManifolds.SetNumBuckets(min(max(cMinBuckets, GetNextPowerOf2(inReadCache.mCachedManifolds.GetNumKeyValues())), mCachedManifolds.GetMaxBuckets()));
-	mCachedBodyPairs.SetNumBuckets(min(max(cMinBuckets, GetNextPowerOf2(inReadCache.mCachedBodyPairs.GetNumKeyValues())), mCachedBodyPairs.GetMaxBuckets()));
+	mCachedManifolds.SetNumBuckets(min(max(cMinBuckets, GetNextPowerOf2(inExpectedNumManifolds)), mCachedManifolds.GetMaxBuckets()));
+	mCachedBodyPairs.SetNumBuckets(min(max(cMinBuckets, GetNextPowerOf2(inExpectedNumBodyPairs)), mCachedBodyPairs.GetMaxBuckets()));
 }
 
 const ContactConstraintManager::MKeyValue *ContactConstraintManager::ManifoldCache::Find(const SubShapeIDPair &inKey, size_t inKeyHash) const
@@ -235,26 +235,27 @@ const ContactConstraintManager::MKeyValue *ContactConstraintManager::ManifoldCac
 	return mCachedManifolds.Find(inKey, inKeyHash);
 }
 
-ContactConstraintManager::MKeyValue *ContactConstraintManager::ManifoldCache::Create(const SubShapeIDPair &inKey, size_t inKeyHash, int inNumContactPoints)
+ContactConstraintManager::MKeyValue *ContactConstraintManager::ManifoldCache::Create(ContactAllocator &ioContactAllocator, const SubShapeIDPair &inKey, size_t inKeyHash, int inNumContactPoints)
 {
 	JPH_ASSERT(!mIsFinalized);
-	MKeyValue *kv = mCachedManifolds.Create(inKey, inKeyHash, CachedManifold::sGetRequiredExtraSize(inNumContactPoints));
+	MKeyValue *kv = mCachedManifolds.Create(ioContactAllocator, inKey, inKeyHash, CachedManifold::sGetRequiredExtraSize(inNumContactPoints));
 	if (kv == nullptr)
 	{
 		JPH_ASSERT(false, "Out of cache space for manifold cache");
 		return nullptr;
 	}
 	kv->GetValue().mNumContactPoints = uint16(inNumContactPoints);
+	++ioContactAllocator.mNumManifolds;
 	return kv;
 }
 
-ContactConstraintManager::MKVAndCreated ContactConstraintManager::ManifoldCache::FindOrCreate(const SubShapeIDPair &inKey, size_t inKeyHash, int inNumContactPoints)
+ContactConstraintManager::MKVAndCreated ContactConstraintManager::ManifoldCache::FindOrCreate(ContactAllocator &ioContactAllocator, const SubShapeIDPair &inKey, size_t inKeyHash, int inNumContactPoints)
 {
 	MKeyValue *kv = const_cast<MKeyValue *>(mCachedManifolds.Find(inKey, inKeyHash));
 	if (kv != nullptr)
 		return { kv, false };
 
-	return { Create(inKey, inKeyHash, inNumContactPoints), true };
+	return { Create(ioContactAllocator, inKey, inKeyHash, inNumContactPoints), true };
 }
 
 uint32 ContactConstraintManager::ManifoldCache::ToHandle(const MKeyValue *inKeyValue) const
@@ -275,15 +276,16 @@ const ContactConstraintManager::BPKeyValue *ContactConstraintManager::ManifoldCa
 	return mCachedBodyPairs.Find(inKey, inKeyHash);
 }
 
-ContactConstraintManager::BPKeyValue *ContactConstraintManager::ManifoldCache::Create(const BodyPair &inKey, size_t inKeyHash)
+ContactConstraintManager::BPKeyValue *ContactConstraintManager::ManifoldCache::Create(ContactAllocator &ioContactAllocator, const BodyPair &inKey, size_t inKeyHash)
 {
 	JPH_ASSERT(!mIsFinalized);
-	BPKeyValue *kv = mCachedBodyPairs.Create(inKey, inKeyHash, 0);
+	BPKeyValue *kv = mCachedBodyPairs.Create(ioContactAllocator, inKey, inKeyHash, 0);
 	if (kv == nullptr)
 	{
 		JPH_ASSERT(false, "Out of cache space for body pair cache");
 		return nullptr;
 	}
+	++ioContactAllocator.mNumBodyPairs;
 	return kv;
 }
 
@@ -424,6 +426,9 @@ bool ContactConstraintManager::ManifoldCache::RestoreState(const ManifoldCache &
 
 	bool success = true;
 
+	// Create a contact allocator for restoring the contact cache
+	ContactAllocator contact_allocator(GetContactAllocator());
+
 	// When validating, get all existing body pairs
 	vector<const BPKeyValue *> all_bp;
 	if (inStream.IsValidating())
@@ -446,7 +451,7 @@ bool ContactConstraintManager::ManifoldCache::RestoreState(const ManifoldCache &
 
 		// Create new entry for this body pair
 		size_t body_pair_hash = BodyPairHash {} (body_pair_key);
-		BPKeyValue *bp_kv = Create(body_pair_key, body_pair_hash);
+		BPKeyValue *bp_kv = Create(contact_allocator, body_pair_key, body_pair_hash);
 		if (bp_kv == nullptr)
 		{
 			// Out of cache space
@@ -488,7 +493,7 @@ bool ContactConstraintManager::ManifoldCache::RestoreState(const ManifoldCache &
 			inStream.Read(num_contact_points);
 
 			// Read manifold
-			MKeyValue *m_kv = Create(sub_shape_key, sub_shape_key_hash, num_contact_points);
+			MKeyValue *m_kv = Create(contact_allocator, sub_shape_key, sub_shape_key_hash, num_contact_points);
 			if (m_kv == nullptr)
 			{
 				// Out of cache space
@@ -533,7 +538,7 @@ bool ContactConstraintManager::ManifoldCache::RestoreState(const ManifoldCache &
 		size_t sub_shape_key_hash = std::hash<SubShapeIDPair> {} (sub_shape_key);
 			
 		// Create CCD manifold
-		MKeyValue *m_kv = Create(sub_shape_key, sub_shape_key_hash, 0);
+		MKeyValue *m_kv = Create(contact_allocator, sub_shape_key, sub_shape_key_hash, 0);
 		if (m_kv == nullptr)
 		{
 			// Out of cache space
@@ -589,13 +594,10 @@ void ContactConstraintManager::PrepareConstraintBuffer(PhysicsUpdateContext *inC
 	// Allocate temporary constraint buffer
 	JPH_ASSERT(mConstraints == nullptr);
 	mConstraints = (ContactConstraint *)inContext->mTempAllocator->Allocate(mMaxConstraints * sizeof(ContactConstraint));
-
-	// Use the amount of contacts from the last iteration to determine the amount of buckets to use in the hash map for this frame
-	mCache[mCacheWriteIdx].Prepare(mCache[mCacheWriteIdx ^ 1]);
 }
 
 // Get the orientation of body 2 in local space of body 1
-static void sGetRelativeOrientation(const Body &inBody1, const Body &inBody2, Vec3 &outDeltaPosition, Quat &outDeltaRotation)
+static JPH_INLINE void sGetRelativeOrientation(const Body &inBody1, const Body &inBody2, Vec3 &outDeltaPosition, Quat &outDeltaRotation)
 {
 	Quat inv_r1 = inBody1.GetRotation().Conjugated();
 
@@ -610,7 +612,7 @@ static void sGetRelativeOrientation(const Body &inBody1, const Body &inBody2, Ve
 	outDeltaPosition = inv_r1 * (inBody2.GetCenterOfMassPosition() - inBody1.GetCenterOfMassPosition());
 }
 
-void ContactConstraintManager::GetContactsFromCache(Body &inBody1, Body &inBody2, bool &outPairHandled, bool &outContactFound)
+void ContactConstraintManager::GetContactsFromCache(ContactAllocator &ioContactAllocator, Body &inBody1, Body &inBody2, bool &outPairHandled, bool &outContactFound)
 {
 	JPH_PROFILE_FUNCTION();
 
@@ -663,7 +665,7 @@ void ContactConstraintManager::GetContactsFromCache(Body &inBody1, Body &inBody2
 
 	// Copy the cached body pair to this frame
 	ManifoldCache &write_cache = mCache[mCacheWriteIdx];
-	BPKeyValue *output_bp_kv = write_cache.Create(body_pair_key, body_pair_hash);
+	BPKeyValue *output_bp_kv = write_cache.Create(ioContactAllocator, body_pair_key, body_pair_hash);
 	if (output_bp_kv == nullptr)
 		return; // Out of cache space
 	CachedBodyPair *output_cbp = &output_bp_kv->GetValue();
@@ -698,7 +700,7 @@ void ContactConstraintManager::GetContactsFromCache(Body &inBody1, Body &inBody2
 
 		// Create room for manifold in write buffer and copy data
 		size_t input_hash = std::hash<SubShapeIDPair> {} (input_key);
-		MKeyValue *output_kv = write_cache.Create(input_key, input_hash, input_cm.mNumContactPoints);
+		MKeyValue *output_kv = write_cache.Create(ioContactAllocator, input_key, input_hash, input_cm.mNumContactPoints);
 		if (output_kv == nullptr)
 			break; // Out of cache space
 		CachedManifold *output_cm = &output_kv->GetValue();
@@ -802,7 +804,7 @@ void ContactConstraintManager::GetContactsFromCache(Body &inBody1, Body &inBody2
 	output_cbp->mFirstCachedManifold = output_handle;
 }
 
-ContactConstraintManager::BodyPairHandle ContactConstraintManager::AddBodyPair(const Body &inBody1, const Body &inBody2)
+ContactConstraintManager::BodyPairHandle ContactConstraintManager::AddBodyPair(ContactAllocator &ioContactAllocator, const Body &inBody1, const Body &inBody2)
 {
 	JPH_PROFILE_FUNCTION();
 
@@ -822,7 +824,7 @@ ContactConstraintManager::BodyPairHandle ContactConstraintManager::AddBodyPair(c
 	// Add an entry
 	BodyPair body_pair_key(body1->GetID(), body2->GetID());
 	size_t body_pair_hash = BodyPairHash {} (body_pair_key);
-	BPKeyValue *body_pair_kv = mCache[mCacheWriteIdx].Create(body_pair_key, body_pair_hash);
+	BPKeyValue *body_pair_kv = mCache[mCacheWriteIdx].Create(ioContactAllocator, body_pair_key, body_pair_hash);
 	if (body_pair_kv == nullptr)
 		return nullptr; // Out of cache space
 	CachedBodyPair *cbp = &body_pair_kv->GetValue();
@@ -840,7 +842,7 @@ ContactConstraintManager::BodyPairHandle ContactConstraintManager::AddBodyPair(c
 	return cbp;
 }
 
-void ContactConstraintManager::AddContactConstraint(BodyPairHandle inBodyPairHandle, Body &inBody1, Body &inBody2, const ContactManifold &inManifold)
+void ContactConstraintManager::AddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPairHandle, Body &inBody1, Body &inBody2, const ContactManifold &inManifold)
 {
 	JPH_PROFILE_FUNCTION();
 
@@ -877,7 +879,7 @@ void ContactConstraintManager::AddContactConstraint(BodyPairHandle inBodyPairHan
 	// Note that for dynamic vs dynamic we always require the first body to have a lower body id to get a consistent key
 	// under which to look up the contact
 	ManifoldCache &write_cache = mCache[mCacheWriteIdx];
-	MKeyValue *new_manifold_kv = write_cache.Create(key, key_hash, num_contact_points);
+	MKeyValue *new_manifold_kv = write_cache.Create(ioContactAllocator, key, key_hash, num_contact_points);
 	if (new_manifold_kv == nullptr)
 		return; // Out of cache space
 	CachedManifold *new_manifold = &new_manifold_kv->GetValue();
@@ -1032,7 +1034,7 @@ void ContactConstraintManager::AddContactConstraint(BodyPairHandle inBodyPairHan
 	cbp->mFirstCachedManifold = write_cache.ToHandle(new_manifold_kv);
 }
 
-void ContactConstraintManager::OnCCDContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &outSettings)
+void ContactConstraintManager::OnCCDContactAdded(ContactAllocator &ioContactAllocator, const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &outSettings)
 {
 	JPH_ASSERT(inManifold.mWorldSpaceNormal.IsNormalized());
 
@@ -1067,7 +1069,7 @@ void ContactConstraintManager::OnCCDContactAdded(const Body &inBody1, const Body
 
 		// Check if we already created this contact this physics update
 		ManifoldCache &write_cache = mCache[mCacheWriteIdx];
-		MKVAndCreated new_manifold_kv = write_cache.FindOrCreate(key, key_hash, 0);
+		MKVAndCreated new_manifold_kv = write_cache.FindOrCreate(ioContactAllocator, key, key_hash, 0);
 		if (new_manifold_kv.second)
 		{
 			// This contact is new for this physics update, check if previous update we already had this contact.
@@ -1117,15 +1119,25 @@ void ContactConstraintManager::SortContacts(uint32 *inConstraintIdxBegin, uint32
 	});
 }
 
-void ContactConstraintManager::FinalizeContactCache()
+void ContactConstraintManager::FinalizeContactCache(uint inExpectedNumBodyPairs, uint inExpectedNumManifolds)
 {
 	JPH_PROFILE_FUNCTION();
 
-	// Buffers are now complete, make write buffer the read buffer
 #ifdef JPH_ENABLE_ASSERTS
-	mCache[mCacheWriteIdx].Finalize();
+	// Mark cache as finalized
+	ManifoldCache &old_write_cache = mCache[mCacheWriteIdx];
+	old_write_cache.Finalize();
+
+	// Check that the count of body pairs and manifolds that we tracked outside of the cache (to avoid contention on an atomic) is correct
+	JPH_ASSERT(old_write_cache.GetNumBodyPairs() == inExpectedNumBodyPairs);
+	JPH_ASSERT(old_write_cache.GetNumManifolds() == inExpectedNumManifolds);
 #endif
+
+	// Buffers are now complete, make write buffer the read buffer
 	mCacheWriteIdx ^= 1;
+
+	// Use the amount of contacts from the last iteration to determine the amount of buckets to use in the hash map for the next iteration
+	mCache[mCacheWriteIdx].Prepare(inExpectedNumBodyPairs, inExpectedNumManifolds);
 }
 
 void ContactConstraintManager::ContactPointRemovedCallbacks()
@@ -1323,9 +1335,6 @@ bool ContactConstraintManager::SolvePositionConstraints(const uint32 *inConstrai
 
 void ContactConstraintManager::RecycleConstraintBuffer()
 {
-	// Use the amount of contacts from the last iteration to determine the amount of buckets to use in the hash map for this frame
-	mCache[mCacheWriteIdx].Prepare(mCache[mCacheWriteIdx ^ 1]);
-
 	// Reset constraint array
 	mNumConstraints = 0;
 }

Jolt/Physics/Constraints/ContactConstraintManager.h

@@ -63,17 +63,30 @@ public:
 	/// Max 4 contact points are needed for a stable manifold
 	static const int			MaxContactPoints = 4;
 
+	/// Contacts are allocated in a lock free hash map
+	class ContactAllocator : public LFHMAllocatorContext
+	{
+	public:
+		using LFHMAllocatorContext::LFHMAllocatorContext;
+
+		uint					mNumBodyPairs = 0;													///< Total number of body pairs added using this allocator
+		uint					mNumManifolds = 0;													///< Total number of manifolds added using this allocator
+	};
+
+	/// Get a new allocator context for storing contacts. Note that you should call this once and then add multiple contacts using the context.
+	ContactAllocator			GetContactAllocator()												{ return mCache[mCacheWriteIdx].GetContactAllocator(); }
+
 	/// Check if the contact points from the previous frame are reusable and if so copy them.
 	/// When the cache was usable and the pair has been handled: outPairHandled = true.
 	/// When a contact was produced: outContactFound = true.
-	void						GetContactsFromCache(Body &inBody1, Body &inBody2, bool &outPairHandled, bool &outContactFound);
+	void						GetContactsFromCache(ContactAllocator &ioContactAllocator, Body &inBody1, Body &inBody2, bool &outPairHandled, bool &outContactFound);
 
 	/// Handle used to keep track of the current body pair
 	using BodyPairHandle = void *;
 
 	/// Create a handle for a colliding body pair so that contact constraints can be added between them.
 	/// Needs to be called once per body pair per frame before calling AddContactConstraint.
-	BodyPairHandle				AddBodyPair(const Body &inBody1, const Body &inBody2);
+	BodyPairHandle				AddBodyPair(ContactAllocator &ioContactAllocator, const Body &inBody1, const Body &inBody2);
 
 	/// Add a contact constraint for this frame.
 	///
@@ -114,11 +127,12 @@ public:
 	/// r1, r2 = contact point relative to center of mass of body 1 and body 2 (r1 = p1 - x1, r2 = p2 - x2).
 	/// v1, v2 = (linear velocity, angular velocity): 6 vectors containing linear and angular velocity for body 1 and 2.
 	/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].
-	void						AddContactConstraint(BodyPairHandle inBodyPair, Body &inBody1, Body &inBody2, const ContactManifold &inManifold);
+	void						AddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPair, Body &inBody1, Body &inBody2, const ContactManifold &inManifold);
 
 	/// Finalizes the contact cache, the contact cache that was generated during the calls to AddContactConstraint in this update
-	/// will be used from now on to read from
-	void						FinalizeContactCache();
+	/// will be used from now on to read from.
+	/// inExpectedNumBodyPairs / inExpectedNumManifolds are the amount of body pairs / manifolds found in the previous step and is used to determine the amount of buckets the contact cache hash map will use.
+	void						FinalizeContactCache(uint inExpectedNumBodyPairs, uint inExpectedNumManifolds);
 
 	/// Notifies the listener of any contact points that were removed. Needs to be callsed after FinalizeContactCache().
 	void						ContactPointRemovedCallbacks();
@@ -199,7 +213,7 @@ public:
 	/// @param inBody2 The second body that is colliding
 	/// @param inManifold The manifold that describes the collision
 	/// @param outSettings The calculated contact settings (may be overridden by the contact listener)
-	void						OnCCDContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &outSettings);
+	void						OnCCDContactAdded(ContactAllocator &ioContactAllocator, const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &outSettings);
 
 #ifdef JPH_DEBUG_RENDERER
 	// Drawing properties
@@ -320,25 +334,35 @@ private:
 		/// Reset all entries from the cache
 		void					Clear();
 
-		/// Prepare cache before creating new contacts. Uses previous frame to determine amount of buckets in the hash map.
-		void					Prepare(const ManifoldCache &inReadCache);
+		/// Prepare cache before creating new contacts. 
+		/// inExpectedNumBodyPairs / inExpectedNumManifolds are the amount of body pairs / manifolds found in the previous step and is used to determine the amount of buckets the contact cache hash map will use.
+		void					Prepare(uint inExpectedNumBodyPairs, uint inExpectedNumManifolds);
+
+		/// Get a new allocator context for storing contacts. Note that you should call this once and then add multiple contacts using the context.
+		ContactAllocator		GetContactAllocator()						{ return ContactAllocator(mAllocator, cAllocatorBlockSize); }
 
 		/// Find / create cached entry for SubShapeIDPair -> CachedManifold
 		const MKeyValue *		Find(const SubShapeIDPair &inKey, size_t inKeyHash) const;
-		MKeyValue *				Create(const SubShapeIDPair &inKey, size_t inKeyHash, int inNumContactPoints);
-		MKVAndCreated			FindOrCreate(const SubShapeIDPair &inKey, size_t inKeyHash, int inNumContactPoints);
+		MKeyValue *				Create(ContactAllocator &ioContactAllocator, const SubShapeIDPair &inKey, size_t inKeyHash, int inNumContactPoints);
+		MKVAndCreated			FindOrCreate(ContactAllocator &ioContactAllocator, const SubShapeIDPair &inKey, size_t inKeyHash, int inNumContactPoints);
 		uint32					ToHandle(const MKeyValue *inKeyValue) const;
 		const MKeyValue *		FromHandle(uint32 inHandle) const;
 
 		/// Find / create entry for BodyPair -> CachedBodyPair
 		const BPKeyValue *		Find(const BodyPair &inKey, size_t inKeyHash) const;
-		BPKeyValue *			Create(const BodyPair &inKey, size_t inKeyHash);
+		BPKeyValue *			Create(ContactAllocator &ioContactAllocator, const BodyPair &inKey, size_t inKeyHash);
 		void					GetAllBodyPairsSorted(vector<const BPKeyValue *> &outAll) const;
 		void					GetAllManifoldsSorted(const CachedBodyPair &inBodyPair, vector<const MKeyValue *> &outAll) const;
 		void					GetAllCCDManifoldsSorted(vector<const MKeyValue *> &outAll) const;
 		void					ContactPointRemovedCallbacks(ContactListener *inListener);
 
 #ifdef JPH_ENABLE_ASSERTS
+		/// Get the amount of manifolds in the cache
+		uint					GetNumManifolds() const						{ return mCachedManifolds.GetNumKeyValues(); }
+
+		/// Get the amount of body pairs in the cache
+		uint					GetNumBodyPairs() const						{ return mCachedBodyPairs.GetNumKeyValues(); }
+
 		/// Before a cache is finalized you can only do Create(), after only Find() or Clear()
 		void					Finalize();
 #endif
@@ -348,11 +372,17 @@ private:
 		bool					RestoreState(const ManifoldCache &inReadCache, StateRecorder &inStream);
 
 	private:
+		/// Block size used when allocating new blocks in the contact cache
+		static constexpr uint32	cAllocatorBlockSize = 4096;
+
+		/// Allocator used by both mCachedManifolds and mCachedBodyPairs, this makes it more likely that a body pair and its manifolds are close in memory
+		LFHMAllocator			mAllocator;
+
 		/// Simple hash map for SubShapeIDPair -> CachedManifold
-		ManifoldMap				mCachedManifolds;
+		ManifoldMap				mCachedManifolds { mAllocator };
 
 		/// Simple hash map for BodyPair -> CachedBodyPair
-		BodyPairMap				mCachedBodyPairs;
+		BodyPairMap				mCachedBodyPairs { mAllocator };
 
 #ifdef JPH_ENABLE_ASSERTS
 		bool					mIsFinalized = false;						///< Marks if this buffer is complete

Jolt/Physics/PhysicsSystem.cpp

@@ -342,7 +342,7 @@ void PhysicsSystem::Update(float inDeltaTime, int inCollisionSteps, int inIntegr
 			// This job will call the contact removed callbacks
 			step.mContactRemovedCallbacks = inJobSystem->CreateJob("ContactRemovedCallbacks", cColorContactRemovedCallbacks, [&context, &step]()
 				{
-					context.mPhysicsSystem->JobContactRemovedCallbacks();
+					context.mPhysicsSystem->JobContactRemovedCallbacks(&step);
 
 					if (step.mStartNextStep.IsValid())
 						step.mStartNextStep.RemoveDependency();
@@ -797,6 +797,9 @@ void PhysicsSystem::JobFindCollisions(PhysicsUpdateContext::Step *ioStep, int in
 	BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::Read);
 #endif
 
+	// Allocation context for allocating new contact points
+	ContactAllocator contact_allocator(mContactManager.GetContactAllocator());
+
 	// Determine initial queue to read pairs from if no broadphase work can be done
 	// (always start looking at results from the next job)
 	int read_queue_idx = (inJobIndex + 1) % ioStep->mBodyPairQueues.size();
@@ -817,8 +820,9 @@ void PhysicsSystem::JobFindCollisions(PhysicsUpdateContext::Step *ioStep, int in
 				{
 				public:
 					// Constructor
-											MyBodyPairCallback(PhysicsUpdateContext::Step *inStep, int inJobIndex) :
+											MyBodyPairCallback(PhysicsUpdateContext::Step *inStep, ContactAllocator &ioContactAllocator, int inJobIndex) :
 						mStep(inStep),
+						mContactAllocator(ioContactAllocator),
 						mJobIndex(inJobIndex)
 					{
 					}
@@ -832,7 +836,7 @@ void PhysicsSystem::JobFindCollisions(PhysicsUpdateContext::Step *ioStep, int in
 						if (body_pairs_in_queue >= mStep->mMaxBodyPairsPerQueue)
 						{
 							// Buffer full, process the pair now
-							mStep->mContext->mPhysicsSystem->ProcessBodyPair(inPair);
+							mStep->mContext->mPhysicsSystem->ProcessBodyPair(mContactAllocator, inPair);
 						}
 						else
 						{
@@ -844,9 +848,10 @@ void PhysicsSystem::JobFindCollisions(PhysicsUpdateContext::Step *ioStep, int in
 
 				private:
 					PhysicsUpdateContext::Step *	mStep;
+					ContactAllocator &				mContactAllocator;
 					int								mJobIndex;
 				};
-				MyBodyPairCallback add_pair(ioStep, inJobIndex);
+				MyBodyPairCallback add_pair(ioStep, contact_allocator, inJobIndex);
 
 				// Copy active bodies to temporary array, broadphase will reorder them
 				uint32 batch_size = active_bodies_read_idx_end - active_bodies_read_idx;
@@ -884,6 +889,10 @@ void PhysicsSystem::JobFindCollisions(PhysicsUpdateContext::Step *ioStep, int in
 					// If we're back at the first queue, we've looked at all of them and found nothing
 					if (read_queue_idx == first_read_queue_idx)
 					{
+						// Atomically accumulate the number of found manifolds and body pairs
+						ioStep->mNumBodyPairs += contact_allocator.mNumBodyPairs;
+						ioStep->mNumManifolds += contact_allocator.mNumManifolds;
+
 						// Mark this job as inactive
 						ioStep->mActiveFindCollisionJobs.fetch_and(~PhysicsUpdateContext::JobMask(1 << inJobIndex));
 
@@ -904,7 +913,7 @@ void PhysicsSystem::JobFindCollisions(PhysicsUpdateContext::Step *ioStep, int in
 				if (queue.mReadIdx.compare_exchange_strong(pair_idx, pair_idx + 1))
 				{
 					// Process the actual body pair
-					ProcessBodyPair(bp);
+					ProcessBodyPair(contact_allocator, bp);
 					break;
 				}
 			}
@@ -912,7 +921,7 @@ void PhysicsSystem::JobFindCollisions(PhysicsUpdateContext::Step *ioStep, int in
 	}
 }
 
-void PhysicsSystem::ProcessBodyPair(const BodyPair &inBodyPair)
+void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const BodyPair &inBodyPair)
 {
 	JPH_PROFILE_FUNCTION();
 
@@ -940,14 +949,14 @@ void PhysicsSystem::ProcessBodyPair(const BodyPair &inBodyPair)
 	// Check if the contact points from the previous frame are reusable and if so copy them
 	bool pair_handled = false, contact_found = false;
 	if (mPhysicsSettings.mUseBodyPairContactCache && !(body1->IsCollisionCacheInvalid() || body2->IsCollisionCacheInvalid()))
-		mContactManager.GetContactsFromCache(*body1, *body2, pair_handled, contact_found);
+		mContactManager.GetContactsFromCache(ioContactAllocator, *body1, *body2, pair_handled, contact_found);
 
 	// If the cache hasn't handled this body pair do actual collision detection
 	if (!pair_handled)
 	{
 		// Create entry in the cache for this body pair
 		// Needs to happen irrespective if we found a collision or not (we want to remember that no collision was found too)
-		ContactConstraintManager::BodyPairHandle body_pair_handle = mContactManager.AddBodyPair(*body1, *body2);
+		ContactConstraintManager::BodyPairHandle body_pair_handle = mContactManager.AddBodyPair(ioContactAllocator, *body1, *body2);
 		if (body_pair_handle == nullptr)
 			return; // Out of cache space
 
@@ -1087,7 +1096,7 @@ void PhysicsSystem::ProcessBodyPair(const BodyPair &inBodyPair)
 					PruneContactPoints(body1->GetCenterOfMassPosition(), manifold.mWorldSpaceNormal, manifold.mWorldSpaceContactPointsOn1, manifold.mWorldSpaceContactPointsOn2);
 
 				// Actually add the contact points to the manager
-				mContactManager.AddContactConstraint(body_pair_handle, *body1, *body2, manifold);
+				mContactManager.AddContactConstraint(ioContactAllocator, body_pair_handle, *body1, *body2, manifold);
 			}
 
 			contact_found = !collector.mManifolds.empty();
@@ -1100,8 +1109,9 @@ void PhysicsSystem::ProcessBodyPair(const BodyPair &inBodyPair)
 			class NonReductionCollideShapeCollector : public CollideShapeCollector
 			{
 			public:
-								NonReductionCollideShapeCollector(PhysicsSystem *inSystem, Body *inBody1, Body *inBody2, const ContactConstraintManager::BodyPairHandle &inPairHandle) : 
+								NonReductionCollideShapeCollector(PhysicsSystem *inSystem, ContactAllocator &ioContactAllocator, Body *inBody1, Body *inBody2, const ContactConstraintManager::BodyPairHandle &inPairHandle) : 
 					mSystem(inSystem), 
+					mContactAllocator(ioContactAllocator),
 					mBody1(inBody1),
 					mBody2(inBody2),
 					mBodyPairHandle(inPairHandle)
@@ -1159,20 +1169,21 @@ void PhysicsSystem::ProcessBodyPair(const BodyPair &inBodyPair)
 					manifold.mSubShapeID2 = inResult.mSubShapeID2;
 
 					// Actually add the contact points to the manager
-					mSystem->mContactManager.AddContactConstraint(mBodyPairHandle, *mBody1, *mBody2, manifold);
+					mSystem->mContactManager.AddContactConstraint(mContactAllocator, mBodyPairHandle, *mBody1, *mBody2, manifold);
 
 					// Mark contact found
 					mContactFound = true;
 				}
 
 				PhysicsSystem *		mSystem;
+				ContactAllocator &	mContactAllocator;
 				Body *				mBody1;
 				Body *				mBody2;
 				ContactConstraintManager::BodyPairHandle mBodyPairHandle;
 				bool				mValidateBodyPair = true;
 				bool				mContactFound = false;
 			};
-			NonReductionCollideShapeCollector collector(this, body1, body2, body_pair_handle);
+			NonReductionCollideShapeCollector collector(this, ioContactAllocator, body1, body2, body_pair_handle);
 
 			// Perform collision detection between the two shapes
 			SubShapeIDCreator part1, part2;
@@ -1542,6 +1553,9 @@ void PhysicsSystem::JobFindCCDContacts(const PhysicsUpdateContext *ioContext, Ph
 	BodyAccess::Grant grant(BodyAccess::EAccess::Read, BodyAccess::EAccess::Read);
 #endif
 
+	// Allocation context for allocating new contact points
+	ContactAllocator contact_allocator(mContactManager.GetContactAllocator());
+
 	// Settings
 	ShapeCastSettings settings;
 	settings.mUseShrunkenShapeAndConvexRadius = true;
@@ -1784,7 +1798,7 @@ void PhysicsSystem::JobFindCCDContacts(const PhysicsUpdateContext *ioContext, Ph
 			manifold.mWorldSpaceNormal = ccd_body.mContactNormal;
 
 			// Call contact point callbacks
-			mContactManager.OnCCDContactAdded(body, body2, manifold, ccd_body.mContactSettings);
+			mContactManager.OnCCDContactAdded(contact_allocator, body, body2, manifold, ccd_body.mContactSettings);
 
 			// Calculate the average position from the manifold (this will result in the same impulse applied as when we apply impulses to all contact points)
 			if (manifold.mWorldSpaceContactPointsOn2.size() > 1)
@@ -1799,6 +1813,10 @@ void PhysicsSystem::JobFindCCDContacts(const PhysicsUpdateContext *ioContext, Ph
 				ccd_body.mContactPointOn2 = cast_shape_result.mContactPointOn2;
 		}
 	}
+
+	// Atomically accumulate the number of found manifolds and body pairs
+	ioSubStep->mStep->mNumBodyPairs += contact_allocator.mNumBodyPairs;
+	ioSubStep->mStep->mNumManifolds += contact_allocator.mNumManifolds;
 }
 
 void PhysicsSystem::JobResolveCCDContacts(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::SubStep *ioSubStep)
@@ -2010,7 +2028,7 @@ void PhysicsSystem::JobResolveCCDContacts(PhysicsUpdateContext *ioContext, Physi
 	ioSubStep->mCCDBodiesCapacity = 0;
 }
 
-void PhysicsSystem::JobContactRemovedCallbacks()
+void PhysicsSystem::JobContactRemovedCallbacks(const PhysicsUpdateContext::Step *ioStep)
 {
 #ifdef JPH_ENABLE_ASSERTS
 	// We don't touch any bodies
@@ -2024,7 +2042,7 @@ void PhysicsSystem::JobContactRemovedCallbacks()
 	mContactManager.ContactPointRemovedCallbacks();
 
 	// Finalize the contact cache (this swaps the read and write versions of the contact cache)
-	mContactManager.FinalizeContactCache();
+	mContactManager.FinalizeContactCache(ioStep->mNumBodyPairs, ioStep->mNumManifolds);
 }
 
 void PhysicsSystem::JobSolvePositionConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::SubStep *ioSubStep)

Jolt/Physics/PhysicsSystem.h

@@ -184,14 +184,16 @@ private:
 	void						JobPostIntegrateVelocity(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::SubStep *ioSubStep) const;
 	void						JobFindCCDContacts(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::SubStep *ioSubStep);
 	void						JobResolveCCDContacts(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::SubStep *ioSubStep);
-	void						JobContactRemovedCallbacks();
+	void						JobContactRemovedCallbacks(const PhysicsUpdateContext::Step *ioStep);
 	void						JobSolvePositionConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::SubStep *ioSubStep);
 
 	/// Tries to spawn a new FindCollisions job if max concurrency hasn't been reached yet
 	void						TrySpawnJobFindCollisions(PhysicsUpdateContext::Step *ioStep) const;
 
+	using ContactAllocator = ContactConstraintManager::ContactAllocator;
+
 	/// Process narrow phase for a single body pair
-	void						ProcessBodyPair(const BodyPair &inBodyPair);
+	void						ProcessBodyPair(ContactAllocator &ioContactAllocator, const BodyPair &inBodyPair);
 
 	/// Number of constraints to process at once in JobDetermineActiveConstraints
 	static constexpr int		cDetermineActiveConstraintsBatchSize = 64;

Jolt/Physics/PhysicsUpdateContext.h

@@ -114,6 +114,9 @@ public:
 
 		atomic<JobMask>		mActiveFindCollisionJobs;								///< A bitmask that indicates which jobs are still active
 
+		atomic<uint>		mNumBodyPairs { 0 };									///< The number of body pairs found in this step (used to size the contact cache in the next step)
+		atomic<uint>		mNumManifolds { 0 };									///< The number of manifolds found in this step (used to size the contact cache in the next step)
+
 		// Jobs in order of execution (some run in parallel)
 		JobHandle			mBroadPhasePrepare;										///< Prepares the new tree in the background
 		JobHandleArray		mStepListeners;											///< Listeners to notify of the beginning of a physics step