Ability to save / restore the state of a single body (#664)

Jolt/Physics/Body/BodyManager.cpp

@@ -863,6 +863,33 @@ bool BodyManager::RestoreState(StateRecorder &inStream)
 	return true;
 }
 
+void BodyManager::SaveBodyState(const Body &inBody, StateRecorder &inStream) const
+{
+	inStream.Write(inBody.IsActive());
+
+	inBody.SaveState(inStream);
+}
+
+void BodyManager::RestoreBodyState(Body &ioBody, StateRecorder &inStream)
+{
+	bool is_active = ioBody.IsActive();
+	inStream.Read(is_active);
+
+	ioBody.RestoreState(inStream);
+
+	if (is_active != ioBody.IsActive())
+	{
+		UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList));
+
+		JPH_ASSERT(!mActiveBodiesLocked || sOverrideAllowActivation);
+
+		if (is_active)
+			AddBodyToActiveBodies(ioBody);
+		else
+			RemoveBodyFromActiveBodies(ioBody);
+	}
+}
+
 #ifdef JPH_DEBUG_RENDERER
 void BodyManager::Draw(const DrawSettings &inDrawSettings, const PhysicsSettings &inPhysicsSettings, DebugRenderer *inRenderer, const BodyDrawFilter *inBodyFilter)
 {

Jolt/Physics/Body/BodyManager.h

@@ -197,6 +197,12 @@ public:
 	/// Restoring state for replay. Returns false if failed.
 	bool							RestoreState(StateRecorder &inStream);
 
+	/// Save the state of a single body for replay
+	void							SaveBodyState(const Body &inBody, StateRecorder &inStream) const;
+
+	/// Save the state of a single body for replay
+	void							RestoreBodyState(Body &inBody, StateRecorder &inStream);
+
 	enum class EShapeColor
 	{
 		InstanceColor,				///< Random color per instance

Jolt/Physics/PhysicsSystem.cpp

@@ -2461,4 +2461,17 @@ bool PhysicsSystem::RestoreState(StateRecorder &inStream)
 	return true;
 }
 
+void PhysicsSystem::SaveBodyState(const Body &inBody, StateRecorder &inStream) const
+{
+	mBodyManager.SaveBodyState(inBody, inStream);
+}
+
+void PhysicsSystem::RestoreBodyState(Body &ioBody, StateRecorder &inStream)
+{
+	mBodyManager.RestoreBodyState(ioBody, inStream);
+
+	BodyID id = ioBody.GetID();
+	mBroadPhase->NotifyBodiesAABBChanged(&id, 1);
+}
+
 JPH_NAMESPACE_END

Jolt/Physics/PhysicsSystem.h

@@ -114,6 +114,12 @@ public:
 	/// Restoring state for replay. Returns false if failed.
 	bool						RestoreState(StateRecorder &inStream);
 
+	/// Saving state of a single body.
+	void						SaveBodyState(const Body &inBody, StateRecorder &inStream) const;
+
+	/// Restoring state of a single body.
+	void						RestoreBodyState(Body &ioBody, StateRecorder &inStream);
+
 #ifdef JPH_DEBUG_RENDERER
 	// Drawing properties
 	static bool					sDrawMotionQualityLinearCast;								///< Draw debug info for objects that perform continuous collision detection through the linear cast motion quality
@@ -128,7 +134,7 @@ public:
 	void						DrawConstraintLimits(DebugRenderer *inRenderer)				{ mConstraintManager.DrawConstraintLimits(inRenderer); }
 
 	/// Draw the constraint reference frames only (debugging purposes)
-	void						DrawConstraintReferenceFrame(DebugRenderer *inRenderer)	{ mConstraintManager.DrawConstraintReferenceFrame(inRenderer); }
+	void						DrawConstraintReferenceFrame(DebugRenderer *inRenderer)		{ mConstraintManager.DrawConstraintReferenceFrame(inRenderer); }
 #endif // JPH_DEBUG_RENDERER
 
 	/// Set gravity value

UnitTests/Physics/PhysicsTests.cpp

@@ -1651,6 +1651,10 @@ TEST_SUITE("PhysicsTests")
 			CHECK(box2.IsActive());
 			CHECK(sphere2.IsActive());
 
+			// Save the state of a single body
+			StateRecorderImpl single_body;
+			c.GetSystem()->SaveBodyState(box2, single_body);
+
 			// Simulate for 2 seconds - again
 			c.Simulate(2.0f);
 
@@ -1663,6 +1667,19 @@ TEST_SUITE("PhysicsTests")
 			CHECK(!sphere1.IsActive());
 			CHECK(!box2.IsActive());
 			CHECK(!sphere2.IsActive());
+
+			// Restore the single body
+			c.GetSystem()->RestoreBodyState(box2, single_body);
+
+			// Only that body should have been restored
+			CHECK(box1.GetWorldTransform() == intermediate_box1_transform);
+			CHECK(sphere1.GetWorldTransform() == intermediate_sphere1_transform);
+			CHECK(box2.GetWorldTransform() == initial_box2_transform);
+			CHECK(sphere2.GetWorldTransform() == intermediate_sphere2_transform);
+			CHECK(!box1.IsActive());
+			CHECK(!sphere1.IsActive());
+			CHECK(box2.IsActive());
+			CHECK(!sphere2.IsActive());
 		}
 	}
 }