Ability to selectively save the state of a physics system (#646)

You can now specify which parts to save using EStateRecorderState and a StateRecorderFilter. This caters for the case where a client simulates less bodies than the server, so needs to receive a selective snapshot of the server while still having an identical (deterministic) simulation for that sub set of objects.

Co-authored-by: Andrea Catania <[email protected]>

Jolt/Physics/Body/BodyManager.cpp

@@ -481,6 +481,56 @@ void BodyManager::DestroyBodies(const BodyID *inBodyIDs, int inNumber)
 #endif // defined(_DEBUG) && _defined(JPH_ENABLE_ASSERTS)
 }
 
+void BodyManager::AddBodyToActiveBodies(Body &ioBody)
+{
+	// Select the correct array to use
+	int type = (int)ioBody.GetBodyType();
+	atomic<uint32> &num_active_bodies = mNumActiveBodies[type];
+	BodyID *active_bodies = mActiveBodies[type];
+
+	MotionProperties *mp = ioBody.mMotionProperties;
+	mp->mIndexInActiveBodies = num_active_bodies;
+	JPH_ASSERT(num_active_bodies < GetMaxBodies());
+	active_bodies[num_active_bodies] = ioBody.GetID();
+	num_active_bodies++; // Increment atomic after setting the body ID so that PhysicsSystem::JobFindCollisions (which doesn't lock the mActiveBodiesMutex) will only read valid IDs
+
+	// Count CCD bodies
+	if (mp->GetMotionQuality() == EMotionQuality::LinearCast)
+		mNumActiveCCDBodies++;
+}
+
+void BodyManager::RemoveBodyFromActiveBodies(Body &ioBody)
+{
+	// Select the correct array to use
+	int type = (int)ioBody.GetBodyType();
+	atomic<uint32> &num_active_bodies = mNumActiveBodies[type];
+	BodyID *active_bodies = mActiveBodies[type];
+
+	uint32 last_body_index = num_active_bodies - 1;
+	MotionProperties *mp = ioBody.mMotionProperties;
+	if (mp->mIndexInActiveBodies != last_body_index)
+	{
+		// This is not the last body, use the last body to fill the hole
+		BodyID last_body_id = active_bodies[last_body_index];
+		active_bodies[mp->mIndexInActiveBodies] = last_body_id;
+
+		// Update that body's index in the active list
+		Body &last_body = *mBodies[last_body_id.GetIndex()];
+		JPH_ASSERT(last_body.mMotionProperties->mIndexInActiveBodies == last_body_index);
+		last_body.mMotionProperties->mIndexInActiveBodies = mp->mIndexInActiveBodies;
+	}
+
+	// Mark this body as no longer active
+	mp->mIndexInActiveBodies = Body::cInactiveIndex;
+
+	// Remove unused element from active bodies list
+	--num_active_bodies;
+
+	// Count CCD bodies
+	if (mp->GetMotionQuality() == EMotionQuality::LinearCast)
+		mNumActiveCCDBodies--;
+}
+
 void BodyManager::ActivateBodies(const BodyID *inBodyIDs, int inNumber)
 {
 	// Don't take lock if no bodies are to be activated
@@ -503,20 +553,10 @@ void BodyManager::ActivateBodies(const BodyID *inBodyIDs, int inNumber)
 			if (!body.IsStatic()
 				&& body.mMotionProperties->mIndexInActiveBodies == Body::cInactiveIndex)
 			{
-				// Select the correct array to use
-				int type = (int)body.GetBodyType();
-				atomic<uint32> &num_active_bodies = mNumActiveBodies[type];
-				BodyID *active_bodies = mActiveBodies[type];
-
-				body.mMotionProperties->mIndexInActiveBodies = num_active_bodies;
+				// Reset sleeping
 				body.ResetSleepTestSpheres();
-				JPH_ASSERT(num_active_bodies < GetMaxBodies());
-				active_bodies[num_active_bodies] = body_id;
-				num_active_bodies++; // Increment atomic after setting the body ID so that PhysicsSystem::JobFindCollisions (which doesn't lock the mActiveBodiesMutex) will only read valid IDs
 
-				// Count CCD bodies
-				if (body.mMotionProperties->GetMotionQuality() == EMotionQuality::LinearCast)
-					mNumActiveCCDBodies++;
+				AddBodyToActiveBodies(body);
 
 				// Call activation listener
 				if (mActivationListener != nullptr)
@@ -547,39 +587,16 @@ void BodyManager::DeactivateBodies(const BodyID *inBodyIDs, int inNumber)
 			if (body.mMotionProperties != nullptr
 				&& body.mMotionProperties->mIndexInActiveBodies != Body::cInactiveIndex)
 			{
-				// Select the correct array to use
-				int type = (int)body.GetBodyType();
-				atomic<uint32> &num_active_bodies = mNumActiveBodies[type];
-				BodyID *active_bodies = mActiveBodies[type];
-
-				uint32 last_body_index = num_active_bodies - 1;
-				if (body.mMotionProperties->mIndexInActiveBodies != last_body_index)
-				{
-					// This is not the last body, use the last body to fill the hole
-					BodyID last_body_id = active_bodies[last_body_index];
-					active_bodies[body.mMotionProperties->mIndexInActiveBodies] = last_body_id;
-
-					// Update that body's index in the active list
-					Body &last_body = *mBodies[last_body_id.GetIndex()];
-					JPH_ASSERT(last_body.mMotionProperties->mIndexInActiveBodies == last_body_index);
-					last_body.mMotionProperties->mIndexInActiveBodies = body.mMotionProperties->mIndexInActiveBodies;
-				}
+				// Remove the body from the active bodies list
+				RemoveBodyFromActiveBodies(body);
 
 				// Mark this body as no longer active
-				body.mMotionProperties->mIndexInActiveBodies = Body::cInactiveIndex;
 				body.mMotionProperties->mIslandIndex = Body::cInactiveIndex;
 
 				// Reset velocity
 				body.mMotionProperties->mLinearVelocity = Vec3::sZero();
 				body.mMotionProperties->mAngularVelocity = Vec3::sZero();
 
-				// Remove unused element from active bodies list
-				--num_active_bodies;
-
-				// Count CCD bodies
-				if (body.mMotionProperties->GetMotionQuality() == EMotionQuality::LinearCast)
-					mNumActiveCCDBodies--;
-
 				// Call activation listener
 				if (mActivationListener != nullptr)
 					mActivationListener->OnBodyDeactivated(body_id, body.GetUserData());
@@ -721,80 +738,108 @@ void BodyManager::UnlockAllBodies() const
 	mBodyMutexes.UnlockAll();
 }
 
-void BodyManager::SaveState(StateRecorder &inStream) const
+void BodyManager::SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const
 {
 	{
 		LockAllBodies();
 
-		// Count number of bodies
-		size_t num_bodies = 0;
+		// Determine which bodies to save
+		Array<const Body *> bodies;
+		bodies.reserve(mNumBodies);
 		for (const Body *b : mBodies)
-			if (sIsValidBodyPointer(b) && b->IsInBroadPhase())
-				++num_bodies;
-		inStream.Write(num_bodies);
+			if (sIsValidBodyPointer(b) && b->IsInBroadPhase() && (inFilter == nullptr || inFilter->ShouldSaveBody(*b)))
+				bodies.push_back(b);
 
 		// Write state of bodies
-		for (const Body *b : mBodies)
-			if (sIsValidBodyPointer(b) && b->IsInBroadPhase())
-			{
-				inStream.Write(b->GetID());
-				b->SaveState(inStream);
-			}
-
-		UnlockAllBodies();
-	}
-
-	{
-		UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList));
-
-		// Loop over the body types
-		for (uint type = 0; type < cBodyTypeCount; ++type)
+		size_t num_bodies = bodies.size();
+		inStream.Write(num_bodies);
+		for (const Body *b : bodies)
 		{
-			const atomic<uint32> &num_active_bodies = mNumActiveBodies[type];
-			const BodyID *active_bodies = mActiveBodies[type];
-
-			// Write active bodies, sort because activation can come from multiple threads, so order is not deterministic
-			inStream.Write(num_active_bodies);
-			BodyIDVector sorted_active_bodies(active_bodies, active_bodies + num_active_bodies);
-			QuickSort(sorted_active_bodies.begin(), sorted_active_bodies.end());
-			for (const BodyID &id : sorted_active_bodies)
-				inStream.Write(id);
+			inStream.Write(b->GetID());
+			inStream.Write(b->IsActive());
+			b->SaveState(inStream);
 		}
+
+		UnlockAllBodies();
 	}
 }
 
 bool BodyManager::RestoreState(StateRecorder &inStream)
 {
+	BodyIDVector bodies_to_activate, bodies_to_deactivate;
+
 	{
 		LockAllBodies();
 
-		// Read state of bodies, note this reads it in a way to be consistent with validation
-		size_t old_num_bodies = 0;
-		for (const Body *b : mBodies)
-			if (sIsValidBodyPointer(b) && b->IsInBroadPhase())
-				++old_num_bodies;
-		size_t num_bodies = old_num_bodies; // Initialize to current value for validation
-		inStream.Read(num_bodies);
-		if (num_bodies != old_num_bodies)
+		if (inStream.IsValidating())
 		{
-			JPH_ASSERT(false, "Cannot handle adding/removing bodies");
-			UnlockAllBodies();
-			return false;
+			// Read state of bodies, note this reads it in a way to be consistent with validation
+			size_t old_num_bodies = 0;
+			for (const Body *b : mBodies)
+				if (sIsValidBodyPointer(b) && b->IsInBroadPhase())
+					++old_num_bodies;
+			size_t num_bodies = old_num_bodies; // Initialize to current value for validation
+			inStream.Read(num_bodies);
+			if (num_bodies != old_num_bodies)
+			{
+				JPH_ASSERT(false, "Cannot handle adding/removing bodies");
+				UnlockAllBodies();
+				return false;
+			}
+
+			for (Body *b : mBodies)
+				if (sIsValidBodyPointer(b) && b->IsInBroadPhase())
+				{
+					BodyID body_id = b->GetID(); // Initialize to current value for validation
+					inStream.Read(body_id);
+					if (body_id != b->GetID())
+					{
+						JPH_ASSERT(false, "Cannot handle adding/removing bodies");
+						UnlockAllBodies();
+						return false;
+					}
+					bool is_active = b->IsActive(); // Initialize to current value for validation
+					inStream.Read(is_active);
+					if (is_active != b->IsActive())
+					{
+						if (is_active)
+							bodies_to_activate.push_back(body_id);
+						else
+							bodies_to_deactivate.push_back(body_id);
+					}
+					b->RestoreState(inStream);
+				}
 		}
+		else
+		{
+			// Not validating, we can be a bit more loose, read number of bodies
+			size_t num_bodies = 0;
+			inStream.Read(num_bodies);
 
-		for (Body *b : mBodies)
-			if (sIsValidBodyPointer(b) && b->IsInBroadPhase())
+			// Iterate over the stored bodies and restore their state
+			for (size_t idx = 0; idx < num_bodies; ++idx)
 			{
-				BodyID body_id = b->GetID(); // Initialize to current value for validation
+				BodyID body_id;
 				inStream.Read(body_id);
-				if (body_id != b->GetID())
+				Body *b = TryGetBody(body_id);
+				if (b == nullptr)
 				{
-					JPH_ASSERT(false, "Cannot handle adding/removing bodies");
+					JPH_ASSERT(false, "Restoring state for non-existing body");
 					UnlockAllBodies();
 					return false;
 				}
+				bool is_active;
+				inStream.Read(is_active);
+				if (is_active != b->IsActive())
+				{
+					if (is_active)
+						bodies_to_activate.push_back(body_id);
+					else
+						bodies_to_deactivate.push_back(body_id);
+				}
 				b->RestoreState(inStream);
 			}
+		}
 
 		UnlockAllBodies();
 	}
@@ -802,28 +847,19 @@ bool BodyManager::RestoreState(StateRecorder &inStream)
 	{
 		UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList));
 
-		// Loop over the body types
-		for (uint type = 0; type < cBodyTypeCount; ++type)
+		for (BodyID body_id : bodies_to_activate)
 		{
-			atomic<uint32> &num_active_bodies = mNumActiveBodies[type];
-			BodyID *active_bodies = mActiveBodies[type];
-
-			// Mark current active bodies as deactivated
-			for (const BodyID *id = active_bodies, *id_end = active_bodies + num_active_bodies; id < id_end; ++id)
-				mBodies[id->GetIndex()]->mMotionProperties->mIndexInActiveBodies = Body::cInactiveIndex;
-
-			QuickSort(active_bodies, active_bodies + num_active_bodies); // Sort for validation
+			Body *body = TryGetBody(body_id);
+			AddBodyToActiveBodies(*body);
+		}
 
-			// Read active bodies
-			inStream.Read(num_active_bodies);
-			for (BodyID *id = active_bodies, *id_end = active_bodies + num_active_bodies; id < id_end; ++id)
-			{
-				inStream.Read(*id);
-				mBodies[id->GetIndex()]->mMotionProperties->mIndexInActiveBodies = uint32(id - active_bodies);
-			}
+		for (BodyID body_id : bodies_to_deactivate)
+		{
+			Body *body = TryGetBody(body_id);
+			RemoveBodyFromActiveBodies(*body);
 		}
 
-		// Count CCD bodies
+		// Count CCD bodies (needs to be done because Body::RestoreState can change the motion quality without notifying the system)
 		mNumActiveCCDBodies = 0;
 		for (const BodyID *id = mActiveBodies[(int)EBodyType::RigidBody], *end = id + mNumActiveBodies[(int)EBodyType::RigidBody]; id < end; ++id)
 			if (mBodies[id->GetIndex()]->GetMotionProperties()->GetMotionQuality() == EMotionQuality::LinearCast)

Jolt/Physics/Body/BodyManager.h

@@ -14,6 +14,7 @@ JPH_NAMESPACE_BEGIN
 class BodyCreationSettings;
 class SoftBodyCreationSettings;
 class BodyActivationListener;
+class StateRecorderFilter;
 struct PhysicsSettings;
 #ifdef JPH_DEBUG_RENDERER
 class DebugRenderer;
@@ -191,7 +192,7 @@ public:
 	void							ValidateContactCacheForAllBodies();
 
 	/// Saving state for replay
-	void							SaveState(StateRecorder &inStream) const;
+	void							SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const;
 
 	/// Restoring state for replay. Returns false if failed.
 	bool							RestoreState(StateRecorder &inStream);
@@ -269,6 +270,12 @@ private:
 #endif
 	inline uint8					GetNextSequenceNumber(int inBodyIndex)		{ return ++mBodySequenceNumbers[inBodyIndex]; }
 
+	/// Add a single body to mActiveBodies, note doesn't lock the active body mutex!
+	inline void						AddBodyToActiveBodies(Body &ioBody);
+
+	/// Remove a single body from mActiveBodies, note doesn't lock the active body mutex!
+	inline void						RemoveBodyFromActiveBodies(Body &ioBody);
+
 	/// Helper function to remove a body from the manager
 	JPH_INLINE Body *				RemoveBodyInternal(const BodyID &inBodyID);
 

Jolt/Physics/Constraints/ConstraintManager.cpp

@@ -223,32 +223,86 @@ void ConstraintManager::DrawConstraintReferenceFrame(DebugRenderer *inRenderer)
 }
 #endif // JPH_DEBUG_RENDERER
 
-void ConstraintManager::SaveState(StateRecorder &inStream) const
+void ConstraintManager::SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const
 {
 	UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList));
 
 	// Write state of constraints
-	size_t num_constraints = mConstraints.size();
-	inStream.Write(num_constraints);
-	for (const Ref<Constraint> &c : mConstraints)
-		c->SaveState(inStream);
+	if (inFilter != nullptr)
+	{
+		// Determine which constraints to save
+		Array<Constraint *> constraints;
+		constraints.reserve(mConstraints.size());
+		for (const Ref<Constraint> &c : mConstraints)
+			if (inFilter->ShouldSaveConstraint(*c))
+				constraints.push_back(c);
+
+		// Save them
+		size_t num_constraints = constraints.size();
+		inStream.Write(num_constraints);
+		for (const Constraint *c : constraints)
+		{
+			inStream.Write(c->mConstraintIndex);
+			c->SaveState(inStream);
+		}
+	}
+	else
+	{
+		// Save all constraints
+		size_t num_constraints = mConstraints.size();
+		inStream.Write(num_constraints);
+		for (const Ref<Constraint> &c : mConstraints)
+		{
+			inStream.Write(c->mConstraintIndex);
+			c->SaveState(inStream);
+		}
+	}
 }
 
 bool ConstraintManager::RestoreState(StateRecorder &inStream)
 {
 	UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList));
 
-	// Read state of constraints
-	size_t num_constraints = mConstraints.size(); // Initialize to current value for validation
-	inStream.Read(num_constraints);
-	if (num_constraints != mConstraints.size())
+	if (inStream.IsValidating())
 	{
-		JPH_ASSERT(false, "Cannot handle adding/removing constraints");
-		return false;
+		// Read state of constraints
+		size_t num_constraints = mConstraints.size(); // Initialize to current value for validation
+		inStream.Read(num_constraints);
+		if (num_constraints != mConstraints.size())
+		{
+			JPH_ASSERT(false, "Cannot handle adding/removing constraints");
+			return false;
+		}
+		for (const Ref<Constraint> &c : mConstraints)
+		{
+			uint32 constraint_index = c->mConstraintIndex;
+			inStream.Read(constraint_index);
+			if (constraint_index != c->mConstraintIndex)
+			{
+				JPH_ASSERT(false, "Unexpected constraint index");
+				return false;
+			}
+			c->RestoreState(inStream);
+		}
 	}
+	else
+	{
+		// Not validating, use more flexible reading, read number of constraints
+		size_t num_constraints = 0;
+		inStream.Read(num_constraints);
 
-	for (const Ref<Constraint> &c : mConstraints)
-		c->RestoreState(inStream);
+		for (size_t idx = 0; idx < num_constraints; ++idx)
+		{
+			uint32 constraint_index;
+			inStream.Read(constraint_index);
+			if (mConstraints.size() <= constraint_index)
+			{
+				JPH_ASSERT(false, "Restoring state for non-existing constraint");
+				return false;
+			}
+			mConstraints[constraint_index]->RestoreState(inStream);
+		}
+	}
 
 	return true;
 }

Jolt/Physics/Constraints/ConstraintManager.h

@@ -12,6 +12,7 @@ JPH_NAMESPACE_BEGIN
 
 class IslandBuilder;
 class BodyManager;
+class StateRecorderFilter;
 #ifdef JPH_DEBUG_RENDERER
 class DebugRenderer;
 #endif // JPH_DEBUG_RENDERER
@@ -83,7 +84,7 @@ public:
 #endif // JPH_DEBUG_RENDERER
 
 	/// Save state of constraints
-	void					SaveState(StateRecorder &inStream) const;
+	void					SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const;
 
 	/// Restore the state of constraints. Returns false if failed.
 	bool					RestoreState(StateRecorder &inStream);

Jolt/Physics/Constraints/ContactConstraintManager.cpp

@@ -132,7 +132,7 @@ JPH_INLINE void ContactConstraintManager::WorldContactPoint::TemplatedCalculateF
 
 #ifdef JPH_DEBUG_RENDERER
 void ContactConstraintManager::ContactConstraint::Draw(DebugRenderer *inRenderer, ColorArg inManifoldColor) const
-{			
+{
 	if (mContactPoints.empty())
 		return;
 
@@ -144,8 +144,8 @@ void ContactConstraintManager::ContactConstraint::Draw(DebugRenderer *inRenderer
 	for (const WorldContactPoint &wcp : mContactPoints)
 	{
 		// Test if any lambda from the previous frame was transferred
-		float radius = wcp.mNonPenetrationConstraint.GetTotalLambda() == 0.0f 
-					&& wcp.mFrictionConstraint1.GetTotalLambda() == 0.0f 
+		float radius = wcp.mNonPenetrationConstraint.GetTotalLambda() == 0.0f
+					&& wcp.mFrictionConstraint1.GetTotalLambda() == 0.0f
 					&& wcp.mFrictionConstraint2.GetTotalLambda() == 0.0f? 0.1f :  0.2f;
 
 		RVec3 next_point = transform_body1 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition1);
@@ -287,19 +287,19 @@ ContactConstraintManager::MKVAndCreated ContactConstraintManager::ManifoldCache:
 
 uint32 ContactConstraintManager::ManifoldCache::ToHandle(const MKeyValue *inKeyValue) const
 {
-	JPH_ASSERT(!mIsFinalized);	
+	JPH_ASSERT(!mIsFinalized);
 	return mCachedManifolds.ToHandle(inKeyValue);
 }
 
 const ContactConstraintManager::MKeyValue *ContactConstraintManager::ManifoldCache::FromHandle(uint32 inHandle) const
 {
-	JPH_ASSERT(mIsFinalized);	
+	JPH_ASSERT(mIsFinalized);
 	return mCachedManifolds.FromHandle(inHandle);
 }
 
 const ContactConstraintManager::BPKeyValue *ContactConstraintManager::ManifoldCache::Find(const BodyPair &inKey, uint64 inKeyHash) const
 {
-	JPH_ASSERT(mIsFinalized);	
+	JPH_ASSERT(mIsFinalized);
 	return mCachedBodyPairs.Find(inKey, inKeyHash);
 }
 
@@ -386,7 +386,7 @@ void ContactConstraintManager::ManifoldCache::Finalize()
 
 #endif
 
-void ContactConstraintManager::ManifoldCache::SaveState(StateRecorder &inStream) const
+void ContactConstraintManager::ManifoldCache::SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const
 {
 	JPH_ASSERT(mIsFinalized);
 
@@ -394,12 +394,22 @@ void ContactConstraintManager::ManifoldCache::SaveState(StateRecorder &inStream)
 	Array<const BPKeyValue *> all_bp;
 	GetAllBodyPairsSorted(all_bp);
 
-	// Write amount of body pairs
-	size_t num_body_pairs = all_bp.size();
-	inStream.Write(num_body_pairs);
+	// Determine which ones to save
+	Array<const BPKeyValue *> selected_bp;
+	if (inFilter == nullptr)
+		selected_bp = std::move(all_bp);
+	else
+	{
+		selected_bp.reserve(all_bp.size());
+		for (const BPKeyValue *bp_kv : all_bp)
+			if (inFilter->ShouldSaveContact(bp_kv->GetKey().mBodyA, bp_kv->GetKey().mBodyB))
+				selected_bp.push_back(bp_kv);
+	}
 
-	// Write all body pairs
-	for (const BPKeyValue *bp_kv : all_bp)
+	// Write body pairs
+	size_t num_body_pairs = selected_bp.size();
+	inStream.Write(num_body_pairs);
+	for (const BPKeyValue *bp_kv : selected_bp)
 	{
 		// Write body pair key
 		inStream.Write(bp_kv->GetKey());
@@ -440,12 +450,22 @@ void ContactConstraintManager::ManifoldCache::SaveState(StateRecorder &inStream)
 	Array<const MKeyValue *> all_m;
 	GetAllCCDManifoldsSorted(all_m);
 
-	// Write num CCD manifolds
-	size_t num_manifolds = all_m.size();
-	inStream.Write(num_manifolds);
+	// Determine which ones to save
+	Array<const MKeyValue *> selected_m;
+	if (inFilter == nullptr)
+		selected_m = std::move(all_m);
+	else
+	{
+		selected_m.reserve(all_m.size());
+		for (const MKeyValue *m_kv : all_m)
+			if (inFilter->ShouldSaveContact(m_kv->GetKey().GetBody1ID(), m_kv->GetKey().GetBody2ID()))
+				selected_m.push_back(m_kv);
+	}
 
 	// Write all CCD manifold keys
-	for (const MKeyValue *m_kv : all_m)
+	size_t num_manifolds = selected_m.size();
+	inStream.Write(num_manifolds);
+	for (const MKeyValue *m_kv : selected_m)
 		inStream.Write(m_kv->GetKey());
 }
 
@@ -485,7 +505,7 @@ bool ContactConstraintManager::ManifoldCache::RestoreState(const ManifoldCache &
 		{
 			// Out of cache space
 			success = false;
-			break; 
+			break;
 		}
 		CachedBodyPair &bp = bp_kv->GetValue();
 
@@ -514,7 +534,7 @@ bool ContactConstraintManager::ManifoldCache::RestoreState(const ManifoldCache &
 				sub_shape_key = all_m[j]->GetKey();
 			inStream.Read(sub_shape_key);
 			uint64 sub_shape_key_hash = sub_shape_key.GetHash();
-			
+
 			// Read amount of contact points
 			uint16 num_contact_points;
 			if (inStream.IsValidating() && j < all_m.size())
@@ -527,7 +547,7 @@ bool ContactConstraintManager::ManifoldCache::RestoreState(const ManifoldCache &
 			{
 				// Out of cache space
 				success = false;
-				break; 
+				break;
 			}
 			CachedManifold &cm = m_kv->GetValue();
 			if (inStream.IsValidating() && j < all_m.size())
@@ -542,7 +562,7 @@ bool ContactConstraintManager::ManifoldCache::RestoreState(const ManifoldCache &
 			// Read contact points
 			for (uint32 k = 0; k < num_contact_points; ++k)
 				cm.mContactPoints[k].RestoreState(inStream);
-		}		
+		}
 		bp.mFirstCachedManifold = handle;
 	}
 
@@ -565,14 +585,14 @@ bool ContactConstraintManager::ManifoldCache::RestoreState(const ManifoldCache &
 			sub_shape_key = all_m[j]->GetKey();
 		inStream.Read(sub_shape_key);
 		uint64 sub_shape_key_hash = sub_shape_key.GetHash();
-			
+
 		// Create CCD manifold
 		MKeyValue *m_kv = Create(contact_allocator, sub_shape_key, sub_shape_key_hash, 0);
 		if (m_kv == nullptr)
 		{
 			// Out of cache space
 			success = false;
-			break; 
+			break;
 		}
 		CachedManifold &cm = m_kv->GetValue();
 		cm.mFlags |= (uint16)CachedManifold::EFlags::CCDContact;
@@ -616,7 +636,7 @@ void ContactConstraintManager::Init(uint inMaxBodyPairs, uint inMaxContactConstr
 }
 
 void ContactConstraintManager::PrepareConstraintBuffer(PhysicsUpdateContext *inContext)
-{	
+{
 	// Store context
 	mUpdateContext = inContext;
 
@@ -727,7 +747,7 @@ void ContactConstraintManager::GetContactsFromCache(ContactAllocator &ioContactA
 	Body *body1, *body2;
 	if (inBody1.GetID() < inBody2.GetID())
 	{
-		body1 = &inBody1; 
+		body1 = &inBody1;
 		body2 = &inBody2;
 	}
 	else
@@ -854,7 +874,7 @@ void ContactConstraintManager::GetContactsFromCache(ContactAllocator &ioContactA
 		JPH_ASSERT(settings.mIsSensor || !(body1->IsSensor() || body2->IsSensor()), "Sensors cannot be converted into regular bodies by a contact callback!");
 		if (!settings.mIsSensor // If one of the bodies is a sensor, don't actually create the constraint
 			&& ((body1->IsDynamic() && settings.mInvMassScale1 != 0.0f) // One of the bodies must have mass to be able to create a contact constraint
-				|| (body2->IsDynamic() && settings.mInvMassScale2 != 0.0f))) 
+				|| (body2->IsDynamic() && settings.mInvMassScale2 != 0.0f)))
 		{
 			// Add contact constraint in world space for the solver
 			uint32 constraint_idx = mNumConstraints++;
@@ -863,7 +883,7 @@ void ContactConstraintManager::GetContactsFromCache(ContactAllocator &ioContactA
 				ioContactAllocator.mErrors |= EPhysicsUpdateError::ContactConstraintsFull;
 				break;
 			}
-			
+
 			// A constraint will be created
 			outConstraintCreated = true;
 
@@ -1041,7 +1061,7 @@ bool ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &i
 		}
 	}
 	else if ((inBody1.IsDynamic() && settings.mInvMassScale1 != 0.0f) // One of the bodies must have mass to be able to create a contact constraint
-			|| (inBody2.IsDynamic() && settings.mInvMassScale2 != 0.0f)) 
+			|| (inBody2.IsDynamic() && settings.mInvMassScale2 != 0.0f))
 	{
 		// Add contact constraint
 		uint32 constraint_idx = mNumConstraints++;
@@ -1051,13 +1071,13 @@ bool ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &i
 
 			// Manifold has been created already, we're not filling it in, so we need to reset the contact number of points.
 			// Note that we don't hook it up to the body pair cache so that it won't be used as a cache during the next simulation.
-			new_manifold->mNumContactPoints = 0; 
+			new_manifold->mNumContactPoints = 0;
 			return false;
 		}
 
 		// We will create a contact constraint
 		contact_constraint_created = true;
-		
+
 		ContactConstraint &constraint = mConstraints[constraint_idx];
 		new (&constraint) ContactConstraint();
 		constraint.mBody1 = &inBody1;
@@ -1118,11 +1138,11 @@ bool ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &i
 			// Convert to local space to the body
 			Vec3 p1_ls = Vec3(inverse_transform_body1 * p1_ws);
 			Vec3 p2_ls = Vec3(inverse_transform_body2 * p2_ws);
-						
+
 			// Check if we have a close contact point from last update
 			bool lambda_set = false;
 			for (const CachedContactPoint *ccp = ccp_start; ccp < ccp_end; ccp++)
-				if (Vec3::sLoadFloat3Unsafe(ccp->mPosition1).IsClose(p1_ls, mPhysicsSettings.mContactPointPreserveLambdaMaxDistSq) 
+				if (Vec3::sLoadFloat3Unsafe(ccp->mPosition1).IsClose(p1_ls, mPhysicsSettings.mContactPointPreserveLambdaMaxDistSq)
 					&& Vec3::sLoadFloat3Unsafe(ccp->mPosition2).IsClose(p2_ls, mPhysicsSettings.mContactPointPreserveLambdaMaxDistSq))
 				{
 					// Get lambdas from previous frame
@@ -1172,7 +1192,7 @@ bool ContactConstraintManager::AddContactConstraint(ContactAllocator &ioContactA
 	JPH_DET_LOG("AddContactConstraint: id1: " << inBody1.GetID() << " id2: " << inBody2.GetID()
 		<< " subshape1: " << inManifold.mSubShapeID1 << " subshape2: " << inManifold.mSubShapeID2
 		<< " normal: " << inManifold.mWorldSpaceNormal << " pendepth: " << inManifold.mPenetrationDepth);
-	
+
 	JPH_ASSERT(inManifold.mWorldSpaceNormal.IsNormalized());
 
 	// Swap bodies so that body 1 id < body 2 id
@@ -1245,7 +1265,7 @@ bool ContactConstraintManager::AddContactConstraint(ContactAllocator &ioContactA
 			return TemplatedAddContactConstraint<EMotionType::Static, EMotionType::Kinematic>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold);
 
 		case EMotionType::Static: // Static vs static not possible
-		default: 
+		default:
 			JPH_ASSERT(false);
 			break;
 		}
@@ -1326,7 +1346,7 @@ void ContactConstraintManager::OnCCDContactAdded(ContactAllocator &ioContactAllo
 		}
 		else
 		{
-			// Already found this contact this physics update. 
+			// Already found this contact this physics update.
 			// Note that we can trigger OnContactPersisted multiple times per physics update, but otherwise we have no way of obtaining the settings
 			mContactListener->OnContactPersisted(*body1, *body2, *manifold, outSettings);
 		}
@@ -1413,7 +1433,7 @@ JPH_INLINE void ContactConstraintManager::sWarmStartConstraint(ContactConstraint
 	ioConstraint.GetTangents(t1, t2);
 
 	Vec3 ws_normal = ioConstraint.GetWorldSpaceNormal();
-		
+
 	for (WorldContactPoint &wcp : ioConstraint.mContactPoints)
 	{
 		// Warm starting: Apply impulse from last frame
@@ -1443,7 +1463,7 @@ void ContactConstraintManager::WarmStartVelocityConstraints(const uint32 *inCons
 		Body &body2 = *constraint.mBody2;
 		EMotionType motion_type2 = body2.GetMotionType();
 		MotionProperties *motion_properties2 = body2.GetMotionPropertiesUnchecked();
-				
+
 		// Dispatch to the correct templated form
 		// Note: Warm starting doesn't differentiate between kinematic/static bodies so we handle both as static bodies
 		if (motion_type1 == EMotionType::Dynamic)
@@ -1659,9 +1679,9 @@ void ContactConstraintManager::FinishConstraintBuffer()
 	mUpdateContext = nullptr;
 }
 
-void ContactConstraintManager::SaveState(StateRecorder &inStream) const
+void ContactConstraintManager::SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const
 {
-	mCache[mCacheWriteIdx ^ 1].SaveState(inStream);
+	mCache[mCacheWriteIdx ^ 1].SaveState(inStream, inFilter);
 }
 
 bool ContactConstraintManager::RestoreState(StateRecorder &inStream)

Jolt/Physics/Constraints/ContactConstraintManager.h

@@ -247,7 +247,7 @@ public:
 #endif // JPH_DEBUG_RENDERER
 
 	/// Saving state for replay
-	void						SaveState(StateRecorder &inStream) const;
+	void						SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const;
 
 	/// Restoring state for replay. Returns false when failed.
 	bool						RestoreState(StateRecorder &inStream);
@@ -391,7 +391,7 @@ private:
 #endif
 
 		/// Saving / restoring state for replay
-		void					SaveState(StateRecorder &inStream) const;
+		void					SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const;
 		bool					RestoreState(const ManifoldCache &inReadCache, StateRecorder &inStream);
 
 	private:

Jolt/Physics/PhysicsSystem.cpp

@@ -2369,35 +2369,58 @@ void PhysicsSystem::JobUpdateSoftBodies(const PhysicsUpdateContext *ioContext)
 		mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false);
 }
 
-void PhysicsSystem::SaveState(StateRecorder &inStream) const
+void PhysicsSystem::SaveState(StateRecorder &inStream, EStateRecorderState inState, const StateRecorderFilter *inFilter) const
 {
 	JPH_PROFILE_FUNCTION();
 
-	inStream.Write(mPreviousStepDeltaTime);
-	inStream.Write(mGravity);
+	inStream.Write(inState);
 
-	mBodyManager.SaveState(inStream);
+	if (uint8(inState) & uint8(EStateRecorderState::Global))
+	{
+		inStream.Write(mPreviousStepDeltaTime);
+		inStream.Write(mGravity);
+	}
+
+	if (uint8(inState) & uint8(EStateRecorderState::Bodies))
+		mBodyManager.SaveState(inStream, inFilter);
 
-	mContactManager.SaveState(inStream);
+	if (uint8(inState) & uint8(EStateRecorderState::Contacts))
+		mContactManager.SaveState(inStream, inFilter);
 
-	mConstraintManager.SaveState(inStream);
+	if (uint8(inState) & uint8(EStateRecorderState::Constraints))
+		mConstraintManager.SaveState(inStream, inFilter);
 }
 
 bool PhysicsSystem::RestoreState(StateRecorder &inStream)
 {
 	JPH_PROFILE_FUNCTION();
 
-	inStream.Read(mPreviousStepDeltaTime);
-	inStream.Read(mGravity);
+	EStateRecorderState state = EStateRecorderState::None;
+	inStream.Read(state);
+
+	if (uint8(state) & uint8(EStateRecorderState::Global))
+	{
+		inStream.Read(mPreviousStepDeltaTime);
+		inStream.Read(mGravity);
+	}
 
-	if (!mBodyManager.RestoreState(inStream))
-		return false;
+	if (uint8(state) & uint8(EStateRecorderState::Bodies))
+	{
+		if (!mBodyManager.RestoreState(inStream))
+			return false;
+	}
 
-	if (!mContactManager.RestoreState(inStream))
-		return false;
+	if (uint8(state) & uint8(EStateRecorderState::Contacts))
+	{
+		if (!mContactManager.RestoreState(inStream))
+			return false;
+	}
 
-	if (!mConstraintManager.RestoreState(inStream))
-		return false;
+	if (uint8(state) & uint8(EStateRecorderState::Constraints))
+	{
+		if (!mConstraintManager.RestoreState(inStream))
+			return false;
+	}
 
 	// Update bounding boxes for all bodies in the broadphase
 	Array<BodyID> bodies;

Jolt/Physics/PhysicsSystem.h

@@ -109,7 +109,7 @@ public:
 	EPhysicsUpdateError			Update(float inDeltaTime, int inCollisionSteps, TempAllocator *inTempAllocator, JobSystem *inJobSystem);
 
 	/// Saving state for replay
-	void						SaveState(StateRecorder &inStream) const;
+	void						SaveState(StateRecorder &inStream, EStateRecorderState inState = EStateRecorderState::All, const StateRecorderFilter *inFilter = nullptr) const;
 
 	/// Restoring state for replay. Returns false if failed.
 	bool						RestoreState(StateRecorder &inStream);

Jolt/Physics/StateRecorder.h

@@ -9,6 +9,38 @@
 
 JPH_NAMESPACE_BEGIN
 
+class Body;
+class Constraint;
+class BodyID;
+
+/// A bit field that determines which aspects of the simulation to save
+enum class EStateRecorderState : uint8
+{
+	None				= 0,														///< Save nothing
+	Global				= 1,														///< Save global physics system state (delta time, gravity, etc.)
+	Bodies				= 2,														///< Save the state of bodies
+	Contacts			= 4,														///< Save the state of contacts
+	Constraints			= 8,														///< Save the state of constraints
+	All					= Global | Bodies | Contacts | Constraints					///< Save all state
+};
+
+/// User callbacks that allow determining which parts of the simulation should be saved by a StateRecorder
+class JPH_EXPORT StateRecorderFilter
+{
+public:
+	/// Destructor
+	virtual				~StateRecorderFilter() = default;
+
+	/// If the state of a specific body should be saved
+	virtual bool		ShouldSaveBody(const Body &inBody) const					{ return true; }
+
+	/// If the state of a specific constraint should be saved
+	virtual bool		ShouldSaveConstraint(const Constraint &inConstraint) const	{ return true; }
+
+	/// If the state of a specific contact should be saved
+	virtual bool		ShouldSaveContact(const BodyID &inBody1, const BodyID &inBody2) const { return true; }
+};
+
 /// Class that records the state of a physics system. Can be used to check if the simulation is deterministic by putting the recorder in validation mode.
 /// Can be used to restore the state to an earlier point in time.
 class JPH_EXPORT StateRecorder : public StreamIn, public StreamOut

UnitTests/Physics/PhysicsTests.cpp

@@ -12,6 +12,7 @@
 #include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
 #include <Jolt/Physics/Body/BodyLockMulti.h>
 #include <Jolt/Physics/Constraints/PointConstraint.h>
+#include <Jolt/Physics/StateRecorderImpl.h>
 
 TEST_SUITE("PhysicsTests")
 {
@@ -113,7 +114,7 @@ TEST_SUITE("PhysicsTests")
 			CHECK_FALSE(lock1.Succeeded());
 			CHECK_FALSE(lock1.SucceededAndIsInBroadPhase());
 		}
-	}		
+	}
 
 	TEST_CASE("TestPhysicsBodyLockMulti")
 	{
@@ -378,12 +379,12 @@ TEST_SUITE("PhysicsTests")
 		Body &body = ioContext.CreateBox(cInitialPos, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1));
 		CHECK_APPROX_EQUAL(cInitialPos, body.GetPosition());
 		CHECK_APPROX_EQUAL(Vec3::sZero(), body.GetLinearVelocity());
-			
+
 		ioContext.Simulate(cSimulationTime);
-			
+
 		// Test resulting velocity (due to gravity)
 		CHECK_APPROX_EQUAL(cSimulationTime * cGravity, body.GetLinearVelocity(), 1.0e-4f);
-			
+
 		// Test resulting position
 		RVec3 expected_pos = ioContext.PredictPosition(cInitialPos, Vec3::sZero(), cGravity, cSimulationTime);
 		CHECK_APPROX_EQUAL(expected_pos, body.GetPosition());
@@ -422,10 +423,10 @@ TEST_SUITE("PhysicsTests")
 
 		// Simulate while applying force
 		ioContext.Simulate(cSimulationTime, [&]() { body.AddForce(mass * cAcceleration); });
-			
+
 		// Test resulting velocity (due to gravity and applied force)
 		CHECK_APPROX_EQUAL(cSimulationTime * (cGravity + cAcceleration), body.GetLinearVelocity(), 1.0e-4f);
-			
+
 		// Test resulting position
 		RVec3 expected_pos = ioContext.PredictPosition(cInitialPos, Vec3::sZero(), cGravity + cAcceleration, cSimulationTime);
 		CHECK_APPROX_EQUAL(expected_pos, body.GetPosition());
@@ -463,13 +464,13 @@ TEST_SUITE("PhysicsTests")
 		CHECK_APPROX_EQUAL(1.0f / mass, body.GetMotionProperties()->GetInverseMass());
 		constexpr float inertia = mass * 8.0f / 12.0f; // See: https://en.wikipedia.org/wiki/List_of_moments_of_inertia
 		CHECK_APPROX_EQUAL(Mat44::sScale(1.0f / inertia), body.GetMotionProperties()->GetLocalSpaceInverseInertia());
-			
+
 		// Simulate while applying torque
 		ioContext.Simulate(cSimulationTime, [&]() { body.AddTorque(inertia * cAngularAcceleration); });
-			
+
 		// Get resulting angular velocity
 		CHECK_APPROX_EQUAL(cSimulationTime * cAngularAcceleration, body.GetAngularVelocity(), 1.0e-4f);
-			
+
 		// Test resulting rotation
 		Quat expected_rot = ioContext.PredictOrientation(Quat::sIdentity(), Vec3::sZero(), cAngularAcceleration, cSimulationTime);
 		CHECK_APPROX_EQUAL(expected_rot, body.GetRotation(), 1.0e-4f);
@@ -488,7 +489,7 @@ TEST_SUITE("PhysicsTests")
 
 		PhysicsTestContext c2(4.0f / 60.0f, 4);
 		TestPhysicsApplyTorque(c2);
-	}		
+	}
 
 	// Let a sphere bounce on the floor with restition = 1
 	static void TestPhysicsCollisionElastic(PhysicsTestContext &ioContext)
@@ -509,13 +510,13 @@ TEST_SUITE("PhysicsTests")
 		CHECK_APPROX_EQUAL(cFloorHitPos, body.GetPosition());
 
 		// Assert collision not yet processed
-		CHECK_APPROX_EQUAL(cSimulationTime * cGravity, body.GetLinearVelocity(), 1.0e-4f); 
+		CHECK_APPROX_EQUAL(cSimulationTime * cGravity, body.GetLinearVelocity(), 1.0e-4f);
 
 		// Simulate one more step to process the collision
 		ioContext.Simulate(ioContext.GetDeltaTime());
 
-		// Assert that collision is processed and velocity is reversed (which is required for a fully elastic collision). 
-		// Note that the physics engine will first apply gravity for the time step and then do collision detection, 
+		// Assert that collision is processed and velocity is reversed (which is required for a fully elastic collision).
+		// Note that the physics engine will first apply gravity for the time step and then do collision detection,
 		// hence the reflected velocity is actually 1 step times gravity bigger than it would be in reality
 		// For the remainder of cDeltaTime normal gravity will be applied
 		float sub_step_delta_time = ioContext.GetStepDeltaTime();
@@ -550,7 +551,7 @@ TEST_SUITE("PhysicsTests")
 
 		PhysicsTestContext c2(4.0f / 60.0f, 4);
 		TestPhysicsCollisionElastic(c2);
-	}		
+	}
 
 	// Let a sphere bounce on the floor with restitution = 0
 	static void TestPhysicsCollisionInelastic(PhysicsTestContext &ioContext)
@@ -571,7 +572,7 @@ TEST_SUITE("PhysicsTests")
 		CHECK_APPROX_EQUAL(cFloorHitPos, body.GetPosition());
 
 		// Assert collision not yet processed
-		CHECK_APPROX_EQUAL(cSimulationTime * cGravity, body.GetLinearVelocity(), 1.0e-4f); 
+		CHECK_APPROX_EQUAL(cSimulationTime * cGravity, body.GetLinearVelocity(), 1.0e-4f);
 
 		// Simulate one more step to process the collision
 		ioContext.Simulate(ioContext.GetDeltaTime());
@@ -601,8 +602,8 @@ TEST_SUITE("PhysicsTests")
 
 		PhysicsTestContext c2(4.0f / 60.0f, 4);
 		TestPhysicsCollisionInelastic(c2);
-	}		
-		
+	}
+
 	// Let box intersect with floor by cPenetrationSlop. It should not move, this is the maximum penetration allowed.
 	static void TestPhysicsPenetrationSlop1(PhysicsTestContext &ioContext)
 	{
@@ -636,7 +637,7 @@ TEST_SUITE("PhysicsTests")
 
 		PhysicsTestContext c2(4.0f / 60.0f, 4);
 		TestPhysicsPenetrationSlop1(c2);
-	}		
+	}
 
 	// Let box intersect with floor with more than cPenetrationSlop. It should be resolved by SolvePositionConstraint until interpenetration is cPenetrationSlop.
 	static void TestPhysicsPenetrationSlop2(PhysicsTestContext &ioContext)
@@ -672,7 +673,7 @@ TEST_SUITE("PhysicsTests")
 
 		PhysicsTestContext c2(4.0f / 60.0f, 4);
 		TestPhysicsPenetrationSlop2(c2);
-	}		
+	}
 
 	// Let box intersect with floor with less than cPenetrationSlop. Body should not move because SolveVelocityConstraint should reset velocity.
 	static void TestPhysicsPenetrationSlop3(PhysicsTestContext &ioContext)
@@ -797,7 +798,7 @@ TEST_SUITE("PhysicsTests")
 
 		// Sphere has only 1 contact point so is much more accurate
 		CHECK_APPROX_EQUAL(sphere.GetPosition(), RVec3(5, 1, 0));
-		CHECK_APPROX_EQUAL(sphere.GetLinearVelocity(), cExpectedVelocity, 1.0e-4f); 
+		CHECK_APPROX_EQUAL(sphere.GetLinearVelocity(), cExpectedVelocity, 1.0e-4f);
 		CHECK_APPROX_EQUAL(sphere.GetAngularVelocity(), Vec3::sZero(), 1.0e-4f);
 
 		// Simulate a step
@@ -815,7 +816,7 @@ TEST_SUITE("PhysicsTests")
 
 		// Sphere should have come to rest
 		CHECK_APPROX_EQUAL(sphere.GetPosition(), RVec3(5, 1, 0), 1.0e-4f);
-		CHECK_APPROX_EQUAL(sphere.GetLinearVelocity(), Vec3::sZero(), 1.0e-4f); 
+		CHECK_APPROX_EQUAL(sphere.GetLinearVelocity(), Vec3::sZero(), 1.0e-4f);
 		CHECK_APPROX_EQUAL(sphere.GetAngularVelocity(), Vec3::sZero(), 1.0e-4f);
 	}
 
@@ -858,12 +859,12 @@ TEST_SUITE("PhysicsTests")
 
 		// Box collision is less accurate than sphere as it hits with 4 corners so there's some floating point precision loss in the calculation
 		CHECK_APPROX_EQUAL(box.GetPosition(), cInitialPosBox - cVelocity * c.GetDeltaTime(), 0.01f);
-		CHECK_APPROX_EQUAL(box.GetLinearVelocity(), -cVelocity, 0.1f); 
+		CHECK_APPROX_EQUAL(box.GetLinearVelocity(), -cVelocity, 0.1f);
 		CHECK_APPROX_EQUAL(box.GetAngularVelocity(), Vec3::sZero(), 0.02f);
 
 		// Sphere has only 1 contact point so is much more accurate
 		CHECK_APPROX_EQUAL(sphere.GetPosition(), cInitialPosSphere - cVelocity * c.GetDeltaTime(), 1.0e-5f);
-		CHECK_APPROX_EQUAL(sphere.GetLinearVelocity(), -cVelocity, 2.0e-4f); 
+		CHECK_APPROX_EQUAL(sphere.GetLinearVelocity(), -cVelocity, 2.0e-4f);
 		CHECK_APPROX_EQUAL(sphere.GetAngularVelocity(), Vec3::sZero(), 2.0e-4f);
 
 		// Simulate a step
@@ -967,12 +968,12 @@ TEST_SUITE("PhysicsTests")
 
 		// Box should have moved unimpeded
 		CHECK_APPROX_EQUAL(box.GetPosition(), cInitialPosBox + cVelocity * c.GetDeltaTime());
-		CHECK_APPROX_EQUAL(box.GetLinearVelocity(), cVelocity); 
+		CHECK_APPROX_EQUAL(box.GetLinearVelocity(), cVelocity);
 		CHECK_APPROX_EQUAL(box.GetAngularVelocity(), Vec3::sZero());
 
 		// Sphere should have moved unimpeded
 		CHECK_APPROX_EQUAL(sphere.GetPosition(), cInitialPosSphere + cVelocity * c.GetDeltaTime());
-		CHECK_APPROX_EQUAL(sphere.GetLinearVelocity(), cVelocity); 
+		CHECK_APPROX_EQUAL(sphere.GetLinearVelocity(), cVelocity);
 		CHECK_APPROX_EQUAL(sphere.GetAngularVelocity(), Vec3::sZero());
 
 		// Simulate a step
@@ -1088,7 +1089,7 @@ TEST_SUITE("PhysicsTests")
 
 		// Step the world
 		ioContext.SimulateSingleStep();
-		
+
 		// Other bodies should now be awake and each body should only collide with its neighbour
 		CHECK(activation_listener.GetEntryCount() == cNumBodies - 1);
 		CHECK(contact_listener.GetEntryCount() == 2 * (cNumBodies - 1));
@@ -1321,7 +1322,7 @@ TEST_SUITE("PhysicsTests")
 			// Check that the bodies in the different systems updated correctly
 			CHECK_APPROX_EQUAL(lock1.GetBody().GetPosition(), cBox1Position + cBox1Velocity * cTime, 1.0e-5f);
 			CHECK_APPROX_EQUAL(lock2.GetBody().GetPosition(), cBox2Position + cBox2Velocity * cTime, 1.0e-5f);
-		}		
+		}
 	}
 
 	TEST_CASE("TestOutOfBodies")
@@ -1477,4 +1478,191 @@ TEST_SUITE("PhysicsTests")
 			CHECK_APPROX_EQUAL(box.GetRotation(), expected_rotation);
 		}
 	}
+
+	TEST_CASE("TestSelectiveStateSaveAndRestore")
+	{
+		class MyFilter : public StateRecorderFilter
+		{
+		public:
+			bool						ShouldSaveBody(const BodyID &inBodyID) const
+			{
+				return find(mIgnoreBodies.cbegin(), mIgnoreBodies.cend(), inBodyID) == mIgnoreBodies.cend();
+			}
+
+			virtual bool				ShouldSaveBody(const Body &inBody) const override
+			{
+				return ShouldSaveBody(inBody.GetID());
+			}
+
+			virtual bool				ShouldSaveContact(const BodyID &inBody1, const BodyID &inBody2) const override
+			{
+				return ShouldSaveBody(inBody1) && ShouldSaveBody(inBody2);
+			}
+
+			Array<BodyID>				mIgnoreBodies;
+		};
+
+		for (int mode = 0; mode < 2; mode++)
+		{
+			PhysicsTestContext c;
+
+			Vec3 gravity = c.GetSystem()->GetGravity();
+			Vec3 upside_down_gravity = -gravity;
+
+  			// Create the ground.
+			Body &ground = c.CreateFloor();
+
+			// Create two sets of bodies that each overlap
+			Body &box1 = c.CreateBox(RVec3(0, 1, 0), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(1.0f), EActivation::Activate);
+			Body &sphere1 = c.CreateSphere(RVec3(0, 1, 0.1f), 1.0f, EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, EActivation::Activate);
+
+			Body &box2 = c.CreateBox(RVec3(5, 1, 0), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(1.0f), EActivation::Activate);
+			Body &sphere2 = c.CreateSphere(RVec3(5, 1, 0.1f), 1.0f, EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, EActivation::Activate);
+
+			// Store the absolute initial state, that will be used for the final test.
+			StateRecorderImpl absolute_initial_state;
+			c.GetSystem()->SaveState(absolute_initial_state);
+
+			EStateRecorderState state_to_save = EStateRecorderState::All;
+			MyFilter filter;
+			if (mode == 1)
+			{
+				// Don't save the global state
+				state_to_save = EStateRecorderState(uint(EStateRecorderState::All) ^ uint(EStateRecorderState::Global));
+
+				// Don't save some bodies
+				filter.mIgnoreBodies.push_back(ground.GetID());
+				filter.mIgnoreBodies.push_back(box2.GetID());
+				filter.mIgnoreBodies.push_back(sphere2.GetID());
+			}
+
+			// Store the initial transform.
+			const RMat44 initial_box1_transform = box1.GetWorldTransform();
+			const RMat44 initial_sphere1_transform = sphere1.GetWorldTransform();
+			const RMat44 initial_box2_transform = box2.GetWorldTransform();
+			const RMat44 initial_sphere2_transform = sphere2.GetWorldTransform();
+
+			// Save partial state
+			StateRecorderImpl initial_state;
+			c.GetSystem()->SaveState(initial_state, state_to_save, &filter);
+
+			// Simulate for 2 seconds
+			c.Simulate(2.0f);
+
+			// The bodies should have moved and come to rest
+			const RMat44 intermediate_box1_transform = box1.GetWorldTransform();
+			const RMat44 intermediate_sphere1_transform = sphere1.GetWorldTransform();
+			const RMat44 intermediate_box2_transform = box2.GetWorldTransform();
+			const RMat44 intermediate_sphere2_transform = sphere2.GetWorldTransform();
+			CHECK(intermediate_box1_transform != initial_box1_transform);
+			CHECK(intermediate_sphere1_transform != initial_sphere1_transform);
+			CHECK(intermediate_box2_transform != initial_box2_transform);
+			CHECK(intermediate_sphere2_transform != initial_sphere2_transform);
+			CHECK(!box1.IsActive());
+			CHECK(!sphere1.IsActive());
+			CHECK(!box2.IsActive());
+			CHECK(!sphere2.IsActive());
+
+			// Save the intermediate state.
+			StateRecorderImpl intermediate_state;
+			c.GetSystem()->SaveState(intermediate_state, state_to_save, &filter);
+
+			// Change the gravity.
+			c.GetSystem()->SetGravity(upside_down_gravity);
+
+			// Restore the initial state.
+			c.GetSystem()->RestoreState(initial_state);
+
+			// Make sure the state is properly set back to the initial state.
+			CHECK(box1.GetWorldTransform() == initial_box1_transform);
+			CHECK(sphere1.GetWorldTransform() == initial_sphere1_transform);
+			CHECK(box1.IsActive());
+			CHECK(sphere1.IsActive());
+			if (mode == 0)
+			{
+				// Make sure the gravity is restored.
+				CHECK(c.GetSystem()->GetGravity() == gravity);
+
+				// The second set of bodies should have been restored as well
+				CHECK(box2.GetWorldTransform() == initial_box2_transform);
+				CHECK(sphere2.GetWorldTransform() == initial_sphere2_transform);
+				CHECK(box2.IsActive());
+				CHECK(sphere2.IsActive());
+			}
+			else
+			{
+				// Make sure the gravity is NOT restored.
+				CHECK(c.GetSystem()->GetGravity() == upside_down_gravity);
+				c.GetSystem()->SetGravity(gravity);
+
+				// The second set of bodies should NOT have been restored
+				CHECK(box2.GetWorldTransform() == intermediate_box2_transform);
+				CHECK(sphere2.GetWorldTransform() == intermediate_sphere2_transform);
+				CHECK(!box2.IsActive());
+				CHECK(!sphere2.IsActive());
+
+				// Apply a velocity to the second set of bodies to make sure they are active again
+				c.GetBodyInterface().SetLinearVelocity(box2.GetID(), Vec3(0, 0, 0.1f));
+				c.GetBodyInterface().SetLinearVelocity(sphere2.GetID(), Vec3(0, 0, 0.1f));
+			}
+
+			// Simulate for 2 seconds - again
+			c.Simulate(2.0f);
+
+			// The first set of bodies have been saved and should have returned to the same positions again
+			CHECK(box1.GetWorldTransform() == intermediate_box1_transform);
+			CHECK(sphere1.GetWorldTransform() == intermediate_sphere1_transform);
+			CHECK(!box1.IsActive());
+			CHECK(!sphere1.IsActive());
+			if (mode == 0)
+			{
+				// The second set of bodies have been saved and should have returned to the same positions again
+				CHECK(box2.GetWorldTransform() == intermediate_box2_transform);
+				CHECK(sphere2.GetWorldTransform() == intermediate_sphere2_transform);
+				CHECK(!box2.IsActive());
+				CHECK(!sphere2.IsActive());
+			}
+			else
+			{
+				// The second set of bodies have not been saved and should have moved on
+				CHECK(box2.GetWorldTransform() != intermediate_box2_transform);
+				CHECK(sphere2.GetWorldTransform() != intermediate_sphere2_transform);
+				CHECK(!box2.IsActive());
+				CHECK(sphere2.IsActive()); // The sphere keeps rolling
+			}
+
+			// Save the final state
+			StateRecorderImpl final_state;
+			c.GetSystem()->SaveState(final_state, state_to_save, &filter);
+
+			// Compare the states to make sure they are the same
+			CHECK(final_state.IsEqual(intermediate_state));
+
+			// Now restore the absolute initial state and make sure all the
+			// bodies are being active and ready to be processed again
+			c.GetSystem()->RestoreState(absolute_initial_state);
+
+			CHECK(box1.GetWorldTransform() == initial_box1_transform);
+			CHECK(sphere1.GetWorldTransform() == initial_sphere1_transform);
+			CHECK(box2.GetWorldTransform() == initial_box2_transform);
+			CHECK(sphere2.GetWorldTransform() == initial_sphere2_transform);
+			CHECK(box1.IsActive());
+			CHECK(sphere1.IsActive());
+			CHECK(box2.IsActive());
+			CHECK(sphere2.IsActive());
+
+			// Simulate for 2 seconds - again
+			c.Simulate(2.0f);
+
+			// We should have reached the same state as before
+			CHECK(box1.GetWorldTransform() == intermediate_box1_transform);
+			CHECK(sphere1.GetWorldTransform() == intermediate_sphere1_transform);
+			CHECK(box2.GetWorldTransform() == intermediate_box2_transform);
+			CHECK(sphere2.GetWorldTransform() == intermediate_sphere2_transform);
+			CHECK(!box1.IsActive());
+			CHECK(!sphere1.IsActive());
+			CHECK(!box2.IsActive());
+			CHECK(!sphere2.IsActive());
+		}
+	}
 }