@bloomengine/engine 0.3.1

package/native/third_party/JoltPhysics/Jolt/Physics/LargeIslandSplitter.h

@@ -0,0 +1,187 @@
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/NonCopyable.h>
+#include <Jolt/Core/Atomics.h>
+
+JPH_NAMESPACE_BEGIN
+
+class Body;
+class BodyID;
+class IslandBuilder;
+class TempAllocator;
+class Constraint;
+class BodyManager;
+class ContactConstraintManager;
+class CalculateSolverSteps;
+
+/// Assigns bodies in large islands to multiple groups that can run in parallel
+///
+/// This basically implements what is described in: High-Performance Physical Simulations on Next-Generation Architecture with Many Cores by Chen et al.
+/// See: http://web.eecs.umich.edu/~msmelyan/papers/physsim_onmanycore_itj.pdf section "PARALLELIZATION METHODOLOGY"
+///
+/// WARNING: This class is an internal part of PhysicsSystem, it has no functions that can be called by users of the library.
+class LargeIslandSplitter : public NonCopyable
+{
+private:
+	using					SplitMask = uint32;
+
+public:
+	static constexpr uint	cNumSplits = sizeof(SplitMask) * 8;
+	static constexpr uint	cNonParallelSplitIdx = cNumSplits - 1;
+	static constexpr uint	cLargeIslandTreshold = 128;							///< If the number of constraints + contacts in an island is larger than this, we will try to split the island
+
+	/// Status code for retrieving a batch
+	enum class EStatus
+	{
+		WaitingForBatch,														///< Work is expected to be available later
+		BatchRetrieved,															///< Work is being returned
+		AllBatchesDone,															///< No further work is expected from this
+	};
+
+	/// Describes a split of constraints and contacts
+	struct Split
+	{
+		inline uint			GetNumContacts() const								{ return mContactBufferEnd - mContactBufferBegin; }
+		inline uint			GetNumConstraints() const							{ return mConstraintBufferEnd - mConstraintBufferBegin; }
+		inline uint			GetNumItems() const									{ return GetNumContacts() + GetNumConstraints(); }
+
+		uint32				mContactBufferBegin;								///< Begin of the contact buffer (offset relative to mContactAndConstraintIndices)
+		uint32				mContactBufferEnd;									///< End of the contact buffer
+
+		uint32				mConstraintBufferBegin;								///< Begin of the constraint buffer (offset relative to mContactAndConstraintIndices)
+		uint32				mConstraintBufferEnd;								///< End of the constraint buffer
+	};
+
+	/// Structure that describes the resulting splits from the large island splitter
+	class Splits
+	{
+	public:
+		inline uint			GetNumSplits() const
+		{
+			return mNumSplits;
+		}
+
+		inline void			GetConstraintsInSplit(uint inSplitIndex, uint32 &outConstraintsBegin, uint32 &outConstraintsEnd) const
+		{
+			const Split &split = mSplits[inSplitIndex];
+			outConstraintsBegin = split.mConstraintBufferBegin;
+			outConstraintsEnd = split.mConstraintBufferEnd;
+		}
+
+		inline void			GetContactsInSplit(uint inSplitIndex, uint32 &outContactsBegin, uint32 &outContactsEnd) const
+		{
+			const Split &split = mSplits[inSplitIndex];
+			outContactsBegin = split.mContactBufferBegin;
+			outContactsEnd = split.mContactBufferEnd;
+		}
+
+		/// Reset current status so that no work can be picked up from this split
+		inline void			ResetStatus()
+		{
+			mStatus.store(StatusItemMask, memory_order_relaxed);
+		}
+
+		/// Make the first batch available to other threads
+		inline void			StartFirstBatch()
+		{
+			uint split_index = mNumSplits > 0? 0 : cNonParallelSplitIdx;
+			mStatus.store(uint64(split_index) << StatusSplitShift, memory_order_release);
+		}
+
+		/// Fetch the next batch to process
+		EStatus				FetchNextBatch(uint32 &outConstraintsBegin, uint32 &outConstraintsEnd, uint32 &outContactsBegin, uint32 &outContactsEnd, bool &outFirstIteration);
+
+		/// Mark a batch as processed
+		void				MarkBatchProcessed(uint inNumProcessed, bool &outLastIteration, bool &outFinalBatch);
+
+		enum EIterationStatus : uint64
+		{
+			StatusIterationMask		= 0xffff000000000000,
+			StatusIterationShift	= 48,
+			StatusSplitMask			= 0x0000ffff00000000,
+			StatusSplitShift		= 32,
+			StatusItemMask			= 0x00000000ffffffff,
+		};
+
+		static inline int	sGetIteration(uint64 inStatus)
+		{
+			return int((inStatus & StatusIterationMask) >> StatusIterationShift);
+		}
+
+		static inline uint	sGetSplit(uint64 inStatus)
+		{
+			return uint((inStatus & StatusSplitMask) >> StatusSplitShift);
+		}
+
+		static inline uint	sGetItem(uint64 inStatus)
+		{
+			return uint(inStatus & StatusItemMask);
+		}
+
+		Split				mSplits[cNumSplits];								///< Data per split
+		uint32				mIslandIndex;										///< Index of the island that was split
+		uint				mNumSplits;											///< Number of splits that were created (excluding the non-parallel split)
+		int					mNumIterations;										///< Number of iterations to do
+		int					mNumVelocitySteps;									///< Number of velocity steps to do (cached for 2nd sub step)
+		int					mNumPositionSteps;									///< Number of position steps to do
+		atomic<uint64>		mStatus;											///< Status of the split, see EIterationStatus
+		atomic<uint>		mItemsProcessed;									///< Number of items that have been marked as processed
+	};
+
+public:
+	/// Destructor
+							~LargeIslandSplitter();
+
+	/// Prepare the island splitter by allocating memory
+	void					Prepare(const IslandBuilder &inIslandBuilder, uint32 inNumActiveBodies, TempAllocator *inTempAllocator);
+
+	/// Assign two bodies to a split. Returns the split index.
+	uint					AssignSplit(const Body *inBody1, const Body *inBody2);
+
+	/// Force a body to be in a non parallel split. Returns the split index.
+	uint					AssignToNonParallelSplit(const Body *inBody);
+
+	/// Splits up an island, the created splits will be added to the list of batches and can be fetched with FetchNextBatch. Returns false if the island did not need splitting.
+	bool					SplitIsland(uint32 inIslandIndex, const IslandBuilder &inIslandBuilder, const BodyManager &inBodyManager, const ContactConstraintManager &inContactManager, Constraint **inActiveConstraints, CalculateSolverSteps &ioStepsCalculator);
+
+	/// Fetch the next batch to process, returns a handle in outSplitIslandIndex that must be provided to MarkBatchProcessed when complete
+	EStatus					FetchNextBatch(uint &outSplitIslandIndex, uint32 *&outConstraintsBegin, uint32 *&outConstraintsEnd, uint32 *&outContactsBegin, uint32 *&outContactsEnd, bool &outFirstIteration);
+
+	/// Mark a batch as processed
+	void					MarkBatchProcessed(uint inSplitIslandIndex, const uint32 *inConstraintsBegin, const uint32 *inConstraintsEnd, const uint32 *inContactsBegin, const uint32 *inContactsEnd, bool &outLastIteration, bool &outFinalBatch);
+
+	/// Get the island index of the island that was split for a particular split island index
+	inline uint32			GetIslandIndex(uint inSplitIslandIndex) const
+	{
+		JPH_ASSERT(inSplitIslandIndex < mNumSplitIslands);
+		return mSplitIslands[inSplitIslandIndex].mIslandIndex;
+	}
+
+	/// Prepare the island splitter for iterating over the split islands again for position solving. Marks all batches as startable.
+	void					PrepareForSolvePositions();
+
+	/// Reset the island splitter
+	void					Reset(TempAllocator *inTempAllocator);
+
+private:
+	static constexpr uint	cSplitCombineTreshold = 32;							///< If the number of constraints + contacts in a split is lower than this, we will merge this split into the 'non-parallel split'
+	static constexpr uint	cBatchSize = 16;									///< Number of items to process in a constraint batch
+
+	uint32					mNumActiveBodies = 0;								///< Cached number of active bodies
+
+	SplitMask *				mSplitMasks = nullptr;								///< Bits that indicate for each body in the BodyManager::mActiveBodies list which split they already belong to
+
+	uint32 *				mContactAndConstraintsSplitIdx = nullptr;			///< Buffer to store the split index per constraint or contact
+	uint32 *				mContactAndConstraintIndices = nullptr;				///< Buffer to store the ordered constraint indices per split
+	uint					mContactAndConstraintsSize = 0;						///< Total size of mContactAndConstraintsSplitIdx and mContactAndConstraintIndices
+	atomic<uint>			mContactAndConstraintsNextFree { 0 };				///< Next element that is free in both buffers
+
+	uint					mNumSplitIslands = 0;								///< Total number of islands that required splitting
+	Splits *				mSplitIslands = nullptr;							///< List of islands that required splitting
+	atomic<uint>			mNextSplitIsland = 0;								///< Next split island to pick from mSplitIslands
+};
+
+JPH_NAMESPACE_END

package/native/third_party/JoltPhysics/Jolt/Physics/PhysicsLock.h

@@ -0,0 +1,169 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/Mutex.h>
+
+JPH_NAMESPACE_BEGIN
+
+#ifdef JPH_ENABLE_ASSERTS
+
+/// This is the list of locks used by the physics engine, they need to be locked in a particular order (from top of the list to bottom of the list) in order to prevent deadlocks
+enum class EPhysicsLockTypes
+{
+	BroadPhaseQuery			= 1 << 0,
+	PerBody					= 1 << 1,
+	BodiesList				= 1 << 2,
+	BroadPhaseUpdate		= 1 << 3,
+	ConstraintsList			= 1 << 4,
+	ActiveBodiesList		= 1 << 5,
+};
+
+/// A token that indicates the context of a lock (we use 1 per physics system and we use the body manager pointer because it's convenient)
+class BodyManager;
+using PhysicsLockContext = const BodyManager *;
+
+#endif // !JPH_ENABLE_ASSERTS
+
+/// Helpers to safely lock the different mutexes that are part of the physics system while preventing deadlock
+/// Class that keeps track per thread which lock are taken and if the order of locking is correct
+class JPH_EXPORT PhysicsLock
+{
+public:
+#ifdef JPH_ENABLE_ASSERTS
+	/// Call before taking the lock
+	static inline void			sCheckLock(PhysicsLockContext inContext, EPhysicsLockTypes inType)
+	{
+		uint32 &mutexes = sGetLockedMutexes(inContext);
+		JPH_ASSERT(uint32(inType) > mutexes, "A lock of same or higher priority was already taken, this can create a deadlock!");
+		mutexes = mutexes | uint32(inType);
+	}
+
+	/// Call after releasing the lock
+	static inline void			sCheckUnlock(PhysicsLockContext inContext, EPhysicsLockTypes inType)
+	{
+		uint32 &mutexes = sGetLockedMutexes(inContext);
+		JPH_ASSERT((mutexes & uint32(inType)) != 0, "Mutex was not locked!");
+		mutexes = mutexes & ~uint32(inType);
+	}
+#endif // !JPH_ENABLE_ASSERTS
+
+	template <class LockType>
+	static inline void			sLock(LockType &inMutex JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType))
+	{
+		JPH_IF_ENABLE_ASSERTS(sCheckLock(inContext, inType);)
+		inMutex.lock();
+	}
+
+	template <class LockType>
+	static inline void			sUnlock(LockType &inMutex JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType))
+	{
+		JPH_IF_ENABLE_ASSERTS(sCheckUnlock(inContext, inType);)
+		inMutex.unlock();
+	}
+
+	template <class LockType>
+	static inline void			sLockShared(LockType &inMutex JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType))
+	{
+		JPH_IF_ENABLE_ASSERTS(sCheckLock(inContext, inType);)
+		inMutex.lock_shared();
+	}
+
+	template <class LockType>
+	static inline void			sUnlockShared(LockType &inMutex JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType))
+	{
+		JPH_IF_ENABLE_ASSERTS(sCheckUnlock(inContext, inType);)
+		inMutex.unlock_shared();
+	}
+
+#ifdef JPH_ENABLE_ASSERTS
+private:
+	struct LockData
+	{
+		uint32					mLockedMutexes = 0;
+		PhysicsLockContext		mContext = nullptr;
+	};
+
+	// Helper function to find the locked mutexes for a particular context
+	static uint32 &				sGetLockedMutexes(PhysicsLockContext inContext)
+	{
+		static thread_local LockData sLocks[4];
+
+		// If we find a matching context we can use it
+		for (LockData &l : sLocks)
+			if (l.mContext == inContext)
+				return l.mLockedMutexes;
+
+		// Otherwise we look for an entry that is not in use
+		for (LockData &l : sLocks)
+			if (l.mLockedMutexes == 0)
+			{
+				l.mContext = inContext;
+				return l.mLockedMutexes;
+			}
+
+		JPH_ASSERT(false, "Too many physics systems locked at the same time!");
+		return sLocks[0].mLockedMutexes;
+	}
+#endif // !JPH_ENABLE_ASSERTS
+};
+
+/// Helper class that is similar to std::unique_lock
+template <class LockType>
+class UniqueLock : public NonCopyable
+{
+public:
+	explicit					UniqueLock(LockType &inLock JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType)) :
+		mLock(inLock)
+#ifdef JPH_ENABLE_ASSERTS
+		, mContext(inContext),
+		mType(inType)
+#endif // JPH_ENABLE_ASSERTS
+	{
+		PhysicsLock::sLock(mLock JPH_IF_ENABLE_ASSERTS(, mContext, mType));
+	}
+
+								~UniqueLock()
+	{
+		PhysicsLock::sUnlock(mLock JPH_IF_ENABLE_ASSERTS(, mContext, mType));
+	}
+
+private:
+	LockType &					mLock;
+#ifdef JPH_ENABLE_ASSERTS
+	PhysicsLockContext			mContext;
+	EPhysicsLockTypes			mType;
+#endif // JPH_ENABLE_ASSERTS
+};
+
+/// Helper class that is similar to std::shared_lock
+template <class LockType>
+class SharedLock : public NonCopyable
+{
+public:
+	explicit					SharedLock(LockType &inLock JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType)) :
+		mLock(inLock)
+#ifdef JPH_ENABLE_ASSERTS
+		, mContext(inContext)
+		, mType(inType)
+#endif // JPH_ENABLE_ASSERTS
+	{
+		PhysicsLock::sLockShared(mLock JPH_IF_ENABLE_ASSERTS(, mContext, mType));
+	}
+
+								~SharedLock()
+	{
+		PhysicsLock::sUnlockShared(mLock JPH_IF_ENABLE_ASSERTS(, mContext, mType));
+	}
+
+private:
+	LockType &					mLock;
+#ifdef JPH_ENABLE_ASSERTS
+	PhysicsLockContext			mContext;
+	EPhysicsLockTypes			mType;
+#endif // JPH_ENABLE_ASSERTS
+};
+
+JPH_NAMESPACE_END

package/native/third_party/JoltPhysics/Jolt/Physics/PhysicsScene.cpp

@@ -0,0 +1,261 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/PhysicsScene.h>
+#include <Jolt/Physics/PhysicsSystem.h>
+#include <Jolt/Physics/Body/BodyLockMulti.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(PhysicsScene)
+{
+	JPH_ADD_ATTRIBUTE(PhysicsScene, mBodies)
+	JPH_ADD_ATTRIBUTE(PhysicsScene, mConstraints)
+	JPH_ADD_ATTRIBUTE(PhysicsScene, mSoftBodies)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(PhysicsScene::ConnectedConstraint)
+{
+	JPH_ADD_ATTRIBUTE(PhysicsScene::ConnectedConstraint, mSettings)
+	JPH_ADD_ATTRIBUTE(PhysicsScene::ConnectedConstraint, mBody1)
+	JPH_ADD_ATTRIBUTE(PhysicsScene::ConnectedConstraint, mBody2)
+}
+
+void PhysicsScene::AddBody(const BodyCreationSettings &inBody)
+{
+	mBodies.push_back(inBody);
+}
+
+void PhysicsScene::AddConstraint(const TwoBodyConstraintSettings *inConstraint, uint32 inBody1, uint32 inBody2)
+{
+	mConstraints.emplace_back(inConstraint, inBody1, inBody2);
+}
+
+void PhysicsScene::AddSoftBody(const SoftBodyCreationSettings &inSoftBody)
+{
+	mSoftBodies.push_back(inSoftBody);
+}
+
+bool PhysicsScene::FixInvalidScales()
+{
+	const Vec3 unit_scale = Vec3::sOne();
+
+	bool success = true;
+	for (BodyCreationSettings &b : mBodies)
+	{
+		// Test if there is an invalid scale in the shape hierarchy
+		const Shape *shape = b.GetShape();
+		if (!shape->IsValidScale(unit_scale))
+		{
+			// Fix it up
+			Shape::ShapeResult result = shape->ScaleShape(unit_scale);
+			if (result.IsValid())
+				b.SetShape(result.Get());
+			else
+				success = false;
+		}
+	}
+	return success;
+}
+
+bool PhysicsScene::CreateBodies(PhysicsSystem *inSystem) const
+{
+	BodyInterface &bi = inSystem->GetBodyInterface();
+
+	BodyIDVector body_ids;
+	body_ids.reserve(mBodies.size() + mSoftBodies.size());
+
+	// Create bodies
+	for (const BodyCreationSettings &b : mBodies)
+	{
+		const Body *body = bi.CreateBody(b);
+		if (body == nullptr)
+			break;
+		body_ids.push_back(body->GetID());
+	}
+
+	// Create soft bodies
+	for (const SoftBodyCreationSettings &b : mSoftBodies)
+	{
+		const Body *body = bi.CreateSoftBody(b);
+		if (body == nullptr)
+			break;
+		body_ids.push_back(body->GetID());
+	}
+
+	// Batch add bodies
+	BodyIDVector temp_body_ids = body_ids; // Body ID's get shuffled by AddBodiesPrepare
+	BodyInterface::AddState add_state = bi.AddBodiesPrepare(temp_body_ids.data(), (int)temp_body_ids.size());
+	bi.AddBodiesFinalize(temp_body_ids.data(), (int)temp_body_ids.size(), add_state, EActivation::Activate);
+
+	// If not all bodies are created, creating constraints will be unreliable
+	if (body_ids.size() != mBodies.size() + mSoftBodies.size())
+		return false;
+
+	// Create constraints
+	for (const ConnectedConstraint &cc : mConstraints)
+	{
+		BodyID body1_id = cc.mBody1 == cFixedToWorld? BodyID() : body_ids[cc.mBody1];
+		BodyID body2_id = cc.mBody2 == cFixedToWorld? BodyID() : body_ids[cc.mBody2];
+		Constraint *c = bi.CreateConstraint(cc.mSettings, body1_id, body2_id);
+		inSystem->AddConstraint(c);
+	}
+
+	// Everything was created
+	return true;
+}
+
+void PhysicsScene::SaveBinaryState(StreamOut &inStream, bool inSaveShapes, bool inSaveGroupFilter) const
+{
+	BodyCreationSettings::ShapeToIDMap shape_to_id;
+	BodyCreationSettings::MaterialToIDMap material_to_id;
+	BodyCreationSettings::GroupFilterToIDMap group_filter_to_id;
+	SoftBodyCreationSettings::SharedSettingsToIDMap settings_to_id;
+
+	// Save bodies
+	inStream.Write((uint32)mBodies.size());
+	for (const BodyCreationSettings &b : mBodies)
+		b.SaveWithChildren(inStream, inSaveShapes? &shape_to_id : nullptr, inSaveShapes? &material_to_id : nullptr, inSaveGroupFilter? &group_filter_to_id : nullptr);
+
+	// Save constraints
+	inStream.Write((uint32)mConstraints.size());
+	for (const ConnectedConstraint &cc : mConstraints)
+	{
+		cc.mSettings->SaveBinaryState(inStream);
+		inStream.Write(cc.mBody1);
+		inStream.Write(cc.mBody2);
+	}
+
+	// Save soft bodies
+	inStream.Write((uint32)mSoftBodies.size());
+	for (const SoftBodyCreationSettings &b : mSoftBodies)
+		b.SaveWithChildren(inStream, &settings_to_id, &material_to_id, inSaveGroupFilter? &group_filter_to_id : nullptr);
+}
+
+PhysicsScene::PhysicsSceneResult PhysicsScene::sRestoreFromBinaryState(StreamIn &inStream)
+{
+	PhysicsSceneResult result;
+
+	// Create scene
+	Ref<PhysicsScene> scene = new PhysicsScene();
+
+	BodyCreationSettings::IDToShapeMap id_to_shape;
+	BodyCreationSettings::IDToMaterialMap id_to_material;
+	BodyCreationSettings::IDToGroupFilterMap id_to_group_filter;
+	SoftBodyCreationSettings::IDToSharedSettingsMap id_to_settings;
+
+	// Reserve some memory to avoid frequent reallocations
+	id_to_shape.reserve(1024);
+	id_to_material.reserve(128);
+	id_to_group_filter.reserve(128);
+
+	// Read bodies
+	uint32 len = 0;
+	inStream.Read(len);
+	scene->mBodies.resize(len);
+	for (BodyCreationSettings &b : scene->mBodies)
+	{
+		// Read creation settings
+		BodyCreationSettings::BCSResult bcs_result = BodyCreationSettings::sRestoreWithChildren(inStream, id_to_shape, id_to_material, id_to_group_filter);
+		if (bcs_result.HasError())
+		{
+			result.SetError(bcs_result.GetError());
+			return result;
+		}
+		b = bcs_result.Get();
+	}
+
+	// Read constraints
+	len = 0;
+	inStream.Read(len);
+	scene->mConstraints.resize(len);
+	for (ConnectedConstraint &cc : scene->mConstraints)
+	{
+		ConstraintSettings::ConstraintResult c_result = ConstraintSettings::sRestoreFromBinaryState(inStream);
+		if (c_result.HasError())
+		{
+			result.SetError(c_result.GetError());
+			return result;
+		}
+		cc.mSettings = StaticCast<TwoBodyConstraintSettings>(c_result.Get());
+		inStream.Read(cc.mBody1);
+		inStream.Read(cc.mBody2);
+	}
+
+	// Read soft bodies
+	len = 0;
+	inStream.Read(len);
+	scene->mSoftBodies.resize(len);
+	for (SoftBodyCreationSettings &b : scene->mSoftBodies)
+	{
+		// Read creation settings
+		SoftBodyCreationSettings::SBCSResult sbcs_result = SoftBodyCreationSettings::sRestoreWithChildren(inStream, id_to_settings, id_to_material, id_to_group_filter);
+		if (sbcs_result.HasError())
+		{
+			result.SetError(sbcs_result.GetError());
+			return result;
+		}
+		b = sbcs_result.Get();
+	}
+
+	result.Set(scene);
+	return result;
+}
+
+void PhysicsScene::FromPhysicsSystem(const PhysicsSystem *inSystem)
+{
+	// This map will track where each body went in mBodies
+	using BodyIDToIdxMap = UnorderedMap<BodyID, uint32>;
+	BodyIDToIdxMap body_id_to_idx;
+
+	// Map invalid ID
+	body_id_to_idx[BodyID()] = cFixedToWorld;
+
+	// Get all bodies
+	BodyIDVector body_ids;
+	inSystem->GetBodies(body_ids);
+
+	// Loop over all bodies
+	const BodyLockInterface &bli = inSystem->GetBodyLockInterface();
+	for (const BodyID &id : body_ids)
+	{
+		BodyLockRead lock(bli, id);
+		if (lock.Succeeded())
+		{
+			// Store location of body
+			body_id_to_idx[id] = (uint32)mBodies.size();
+
+			const Body &body = lock.GetBody();
+
+			// Convert to body creation settings
+			if (body.IsRigidBody())
+				AddBody(body.GetBodyCreationSettings());
+			else
+				AddSoftBody(body.GetSoftBodyCreationSettings());
+		}
+	}
+
+	// Loop over all constraints
+	Constraints constraints = inSystem->GetConstraints();
+	for (const Constraint *c : constraints)
+		if (c->GetType() == EConstraintType::TwoBodyConstraint)
+		{
+			// Cast to two body constraint
+			const TwoBodyConstraint *tbc = static_cast<const TwoBodyConstraint *>(c);
+
+			// Find the body indices
+			BodyIDToIdxMap::const_iterator b1 = body_id_to_idx.find(tbc->GetBody1()->GetID());
+			BodyIDToIdxMap::const_iterator b2 = body_id_to_idx.find(tbc->GetBody2()->GetID());
+			JPH_ASSERT(b1 != body_id_to_idx.end() && b2 != body_id_to_idx.end());
+
+			// Create constraint settings and add the constraint
+			Ref<ConstraintSettings> settings = c->GetConstraintSettings();
+			AddConstraint(StaticCast<TwoBodyConstraintSettings>(settings), b1->second, b2->second);
+		}
+}
+
+JPH_NAMESPACE_END