Added possibility to save the current state of the physics world as a scene (#165)

* Added possibility to save the current state of the physics world as a scene
* Added ability to take and reload snapshot to samples app
* Added enabled flag to constraint settings
* Fixed bug in RotationEulerConstraintPart where the constraint could rotate 360 degrees (due to -quaternion representing the same rotation as quaternion)

.gitignore

@@ -3,6 +3,5 @@
 /profile_list_*.html
 /profile_chart_*.html
 /stats*.html
-/Docs/JoltPhysics.chm
-/Docs/JoltPhysics.chw
+/snapshot.bin
 /*.jor

Jolt/Physics/Body/BodyID.h

@@ -3,6 +3,8 @@
 
 #pragma once
 
+#include <Jolt/Core/HashCombine.h>
+
 JPH_NAMESPACE_BEGIN
 
 /// ID of a body. This is a way of reasoning about bodies in a multithreaded simulation while avoiding race conditions.
@@ -90,3 +92,6 @@ private:
 };
 
 JPH_NAMESPACE_END
+
+// Create a std::hash for BodyID
+JPH_MAKE_HASHABLE(JPH::BodyID, t.GetIndexAndSequenceNumber())

Jolt/Physics/Body/BodyInterface.cpp

@@ -157,6 +157,20 @@ bool BodyInterface::IsActive(const BodyID &inBodyID) const
 	return lock.Succeeded() && lock.GetBody().IsActive();
 }
 
+TwoBodyConstraint *BodyInterface::CreateConstraint(const TwoBodyConstraintSettings *inSettings, const BodyID &inBodyID1, const BodyID &inBodyID2)
+{
+	BodyID constraint_bodies[] = { inBodyID1, inBodyID2 };
+	BodyLockMultiWrite lock(*mBodyLockInterface, constraint_bodies, 2);
+
+	Body *body1 = lock.GetBody(0);
+	Body *body2 = lock.GetBody(1);
+
+	JPH_ASSERT(body1 != body2);
+	JPH_ASSERT(body1 != nullptr || body2 != nullptr);
+
+	return inSettings->Create(body1 != nullptr? *body1 : Body::sFixedToWorld, body2 != nullptr? *body2 : Body::sFixedToWorld);
+}
+
 void BodyInterface::ActivateConstraint(const TwoBodyConstraint *inConstraint)
 {
 	BodyID bodies[] = { inConstraint->GetBody1()->GetID(), inConstraint->GetBody2()->GetID() };

Jolt/Physics/Body/BodyInterface.h

@@ -20,6 +20,7 @@ class TransformedShape;
 class PhysicsMaterial;
 class SubShapeID;
 class Shape;
+class TwoBodyConstraintSettings;
 class TwoBodyConstraint;
 
 /// Class that provides operations on bodies using a body ID. Note that if you need to do multiple operations on a single body, it is more efficient to lock the body once and combine the operations.
@@ -77,6 +78,9 @@ public:
 	bool						IsActive(const BodyID &inBodyID) const;
 	///@}
 
+	/// Create a two body constraint
+	TwoBodyConstraint *			CreateConstraint(const TwoBodyConstraintSettings *inSettings, const BodyID &inBodyID1, const BodyID &inBodyID2);
+
 	/// Activate non-static bodies attached to a constraint
 	void						ActivateConstraint(const TwoBodyConstraint *inConstraint);
 

Jolt/Physics/Constraints/ConeConstraint.cpp

@@ -197,6 +197,19 @@ void ConeConstraint::RestoreState(StateRecorder &inStream)
 	inStream.Read(mWorldSpaceRotationAxis);
 }
 
+Ref<ConstraintSettings> ConeConstraint::GetConstraintSettings() const
+{
+	ConeConstraintSettings *settings = new ConeConstraintSettings;
+	ToConstraintSettings(*settings);
+	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
+	settings->mPoint1 = mLocalSpacePosition1;
+	settings->mTwistAxis1 = mLocalSpaceTwistAxis1;
+	settings->mPoint2 = mLocalSpacePosition2;
+	settings->mTwistAxis2 = mLocalSpaceTwistAxis2;
+	settings->mHalfConeAngle = acos(mCosHalfConeAngle);
+	return settings;
+}
+
 Mat44 ConeConstraint::GetConstraintToBody1Matrix() const 
 { 
 	Vec3 perp = mLocalSpaceTwistAxis1.GetNormalizedPerpendicular(); 

Jolt/Physics/Constraints/ConeConstraint.h

@@ -72,7 +72,7 @@ public:
 								ConeConstraint(Body &inBody1, Body &inBody2, const ConeConstraintSettings &inSettings);
 
 	// Generic interface of a constraint
-	virtual EConstraintType		GetType() const override					{ return EConstraintType::Cone; }
+	virtual EConstraintSubType	GetSubType() const override					{ return EConstraintSubType::Cone; }
 	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
 	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
 	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
@@ -83,6 +83,7 @@ public:
 #endif // JPH_DEBUG_RENDERER
 	virtual void				SaveState(StateRecorder &inStream) const override;
 	virtual void				RestoreState(StateRecorder &inStream) override;
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
 
 	// See: TwoBodyConstraint
 	virtual Mat44				GetConstraintToBody1Matrix() const override;

Jolt/Physics/Constraints/Constraint.cpp

@@ -16,18 +16,21 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(ConstraintSettings)
 {
 	JPH_ADD_BASE_CLASS(ConstraintSettings, SerializableObject)
 
+	JPH_ADD_ATTRIBUTE(ConstraintSettings, mEnabled)
 	JPH_ADD_ATTRIBUTE(ConstraintSettings, mDrawConstraintSize)
 }
 
 void ConstraintSettings::SaveBinaryState(StreamOut &inStream) const
 { 
 	inStream.Write(GetRTTI()->GetHash());
+	inStream.Write(mEnabled);
 	inStream.Write(mDrawConstraintSize);
 }
 
 void ConstraintSettings::RestoreBinaryState(StreamIn &inStream)
 {
 	// Type hash read by sRestoreFromBinaryState
+	inStream.Read(mEnabled);
 	inStream.Read(mDrawConstraintSize);
 }
 
@@ -75,4 +78,13 @@ void Constraint::RestoreState(StateRecorder &inStream)
 	inStream.Read(mEnabled);
 }
 
+void Constraint::ToConstraintSettings(ConstraintSettings &outSettings) const
+{
+	outSettings.mEnabled = mEnabled;
+
+#ifdef JPH_DEBUG_RENDERER
+	outSettings.mDrawConstraintSize = mDrawConstraintSize;
+#endif // JPH_DEBUG_RENDERER
+}
+
 JPH_NAMESPACE_END

Jolt/Physics/Constraints/Constraint.h

@@ -21,6 +21,13 @@ class DebugRenderer;
 
 /// Enum to identify constraint type
 enum class EConstraintType
+{
+	Constraint,
+	TwoBodyConstraint,
+};
+
+/// Enum to identify constraint sub type
+enum class EConstraintSubType
 {
 	Fixed,
 	Point,
@@ -61,6 +68,9 @@ public:
 	/// Creates a constraint of the correct type and restores its contents from the binary stream inStream.
 	static ConstraintResult		sRestoreFromBinaryState(StreamIn &inStream);
 
+	/// If this constraint is enabled initially. Use Constraint::SetEnabled to toggle after creation.
+	bool						mEnabled = true;
+
 	/// Size of constraint when drawing it through the debug renderer
 	float						mDrawConstraintSize = 1.0f;
 
@@ -74,10 +84,11 @@ class Constraint : public RefTarget<Constraint>, public NonCopyable
 {
 public:
 	/// Constructor
-	explicit					Constraint([[maybe_unused]] const ConstraintSettings &inSettings)
+	explicit					Constraint(const ConstraintSettings &inSettings) :
 #ifdef JPH_DEBUG_RENDERER
-									: mDrawConstraintSize(inSettings.mDrawConstraintSize)
+		mDrawConstraintSize(inSettings.mDrawConstraintSize),
 #endif // JPH_DEBUG_RENDERER
+		mEnabled(inSettings.mEnabled)
 	{
 	}
 
@@ -85,7 +96,10 @@ public:
 	virtual						~Constraint() = default;
 
 	/// Get the type of a constraint
-	virtual EConstraintType		GetType() const = 0;
+	virtual EConstraintType		GetType() const								{ return EConstraintType::Constraint; }
+
+	/// Get the sub type of a constraint
+	virtual EConstraintSubType	GetSubType() const = 0;
 
 	/// Enable / disable this constraint. This can e.g. be used to implement a breakable constraint by detecting that the constraint impulse
 	/// (see e.g. PointConstraint::GetTotalLambdaPosition) went over a certain limit and then disabling the constraint.
@@ -125,7 +139,13 @@ public:
 	/// Restoring state for replay
 	virtual void				RestoreState(StateRecorder &inStream);
 
+	/// Debug function to convert a constraint to its settings, note that this will not save to which bodies the constraint is connected to
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const = 0;
+
 protected:
+	/// Helper function to copy settings back to constraint settings for this base class
+	void						ToConstraintSettings(ConstraintSettings &outSettings) const;
+
 #ifdef JPH_DEBUG_RENDERER
 	/// Size of constraint when drawing it through the debug renderer
 	float						mDrawConstraintSize;

Jolt/Physics/Constraints/ConstraintManager.cpp

@@ -57,6 +57,14 @@ void ConstraintManager::Remove(Constraint **inConstraints, int inNumber)
 	}
 }
 
+Constraints ConstraintManager::GetConstraints() const
+{
+	UniqueLock lock(mConstraintsMutex, EPhysicsLockTypes::ConstraintsList);
+
+	Constraints copy = mConstraints;
+	return copy;
+}
+
 void ConstraintManager::GetActiveConstraints(uint32 inStartConstraintIdx, uint32 inEndConstraintIdx, Constraint **outActiveConstraints, uint32 &outNumActiveConstraints) const
 {
 	JPH_PROFILE_FUNCTION();

Jolt/Physics/Constraints/ConstraintManager.h

@@ -15,6 +15,9 @@ class BodyManager;
 class DebugRenderer;
 #endif // JPH_DEBUG_RENDERER
 
+/// A list of constraints
+using Constraints = vector<Ref<Constraint>>;
+
 /// A constraint manager manages all constraints of the same type
 class ConstraintManager : public NonCopyable
 {
@@ -27,6 +30,9 @@ public:
 	/// Note that the inConstraints array is allowed to have nullptrs, these will be ignored.
 	void					Remove(Constraint **inConstraint, int inNumber);
 
+	/// Get a list of all constraints
+	Constraints				GetConstraints() const;
+
 	/// Get total number of constraints
 	inline uint32			GetNumConstraints() const					{ return (uint32)mConstraints.size(); }
 
@@ -76,8 +82,6 @@ public:
 	void					UnlockAllConstraints()						{ PhysicsLock::sUnlock(mConstraintsMutex, EPhysicsLockTypes::ConstraintsList); }
 
 private:
-	using Constraints = vector<Ref<Constraint>>;
-
 	Constraints				mConstraints;
 	mutable Mutex			mConstraintsMutex;
 };

Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h

@@ -145,7 +145,7 @@ public:
 		// theta = rotation angle
 		// 
 		// If we assume theta is small (error is small) then sin(x) = x so an approximation of the error angles is:
-		Vec3 error = 2.0f * diff.GetXYZ();
+		Vec3 error = 2.0f * diff.EnsureWPositive().GetXYZ();
 		if (error != Vec3::sZero())
 		{
 			// Calculate lagrange multiplier (lambda) for Baumgarte stabilization:

Jolt/Physics/Constraints/DistanceConstraint.cpp

@@ -223,4 +223,18 @@ void DistanceConstraint::RestoreState(StateRecorder &inStream)
 	inStream.Read(mWorldSpaceNormal);
 }
 
+Ref<ConstraintSettings> DistanceConstraint::GetConstraintSettings() const
+{
+	DistanceConstraintSettings *settings = new DistanceConstraintSettings;
+	ToConstraintSettings(*settings);
+	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
+	settings->mPoint1 = mLocalSpacePosition1;
+	settings->mPoint2 = mLocalSpacePosition2;
+	settings->mMinDistance = mMinDistance;
+	settings->mMaxDistance = mMaxDistance;
+	settings->mFrequency = mFrequency;
+	settings->mDamping = mDamping;
+	return settings;
+}
+
 JPH_NAMESPACE_END

Jolt/Physics/Constraints/DistanceConstraint.h

@@ -55,7 +55,7 @@ public:
 								DistanceConstraint(Body &inBody1, Body &inBody2, const DistanceConstraintSettings &inSettings);
 
 	// Generic interface of a constraint
-	virtual EConstraintType		GetType() const override									{ return EConstraintType::Distance; }
+	virtual EConstraintSubType	GetSubType() const override									{ return EConstraintSubType::Distance; }
 	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
 	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
 	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
@@ -65,6 +65,7 @@ public:
 #endif // JPH_DEBUG_RENDERER
 	virtual void				SaveState(StateRecorder &inStream) const override;
 	virtual void				RestoreState(StateRecorder &inStream) override;
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
 
 	// See: TwoBodyConstraint
 	virtual Mat44				GetConstraintToBody1Matrix() const override					{ return Mat44::sTranslation(mLocalSpacePosition1); }

Jolt/Physics/Constraints/FixedConstraint.cpp

@@ -114,4 +114,10 @@ void FixedConstraint::RestoreState(StateRecorder &inStream)
 	mPointConstraintPart.RestoreState(inStream);
 }
 
+Ref<ConstraintSettings> FixedConstraint::GetConstraintSettings() const
+{
+	JPH_ASSERT(false); // Not implemented yet
+	return nullptr;
+}
+
 JPH_NAMESPACE_END

Jolt/Physics/Constraints/FixedConstraint.h

@@ -28,7 +28,7 @@ public:
 								FixedConstraint(Body &inBody1, Body &inBody2, const FixedConstraintSettings &inSettings);
 
 	// Generic interface of a constraint
-	virtual EConstraintType		GetType() const override					{ return EConstraintType::Fixed; }
+	virtual EConstraintSubType	GetSubType() const override									{ return EConstraintSubType::Fixed; }
 	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
 	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
 	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
@@ -38,6 +38,7 @@ public:
 #endif // JPH_DEBUG_RENDERER
 	virtual void				SaveState(StateRecorder &inStream) const override;
 	virtual void				RestoreState(StateRecorder &inStream) override;
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
 
 	// See: TwoBodyConstraint
 	virtual Mat44				GetConstraintToBody1Matrix() const override					{ return Mat44::sTranslation(mLocalSpacePosition1); }

Jolt/Physics/Constraints/HingeConstraint.cpp

@@ -359,6 +359,25 @@ void HingeConstraint::RestoreState(StateRecorder &inStream)
 	inStream.Read(mTargetAngle);
 }
 
+
+Ref<ConstraintSettings> HingeConstraint::GetConstraintSettings() const
+{
+	HingeConstraintSettings *settings = new HingeConstraintSettings;
+	ToConstraintSettings(*settings);
+	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
+	settings->mPoint1 = mLocalSpacePosition1;
+	settings->mHingeAxis1 = mLocalSpaceHingeAxis1;
+	settings->mNormalAxis1 = mLocalSpaceNormalAxis1;
+	settings->mPoint2 = mLocalSpacePosition2;
+	settings->mHingeAxis2 = mLocalSpaceHingeAxis2;
+	settings->mNormalAxis2 = mLocalSpaceNormalAxis2;
+	settings->mLimitsMin = mLimitsMin;
+	settings->mLimitsMax = mLimitsMax;
+	settings->mMaxFrictionTorque = mMaxFrictionTorque;
+	settings->mMotorSettings = mMotorSettings;
+	return settings;
+}
+
 Mat44 HingeConstraint::GetConstraintToBody1Matrix() const
 { 
 	return Mat44(Vec4(mLocalSpaceHingeAxis1, 0), Vec4(mLocalSpaceNormalAxis1, 0), Vec4(mLocalSpaceHingeAxis1.Cross(mLocalSpaceNormalAxis1), 0), Vec4(mLocalSpacePosition1, 1)); 

Jolt/Physics/Constraints/HingeConstraint.h

@@ -61,7 +61,7 @@ public:
 								HingeConstraint(Body &inBody1, Body &inBody2, const HingeConstraintSettings &inSettings);
 
 	// Generic interface of a constraint
-	virtual EConstraintType		GetType() const override								{ return EConstraintType::Hinge; }
+	virtual EConstraintSubType	GetSubType() const override								{ return EConstraintSubType::Hinge; }
 	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
 	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
 	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
@@ -72,6 +72,7 @@ public:
 #endif // JPH_DEBUG_RENDERER
 	virtual void				SaveState(StateRecorder &inStream) const override;
 	virtual void				RestoreState(StateRecorder &inStream) override;
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
 
 	// See: TwoBodyConstraint
 	virtual Mat44				GetConstraintToBody1Matrix() const override;

Jolt/Physics/Constraints/PathConstraint.cpp

@@ -426,4 +426,10 @@ void PathConstraint::RestoreState(StateRecorder &inStream)
 	inStream.Read(mPathFraction);
 }
 
+Ref<ConstraintSettings> PathConstraint::GetConstraintSettings() const
+{
+	JPH_ASSERT(false); // Not implemented yet
+	return nullptr;
+}
+
 JPH_NAMESPACE_END

Jolt/Physics/Constraints/PathConstraint.h

@@ -70,7 +70,7 @@ public:
 									PathConstraint(Body &inBody1, Body &inBody2, const PathConstraintSettings &inSettings);
 
 	// Generic interface of a constraint
-	virtual EConstraintType			GetType() const override								{ return EConstraintType::Path; }
+	virtual EConstraintSubType		GetSubType() const override								{ return EConstraintSubType::Path; }
 	virtual void					SetupVelocityConstraint(float inDeltaTime) override;
 	virtual void					WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
 	virtual bool					SolveVelocityConstraint(float inDeltaTime) override;
@@ -81,6 +81,7 @@ public:
 	virtual void					SaveState(StateRecorder &inStream) const override;
 	virtual void					RestoreState(StateRecorder &inStream) override;
 	virtual bool					IsActive() const override								{ return TwoBodyConstraint::IsActive() && mPath != nullptr; }
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
 
 	// See: TwoBodyConstraint
 	virtual Mat44					GetConstraintToBody1Matrix() const override				{ return mPathToBody1; }

Jolt/Physics/Constraints/PointConstraint.cpp

@@ -111,4 +111,14 @@ void PointConstraint::RestoreState(StateRecorder &inStream)
 	mPointConstraintPart.RestoreState(inStream);
 }
 
+Ref<ConstraintSettings> PointConstraint::GetConstraintSettings() const
+{
+	PointConstraintSettings *settings = new PointConstraintSettings;
+	ToConstraintSettings(*settings);
+	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
+	settings->mPoint1 = mLocalSpacePosition1;
+	settings->mPoint2 = mLocalSpacePosition2;
+	return settings;
+}
+
 JPH_NAMESPACE_END

Jolt/Physics/Constraints/PointConstraint.h

@@ -43,7 +43,7 @@ public:
 								PointConstraint(Body &inBody1, Body &inBody2, const PointConstraintSettings &inSettings);
 
 	// Generic interface of a constraint
-	virtual EConstraintType		GetType() const override									{ return EConstraintType::Point; }
+	virtual EConstraintSubType	GetSubType() const override									{ return EConstraintSubType::Point; }
 	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
 	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
 	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
@@ -53,6 +53,7 @@ public:
 #endif // JPH_DEBUG_RENDERER
 	virtual void				SaveState(StateRecorder &inStream) const override;
 	virtual void				RestoreState(StateRecorder &inStream) override;
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
 
 	// See: TwoBodyConstraint
 	virtual Mat44				GetConstraintToBody1Matrix() const override					{ return Mat44::sTranslation(mLocalSpacePosition1); }

Jolt/Physics/Constraints/SixDOFConstraint.cpp

@@ -754,4 +754,23 @@ void SixDOFConstraint::RestoreState(StateRecorder &inStream)
 	inStream.Read(mTargetOrientation);
 }
 
+Ref<ConstraintSettings> SixDOFConstraint::GetConstraintSettings() const
+{
+	SixDOFConstraintSettings *settings = new SixDOFConstraintSettings;
+	ToConstraintSettings(*settings);
+	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
+	settings->mPosition1 = mLocalSpacePosition1;
+	settings->mAxisX1 = mConstraintToBody1.RotateAxisX();
+	settings->mAxisY1 = mConstraintToBody1.RotateAxisY();
+	settings->mPosition2 = mLocalSpacePosition2;
+	settings->mAxisX2 = mConstraintToBody2.RotateAxisX();
+	settings->mAxisY2 = mConstraintToBody2.RotateAxisY();
+	memcpy(settings->mLimitMin, mLimitMin, sizeof(mLimitMin)); 
+	memcpy(settings->mLimitMax, mLimitMax, sizeof(mLimitMax)); 
+	memcpy(settings->mMaxFriction, mMaxFriction, sizeof(mMaxFriction));
+	for (int i = 0; i < EAxis::Num; ++i)
+		settings->mMotorSettings[i] = mMotorSettings[i];
+	return settings;
+}
+
 JPH_NAMESPACE_END

Jolt/Physics/Constraints/SixDOFConstraint.h

@@ -97,7 +97,7 @@ public:
 								SixDOFConstraint(Body &inBody1, Body &inBody2, const SixDOFConstraintSettings &inSettings);
 
 	/// Generic interface of a constraint
-	virtual EConstraintType		GetType() const override									{ return EConstraintType::SixDOF; }
+	virtual EConstraintSubType	GetSubType() const override									{ return EConstraintSubType::SixDOF; }
 	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
 	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
 	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
@@ -108,6 +108,7 @@ public:
 #endif // JPH_DEBUG_RENDERER
 	virtual void				SaveState(StateRecorder &inStream) const override;
 	virtual void				RestoreState(StateRecorder &inStream) override;
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
 
 	// See: TwoBodyConstraint
 	virtual Mat44				GetConstraintToBody1Matrix() const override					{ return Mat44::sRotationTranslation(mConstraintToBody1, mLocalSpacePosition1); }

Jolt/Physics/Constraints/SliderConstraint.cpp

@@ -428,6 +428,25 @@ void SliderConstraint::RestoreState(StateRecorder &inStream)
 	inStream.Read(mTargetPosition);
 }
 
+Ref<ConstraintSettings> SliderConstraint::GetConstraintSettings() const
+{
+	SliderConstraintSettings *settings = new SliderConstraintSettings;
+	ToConstraintSettings(*settings);
+	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
+	settings->mPoint1 = mLocalSpacePosition1;
+	settings->mSliderAxis1 = mLocalSpaceSliderAxis1;
+	settings->mNormalAxis1 = mLocalSpaceNormal1;
+	settings->mPoint2 = mLocalSpacePosition2;
+	Mat44 inv_initial_rotation = Mat44::sRotation(mInvInitialOrientation);
+	settings->mSliderAxis2 = inv_initial_rotation.Multiply3x3(mLocalSpaceSliderAxis1);
+	settings->mNormalAxis2 = inv_initial_rotation.Multiply3x3(mLocalSpaceNormal1);
+	settings->mLimitsMin = mLimitsMin;
+	settings->mLimitsMax = mLimitsMax;
+	settings->mMaxFrictionForce = mMaxFrictionForce;
+	settings->mMotorSettings = mMotorSettings;
+	return settings;
+}
+
 Mat44 SliderConstraint::GetConstraintToBody1Matrix() const
 { 
 	return Mat44(Vec4(mLocalSpaceSliderAxis1, 0), Vec4(mLocalSpaceNormal1, 0), Vec4(mLocalSpaceNormal2, 0), Vec4(mLocalSpacePosition1, 1)); 

Jolt/Physics/Constraints/SliderConstraint.h

@@ -67,7 +67,7 @@ public:
 								SliderConstraint(Body &inBody1, Body &inBody2, const SliderConstraintSettings &inSettings);
 
 	// Generic interface of a constraint
-	virtual EConstraintType		GetType() const override								{ return EConstraintType::Slider; }
+	virtual EConstraintSubType	GetSubType() const override								{ return EConstraintSubType::Slider; }
 	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
 	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
 	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
@@ -78,6 +78,7 @@ public:
 #endif // JPH_DEBUG_RENDERER
 	virtual void				SaveState(StateRecorder &inStream) const override;
 	virtual void				RestoreState(StateRecorder &inStream) override;
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
 
 	// See: TwoBodyConstraint
 	virtual Mat44				GetConstraintToBody1Matrix() const override;

Jolt/Physics/Constraints/SwingTwistConstraint.cpp

@@ -459,4 +459,25 @@ void SwingTwistConstraint::RestoreState(StateRecorder &inStream)
 	inStream.Read(mTargetOrientation);
 }
 
+Ref<ConstraintSettings> SwingTwistConstraint::GetConstraintSettings() const
+{
+	SwingTwistConstraintSettings *settings = new SwingTwistConstraintSettings;
+	ToConstraintSettings(*settings);
+	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
+	settings->mPosition1 = mLocalSpacePosition1;
+	settings->mTwistAxis1 = mConstraintToBody1.RotateAxisX();
+	settings->mPlaneAxis1 = mConstraintToBody1.RotateAxisZ();
+	settings->mPosition2 = mLocalSpacePosition2;
+	settings->mTwistAxis2 = mConstraintToBody2.RotateAxisX();
+	settings->mPlaneAxis2 = mConstraintToBody2.RotateAxisZ();
+	settings->mNormalHalfConeAngle = mNormalHalfConeAngle;
+	settings->mPlaneHalfConeAngle = mPlaneHalfConeAngle;
+	settings->mTwistMinAngle = mTwistMinAngle;
+	settings->mTwistMaxAngle = mTwistMaxAngle;
+	settings->mMaxFrictionTorque = mMaxFrictionTorque;
+	settings->mSwingMotorSettings = mSwingMotorSettings;
+	settings->mTwistMotorSettings = mTwistMotorSettings;
+	return settings;
+}
+
 JPH_NAMESPACE_END

Jolt/Physics/Constraints/SwingTwistConstraint.h

@@ -7,7 +7,6 @@
 #include <Jolt/Physics/Constraints/MotorSettings.h>
 #include <Jolt/Physics/Constraints/ConstraintPart/PointConstraintPart.h>
 #include <Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h>
-#include <Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h>
 #include <Jolt/Physics/Constraints/ConstraintPart/SwingTwistConstraintPart.h>
 
 JPH_NAMESPACE_BEGIN
@@ -71,7 +70,7 @@ public:
 								SwingTwistConstraint(Body &inBody1, Body &inBody2, const SwingTwistConstraintSettings &inSettings);
 
 	///@name Generic interface of a constraint
-	virtual EConstraintType		GetType() const override									{ return EConstraintType::SwingTwist; }
+	virtual EConstraintSubType	GetSubType() const override									{ return EConstraintSubType::SwingTwist; }
 	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
 	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
 	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
@@ -82,6 +81,7 @@ public:
 #endif // JPH_DEBUG_RENDERER
 	virtual void				SaveState(StateRecorder &inStream) const override;
 	virtual void				RestoreState(StateRecorder &inStream) override;
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
 
 	// See: TwoBodyConstraint
 	virtual Mat44				GetConstraintToBody1Matrix() const override					{ return Mat44::sRotationTranslation(mConstraintToBody1, mLocalSpacePosition1); }

Jolt/Physics/Constraints/TwoBodyConstraint.h

@@ -28,6 +28,9 @@ public:
 	/// Constructor
 								TwoBodyConstraint(Body &inBody1, Body &inBody2, const TwoBodyConstraintSettings &inSettings) : Constraint(inSettings), mBody1(&inBody1), mBody2(&inBody2) { }
 
+	/// Get the type of a constraint
+	virtual EConstraintType		GetType() const override				{ return EConstraintType::TwoBodyConstraint; }
+
 	/// Solver interface
 	virtual bool				IsActive() const override				{ return Constraint::IsActive() && (mBody1->IsActive() || mBody2->IsActive()) && (mBody2->IsDynamic() || mBody1->IsDynamic()); }
 #ifdef JPH_DEBUG_RENDERER

Jolt/Physics/PhysicsScene.cpp

@@ -5,6 +5,7 @@
 
 #include <Jolt/Physics/PhysicsScene.h>
 #include <Jolt/Physics/PhysicsSystem.h>
+#include <Jolt/Physics/Body/BodyLockMulti.h>
 #include <Jolt/ObjectStream/TypeDeclarations.h>
 
 JPH_NAMESPACE_BEGIN
@@ -12,6 +13,14 @@ JPH_NAMESPACE_BEGIN
 JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(PhysicsScene)
 {
 	JPH_ADD_ATTRIBUTE(PhysicsScene, mBodies)
+	JPH_ADD_ATTRIBUTE(PhysicsScene, mConstraints)
+}
+
+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)
@@ -19,6 +28,11 @@ 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);
+}
+
 bool PhysicsScene::FixInvalidScales()
 {
 	const Vec3 unit_scale = Vec3::sReplicate(1.0f);
@@ -52,16 +66,30 @@ bool PhysicsScene::CreateBodies(PhysicsSystem *inSystem) const
 	{
 		const Body *body = bi.CreateBody(b);
 		if (body == nullptr)
-			break; // Out of bodies
+			break;
 		body_ids.push_back(body->GetID());
 	}
 
 	// Batch add bodies
-	BodyInterface::AddState add_state = bi.AddBodiesPrepare(body_ids.data(), (int)body_ids.size());
-	bi.AddBodiesFinalize(body_ids.data(), (int)body_ids.size(), add_state, EActivation::Activate);
+	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())
+		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);
+	}
 
-	// Return true if all bodies were added
-	return body_ids.size() == mBodies.size();
+	// Everything was created
+	return true;
 }
 
 void PhysicsScene::SaveBinaryState(StreamOut &inStream, bool inSaveShapes, bool inSaveGroupFilter) const
@@ -74,6 +102,15 @@ void PhysicsScene::SaveBinaryState(StreamOut &inStream, bool inSaveShapes, bool
 	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);
+	}
 }
 
 PhysicsScene::PhysicsSceneResult PhysicsScene::sRestoreFromBinaryState(StreamIn &inStream)
@@ -108,8 +145,72 @@ PhysicsScene::PhysicsSceneResult PhysicsScene::sRestoreFromBinaryState(StreamIn
 		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 = static_cast<const TwoBodyConstraintSettings *>(c_result.Get().GetPtr());
+		inStream.Read(cc.mBody1);
+		inStream.Read(cc.mBody2);
+	}
+
 	result.Set(scene);
 	return result;
 }
 
+void PhysicsScene::FromPhysicsSystem(const PhysicsSystem *inSystem)
+{
+	// This map will track where each body went in mBodies
+	using BodyIDToIdxMap = unordered_map<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();
+
+			// Convert to body creation settings
+			AddBody(lock.GetBody().GetBodyCreationSettings());
+		}
+	}
+
+	// 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(static_cast<const TwoBodyConstraintSettings *>(settings.GetPtr()), b1->second, b2->second);
+		}
+}
+
 JPH_NAMESPACE_END

Jolt/Physics/PhysicsScene.h

@@ -5,6 +5,7 @@
 
 #include <Jolt/Core/Reference.h>
 #include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
 
 JPH_NAMESPACE_BEGIN
 
@@ -19,13 +20,43 @@ public:
 	/// Add a body to the scene
 	void									AddBody(const BodyCreationSettings &inBody);
 
-	/// Get amount of bodies in this scene
+	/// Body constant to use to indicate that the constraint is attached to the fixed world
+	static constexpr uint32					cFixedToWorld = 0xffffffff;
+
+	/// Add a constraint to the scene
+	/// @param inConstraint Constraint settings
+	/// @param inBody1 Index in the bodies list of first body to attach constraint to
+	/// @param inBody2 Index in the bodies list of the second body to attach constraint to
+	void									AddConstraint(const TwoBodyConstraintSettings *inConstraint, uint32 inBody1, uint32 inBody2);
+
+	/// Get number of bodies in this scene
 	size_t									GetNumBodies() const							{ return mBodies.size(); }
 
 	/// Access to the body settings for this scene
 	const vector<BodyCreationSettings> &	GetBodies() const								{ return mBodies; }
 	vector<BodyCreationSettings> &			GetBodies()										{ return mBodies; }
 
+	/// A constraint and how it is connected to the bodies in the scene
+	class ConnectedConstraint
+	{
+	public:
+		JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(ConnectedConstraint)
+
+											ConnectedConstraint() = default;
+											ConnectedConstraint(const TwoBodyConstraintSettings *inSettings, uint inBody1, uint inBody2) : mSettings(inSettings), mBody1(inBody1), mBody2(inBody2) { }
+
+		RefConst<TwoBodyConstraintSettings>	mSettings;										///< Constraint settings
+		uint32								mBody1;											///< Index of first body (in mBodies)
+		uint32								mBody2;											///< Index of second body (in mBodies)
+	};
+
+	/// Get number of constraints in this scene
+	size_t									GetNumConstraints() const						{ return mConstraints.size(); }
+
+	/// Access to the constraints for this scene
+	const vector<ConnectedConstraint> &		GetConstraints() const							{ return mConstraints; }
+	vector<ConnectedConstraint> &			GetConstraints()								{ return mConstraints; }
+
 	/// Instantiate all bodies, returns false if not all bodies could be created
 	bool									CreateBodies(PhysicsSystem *inSystem) const;
 
@@ -44,9 +75,15 @@ public:
 	/// Restore a saved scene from inStream
 	static PhysicsSceneResult				sRestoreFromBinaryState(StreamIn &inStream);
 
+	/// For debugging purposes: Construct a scene from the current state of the physics system
+	void									FromPhysicsSystem(const PhysicsSystem *inSystem);
+
 private:
 	/// The bodies that are part of this scene
 	vector<BodyCreationSettings>			mBodies;
+
+	/// Constraints that are part of this scene
+	vector<ConnectedConstraint>				mConstraints;
 };
 
 JPH_NAMESPACE_END

Jolt/Physics/PhysicsSystem.h

@@ -83,6 +83,9 @@ public:
 	/// Batch remove constraints. Note that the inConstraints array is allowed to have nullptrs, these will be ignored.
 	void						RemoveConstraints(Constraint **inConstraints, int inNumber)	{ mConstraintManager.Remove(inConstraints, inNumber); }
 
+	/// Get a list of all constraints
+	Constraints					GetConstraints() const										{ return mConstraintManager.GetConstraints(); }
+
 	/// Optimize the broadphase, needed only if you've added many bodies prior to calling Update() for the first time.
 	void						OptimizeBroadPhase();
 

Jolt/Physics/Vehicle/VehicleConstraint.cpp

@@ -481,4 +481,10 @@ void VehicleConstraint::RestoreState(StateRecorder &inStream)
 	mPitchRollPart.RestoreState(inStream);
 }
 
+Ref<ConstraintSettings> VehicleConstraint::GetConstraintSettings() const
+{
+	JPH_ASSERT(false); // Not implemented yet
+	return nullptr;
+}
+
 JPH_NAMESPACE_END

Jolt/Physics/Vehicle/VehicleConstraint.h

@@ -50,7 +50,7 @@ public:
 	virtual						~VehicleConstraint() override;
 
 	/// Get the type of a constraint
-	virtual EConstraintType		GetType() const override					{ return EConstraintType::Vehicle; }
+	virtual EConstraintSubType	GetSubType() const override					{ return EConstraintSubType::Vehicle; }
 
 	/// Defines the maximum pitch/roll angle (rad), can be used to avoid the car from getting upside down. The vehicle up direction will stay within a cone centered around the up axis with half top angle mMaxPitchRollAngle, set to pi to turn off.
 	void						SetMaxPitchRollAngle(float inMaxPitchRollAngle) { mCosMaxPitchRollAngle = cos(inMaxPitchRollAngle); }
@@ -104,6 +104,7 @@ public:
 #endif // JPH_DEBUG_RENDERER
 	virtual void				SaveState(StateRecorder &inStream) const override;
 	virtual void				RestoreState(StateRecorder &inStream) override;
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
 
 private:
 	// See: PhysicsStepListener

Samples/Samples.cmake

@@ -131,6 +131,8 @@ set(SAMPLES_SRC_FILES
 	${SAMPLES_ROOT}/Tests/Rig/RigPileTest.h
 	${SAMPLES_ROOT}/Tests/Test.cpp
 	${SAMPLES_ROOT}/Tests/Test.h
+	${SAMPLES_ROOT}/Tests/Tools/LoadSnapshotTest.cpp
+	${SAMPLES_ROOT}/Tests/Tools/LoadSnapshotTest.h
 	${SAMPLES_ROOT}/Tests/ScaledShapes/ScaledBoxShapeTest.cpp
 	${SAMPLES_ROOT}/Tests/ScaledShapes/ScaledBoxShapeTest.h
 	${SAMPLES_ROOT}/Tests/ScaledShapes/ScaledCapsuleShapeTest.cpp

Samples/SamplesApp.cpp

@@ -7,10 +7,12 @@
 #include <Application/EntryPoint.h>
 #include <Jolt/Core/JobSystemThreadPool.h>
 #include <Jolt/Core/TempAllocator.h>
+#include <Jolt/Core/StreamWrapper.h>
 #include <Jolt/Geometry/OrientedBox.h>
 #include <Jolt/Physics/PhysicsSystem.h>
 #include <Jolt/Physics/StateRecorderImpl.h>
 #include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Jolt/Physics/PhysicsScene.h>
 #include <Jolt/Physics/Collision/RayCast.h>
 #include <Jolt/Physics/Collision/ShapeCast.h>
 #include <Jolt/Physics/Collision/CastResult.h>
@@ -278,6 +280,13 @@ static TestNameAndRTTI sConvexCollisionTests[] =
 	{ "Capsule Vs Box",						JPH_RTTI(CapsuleVsBoxTest) }
 };
 
+JPH_DECLARE_RTTI_FOR_FACTORY(LoadSnapshotTest)
+
+static TestNameAndRTTI sTools[] =
+{
+	{ "Load Snapshot",						JPH_RTTI(LoadSnapshotTest) },
+};
+
 static TestCategory sAllCategories[] = 
 { 
 	{ "General", sGeneralTests, size(sGeneralTests) },
@@ -289,7 +298,8 @@ static TestCategory sAllCategories[] =
 	{ "Water", sWaterTests, size(sWaterTests) },
 	{ "Vehicle", sVehicleTests, size(sVehicleTests) },
 	{ "Broad Phase", sBroadPhaseTests, size(sBroadPhaseTests) },
-	{ "Convex Collision", sConvexCollisionTests, size(sConvexCollisionTests) } 
+	{ "Convex Collision", sConvexCollisionTests, size(sConvexCollisionTests) },
+	{ "Tools", sTools, size(sTools) } 
 };
 
 //-----------------------------------------------------------------------------
@@ -339,6 +349,8 @@ SamplesApp::SamplesApp()
 	mDebugUI->CreateTextButton(main_menu, "Run All Tests", [this]() { RunAllTests(); });
 	mNextTestButton = mDebugUI->CreateTextButton(main_menu, "Next Test (N)", [this]() { NextTest(); });
 	mNextTestButton->SetDisabled(true);
+	mDebugUI->CreateTextButton(main_menu, "Take Snapshot", [this]() { TakeSnapshot(); });
+	mDebugUI->CreateTextButton(main_menu, "Take And Reload Snapshot", [this]() { TakeAndReloadSnapshot(); });
 	mDebugUI->CreateTextButton(main_menu, "Physics Settings", [this]() { 
 		UIElement *phys_settings = mDebugUI->CreateMenu();
 		mDebugUI->CreateSlider(phys_settings, "Max Concurrent Jobs", float(mMaxConcurrentJobs), 1, float(thread::hardware_concurrency()), 1, [this](float inValue) { mMaxConcurrentJobs = (int)inValue; });
@@ -625,6 +637,26 @@ bool SamplesApp::CheckNextTest()
 	return true;
 }
 
+void SamplesApp::TakeSnapshot()
+{
+	// Convert physics system to scene
+	Ref<PhysicsScene> scene = new PhysicsScene();
+	scene->FromPhysicsSystem(mPhysicsSystem);
+
+	// Save scene
+	ofstream stream("snapshot.bin", ofstream::out | ofstream::trunc | ofstream::binary);
+	StreamOutWrapper wrapper(stream);
+	if (stream.is_open())
+		scene->SaveBinaryState(wrapper, true, true);
+}
+
+void SamplesApp::TakeAndReloadSnapshot()
+{
+	TakeSnapshot();
+
+	StartTest(JPH_RTTI(LoadSnapshotTest));
+}
+
 RefConst<Shape> SamplesApp::CreateProbeShape()
 {
 	// Get the scale

Samples/SamplesApp.h

@@ -52,6 +52,12 @@ private:
 	// Check if we've got to start the next test. Returns false when the application should exit.
 	bool					CheckNextTest();
 
+	// Create a snapshot of the physics system and save it to disc
+	void					TakeSnapshot();
+
+	// Create a snapshot of the physics system, save it to disc and immediately reload it
+	void					TakeAndReloadSnapshot();
+
 	// Probing the collision world
 	RefConst<Shape>			CreateProbeShape();
 	bool					CastProbe(float inProbeLength, float &outFraction, Vec3 &outPosition, BodyID &outID);

Samples/Tests/General/LoadSaveBinaryTest.cpp

@@ -8,6 +8,7 @@
 #include <Jolt/Physics/PhysicsScene.h>
 #include <Utils/Log.h>
 #include <Jolt/Core/StreamWrapper.h>
+#include <Layers.h>
 
 JPH_IMPLEMENT_RTTI_VIRTUAL(LoadSaveBinaryTest) 
 { 
@@ -20,15 +21,13 @@ void LoadSaveBinaryTest::Initialize()
 	Ref<PhysicsScene> scene = LoadSaveSceneTest::sCreateScene();
 
 	{
-		// Create a new scene by creating the body first and then converting it back to body creation settings
+		// Create a new scene by instantiating the scene in a physics system and then converting it back to a scene
+		PhysicsSystem system;
+		BPLayerInterfaceImpl layer_interface;
+		system.Init(mPhysicsSystem->GetMaxBodies(), 0, 1024, 1024, layer_interface, BroadPhaseCanCollide, ObjectCanCollide);
+		scene->CreateBodies(&system);
 		Ref<PhysicsScene> scene_copy = new PhysicsScene();
-		BodyInterface &bi = mPhysicsSystem->GetBodyInterface();
-		for (const BodyCreationSettings &b : scene->GetBodies())
-		{
-			Body &body = *bi.CreateBody(b);
-			scene_copy->AddBody(body.GetBodyCreationSettings());
-			bi.DestroyBody(body.GetID());
-		}
+		scene_copy->FromPhysicsSystem(&system);
 
 		// Replace the original scene
 		scene = scene_copy;

Samples/Tests/General/LoadSaveSceneTest.cpp

@@ -21,6 +21,7 @@
 #include <Jolt/Physics/Collision/Shape/TriangleShape.h>
 #include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
 #include <Jolt/Physics/Collision/PhysicsMaterialSimple.h>
+#include <Jolt/Physics/Constraints/DistanceConstraint.h>
 #include <Jolt/Physics/Body/BodyCreationSettings.h>
 #include <Layers.h>
 #include <Utils/Log.h>
@@ -170,6 +171,11 @@ Ref<PhysicsScene> LoadSaveSceneTest::sCreateScene()
 	mutable_compound->AddShape(Vec3(0, 0, 0.5f), Quat::sRotation(Vec3::sAxisX(), 0.5f * JPH_PI), new TaperedCapsuleShapeSettings(0.5f, 0.2f, 0.1f, new PhysicsMaterialSimple("MutableCompound Tapered Capsule Material", Color::sGetDistinctColor(12))));
 	scene->AddBody(BodyCreationSettings(mutable_compound, Vec3(0, cMaxHeight + 9.0f, 0), Quat::sRotation(Vec3::sAxisZ(), 0.5f * JPH_PI), EMotionType::Dynamic, Layers::MOVING));
 
+	// Connect the first two dynamic bodies with a distance constraint
+	DistanceConstraintSettings *dist_constraint = new DistanceConstraintSettings();
+	dist_constraint->mSpace = EConstraintSpace::LocalToBodyCOM;
+	scene->AddConstraint(dist_constraint, 3, 4);
+
 	return scene;
 }
 

Samples/Tests/Tools/LoadSnapshotTest.cpp

@@ -0,0 +1,28 @@
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/Tools/LoadSnapshotTest.h>
+#include <Jolt/Physics/PhysicsScene.h>
+#include <Jolt/Core/StreamWrapper.h>
+#include <Utils/Log.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(LoadSnapshotTest) 
+{ 
+	JPH_ADD_BASE_CLASS(LoadSnapshotTest, Test) 
+}
+
+void LoadSnapshotTest::Initialize()
+{
+	ifstream stream("snapshot.bin", ifstream::in | ifstream::binary);
+	if (!stream.is_open()) 
+		FatalError("Unable to open 'snapshot.bin'");
+
+	StreamInWrapper wrapper(stream);
+	PhysicsScene::PhysicsSceneResult result = PhysicsScene::sRestoreFromBinaryState(wrapper);
+	if (result.HasError())
+		FatalError(result.GetError().c_str());
+
+	result.Get()->CreateBodies(mPhysicsSystem);
+}

Samples/Tests/Tools/LoadSnapshotTest.h

@@ -0,0 +1,16 @@
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+// This test loads a physics scene from 'snapshot.bin' and runs it
+class LoadSnapshotTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(LoadSnapshotTest)
+
+	// See: Test
+	virtual void		Initialize() override;
+};