Allow soft bodies to go to sleep (#658)

Docs/Architecture.md

@@ -301,7 +301,7 @@ Soft bodies try to implement as much as possible of the normal Body interface, b
 
 Soft bodies are currently in development, please note the following:
 
-* Soft bodies currently can't sleep, are executed on a single CPU core and collision checks are not as efficient as they could be.
+* Soft bodies are currently executed on a single CPU core and collision checks are not as efficient as they could be.
 * Soft bodies can only collide with rigid bodies, collisions between soft bodies are not implemented yet.
 * ContactListener callbacks are not triggered for soft bodies.
 * AddForce/AddTorque/SetLinearVelocity/SetLinearVelocityClamped/SetAngularVelocity/SetAngularVelocityClamped/AddImpulse/AddAngularImpulse have no effect on soft bodies as the velocity is stored per particle rather than per body.

Jolt/Physics/Body/Body.cpp

@@ -99,7 +99,7 @@ void Body::CalculateWorldSpaceBoundsInternal()
 	mBounds = mShape->GetWorldSpaceBounds(GetCenterOfMassTransform(), Vec3::sReplicate(1.0f));
 }
 
-void Body::SetPositionAndRotationInternal(RVec3Arg inPosition, QuatArg inRotation)
+void Body::SetPositionAndRotationInternal(RVec3Arg inPosition, QuatArg inRotation, bool inResetSleepTestSpheres)
 {
 	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::ReadWrite));
 
@@ -110,7 +110,7 @@ void Body::SetPositionAndRotationInternal(RVec3Arg inPosition, QuatArg inRotatio
 	CalculateWorldSpaceBoundsInternal();
 
 	// Reset sleeping test
-	if (mMotionProperties != nullptr)
+	if (inResetSleepTestSpheres && mMotionProperties != nullptr)
 		ResetSleepTestSpheres();
 }
 
@@ -142,7 +142,7 @@ void Body::SetShapeInternal(const Shape *inShape, bool inUpdateMassProperties)
 	CalculateWorldSpaceBoundsInternal();
 }
 
-Body::ECanSleep Body::UpdateSleepStateInternal(float inDeltaTime, float inMaxMovement, float inTimeBeforeSleep)
+ECanSleep Body::UpdateSleepStateInternal(float inDeltaTime, float inMaxMovement, float inTimeBeforeSleep)
 {
 	// Check override & sensors will never go to sleep (they would stop detecting collisions with sleeping bodies)
 	if (!mMotionProperties->mAllowSleeping || IsSensor())
@@ -180,8 +180,7 @@ Body::ECanSleep Body::UpdateSleepStateInternal(float inDeltaTime, float inMaxMov
 		}
 	}
 
-	mMotionProperties->mSleepTestTimer += inDeltaTime;
-	return mMotionProperties->mSleepTestTimer >= inTimeBeforeSleep? ECanSleep::CanSleep : ECanSleep::CannotSleep;
+	return mMotionProperties->AccumulateSleepTime(inDeltaTime, inTimeBeforeSleep);
 }
 
 bool Body::ApplyBuoyancyImpulse(RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime)
@@ -370,6 +369,7 @@ SoftBodyCreationSettings Body::GetSoftBodyCreationSettings() const
 	const SoftBodyMotionProperties *mp = static_cast<const SoftBodyMotionProperties *>(mMotionProperties);
 	result.mNumIterations = mp->GetNumIterations();
 	result.mLinearDamping = mp->GetLinearDamping();
+	result.mMaxLinearVelocity = mp->GetMaxLinearVelocity();
 	result.mGravityFactor = mp->GetGravityFactor();
 	result.mPressure = mp->GetPressure();
 	result.mUpdatePosition = mp->GetUpdatePosition();

Jolt/Physics/Body/Body.h

@@ -279,7 +279,7 @@ public:
 	void					CalculateWorldSpaceBoundsInternal();
 
 	/// Function to update body's position (should only be called by the BodyInterface since it also requires updating the broadphase)
-	void					SetPositionAndRotationInternal(RVec3Arg inPosition, QuatArg inRotation);
+	void					SetPositionAndRotationInternal(RVec3Arg inPosition, QuatArg inRotation, bool inResetSleepTestSpheres = true);
 
 	/// Updates the center of mass and optionally mass propertes after shifting the center of mass or changes to the shape (should only be called by the BodyInterface since it also requires updating the broadphase)
 	/// @param inPreviousCenterOfMass Center of mass of the shape before the alterations
@@ -294,12 +294,6 @@ public:
 	/// Access to the index in the BodyManager::mActiveBodies list
 	uint32					GetIndexInActiveBodiesInternal() const							{ return mMotionProperties != nullptr? mMotionProperties->mIndexInActiveBodies : cInactiveIndex; }
 
-	enum class ECanSleep
-	{
-		CannotSleep = 0,																	///< Object cannot go to sleep
-		CanSleep = 1,																		///< Object can go to sleep
-	};
-
 	/// Update eligibility for sleeping
 	ECanSleep				UpdateSleepStateInternal(float inDeltaTime, float inMaxMovement, float inTimeBeforeSleep);
 

Jolt/Physics/Body/Body.inl

@@ -41,10 +41,8 @@ inline static bool sIsValidSensorBodyPair(const Body &inSensor, const Body &inOt
 
 inline bool Body::sFindCollidingPairsCanCollide(const Body &inBody1, const Body &inBody2)
 {
-	// Soft bodies collide later in the pipeline
+	// First body should never be a soft body
 	JPH_ASSERT(!inBody1.IsSoftBody());
-	if (inBody2.IsSoftBody())
-		return false;
 
 	// One of these conditions must be true
 	// - One of the bodies must be dynamic to collide
@@ -78,7 +76,7 @@ inline bool Body::sFindCollidingPairsCanCollide(const Body &inBody1, const Body
 	//     bodies during the Broad/NarrowPhase step), so to collide A.Index < B.Index.
 	// (5) As tie breaker we can use the same condition A.Index < B.Index to collide, this means that if A, B collides then B, A won't
 	static_assert(Body::cInactiveIndex == 0xffffffff, "The algorithm below uses this value");
-	if (body1_index_in_active_bodies >= inBody2.GetIndexInActiveBodiesInternal())
+	if (!inBody2.IsSoftBody() && body1_index_in_active_bodies >= inBody2.GetIndexInActiveBodiesInternal())
 		return false;
 	JPH_ASSERT(inBody1.GetID() != inBody2.GetID(), "Read the comment above, A and B are the same body which should not be possible!");
 

Jolt/Physics/Body/BodyManager.cpp

@@ -250,7 +250,7 @@ Body *BodyManager::AllocateSoftBody(const SoftBodyCreationSettings &inSoftBodyCr
 	body->mCollisionGroup = inSoftBodyCreationSettings.mCollisionGroup;
 	mp->SetLinearDamping(inSoftBodyCreationSettings.mLinearDamping);
 	mp->SetAngularDamping(0);
-	mp->SetMaxLinearVelocity(FLT_MAX);
+	mp->SetMaxLinearVelocity(inSoftBodyCreationSettings.mMaxLinearVelocity);
 	mp->SetMaxAngularVelocity(FLT_MAX);
 	mp->SetLinearVelocity(Vec3::sZero());
 	mp->SetAngularVelocity(Vec3::sZero());

Jolt/Physics/Body/MotionProperties.h

@@ -17,6 +17,13 @@ JPH_NAMESPACE_BEGIN
 
 class StateRecorder;
 
+/// Enum that determines if an object can go to sleep
+enum class ECanSleep
+{
+	CannotSleep = 0,																		///< Object cannot go to sleep
+	CanSleep = 1,																			///< Object can go to sleep
+};
+
 /// The Body class only keeps track of state for static bodies, the MotionProperties class keeps the additional state needed for a moving Body. It has a 1-on-1 relationship with the body.
 class JPH_EXPORT MotionProperties
 {
@@ -167,6 +174,12 @@ public:
 	/// Reset spheres to center around inPoints with radius 0
 	inline void				ResetSleepTestSpheres(const RVec3 *inPoints);
 
+	/// Reset the sleep test timer without resetting the sleep test spheres
+	inline void				ResetSleepTestTimer()											{ mSleepTestTimer = 0.0f; }
+
+	/// Accumulate sleep time and return if a body can go to sleep
+	inline ECanSleep		AccumulateSleepTime(float inDeltaTime, float inTimeBeforeSleep);
+
 	/// Saving state for replay
 	void					SaveState(StateRecorder &inStream) const;
 

Jolt/Physics/Body/MotionProperties.inl

@@ -115,4 +115,10 @@ void MotionProperties::ResetSleepTestSpheres(const RVec3 *inPoints)
 	mSleepTestTimer = 0.0f;
 }
 
+ECanSleep MotionProperties::AccumulateSleepTime(float inDeltaTime, float inTimeBeforeSleep)
+{
+	mSleepTestTimer += inDeltaTime;
+	return mSleepTestTimer >= inTimeBeforeSleep? ECanSleep::CanSleep : ECanSleep::CannotSleep;
+}
+
 JPH_NAMESPACE_END

Jolt/Physics/PhysicsSystem.cpp

@@ -938,6 +938,17 @@ void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const
 
 	JPH_DET_LOG("ProcessBodyPair: id1: " << inBodyPair.mBodyA << " id2: " << inBodyPair.mBodyB << " p1: " << body1->GetCenterOfMassPosition() << " p2: " << body2->GetCenterOfMassPosition() << " r1: " << body1->GetRotation() << " r2: " << body2->GetRotation());
 
+	// Check for soft bodies
+	if (body2->IsSoftBody())
+	{
+		// If the 2nd body is a soft body and not active, we activate it now
+		if (!body2->IsActive())
+			mBodyManager.ActivateBodies(&inBodyPair.mBodyB, 1);
+
+		// Soft body processing is done later in the pipeline
+		return;
+	}
+
 	// Ensure that body1 is dynamic, this ensures that we do the collision detection in the space of a moving body, which avoids accuracy problems when testing a very large static object against a small dynamic object
 	// Ensure that body1 id < body2 id for dynamic vs dynamic
 	// Keep body order unchanged when colliding with a sensor
@@ -2163,8 +2174,8 @@ void PhysicsSystem::CheckSleepAndUpdateBounds(uint32 inIslandIndex, const Physic
 	{
 		JPH_PROFILE("Check Sleeping");
 
-		static_assert(int(Body::ECanSleep::CannotSleep) == 0 && int(Body::ECanSleep::CanSleep) == 1, "Loop below makes this assumption");
-		int all_can_sleep = mPhysicsSettings.mAllowSleeping? int(Body::ECanSleep::CanSleep) : int(Body::ECanSleep::CannotSleep);
+		static_assert(int(ECanSleep::CannotSleep) == 0 && int(ECanSleep::CanSleep) == 1, "Loop below makes this assumption");
+		int all_can_sleep = mPhysicsSettings.mAllowSleeping? int(ECanSleep::CanSleep) : int(ECanSleep::CannotSleep);
 
 		float time_before_sleep = mPhysicsSettings.mTimeBeforeSleep;
 		float max_movement = mPhysicsSettings.mPointVelocitySleepThreshold * time_before_sleep;
@@ -2186,7 +2197,7 @@ void PhysicsSystem::CheckSleepAndUpdateBounds(uint32 inIslandIndex, const Physic
 		}
 
 		// If all bodies indicate they can sleep we can deactivate them
-		if (all_can_sleep == int(Body::ECanSleep::CanSleep))
+		if (all_can_sleep == int(ECanSleep::CanSleep))
 			ioBodiesToSleep.PutToSleep(bodies_begin, bodies_end);
 	}
 	else
@@ -2335,38 +2346,55 @@ void PhysicsSystem::JobUpdateSoftBodies(const PhysicsUpdateContext *ioContext)
 
 #ifdef JPH_ENABLE_ASSERTS
 	// Can activate bodies only
-	BodyManager::GrantActiveBodiesAccess grant_active(true, false);
+	BodyManager::GrantActiveBodiesAccess grant_active(true, true);
 #endif
 
 	static constexpr int cBodiesBatch = 64;
 	BodyID *bodies_to_update_bounds = (BodyID *)JPH_STACK_ALLOC(cBodiesBatch * sizeof(BodyID));
 	int num_bodies_to_update_bounds = 0;
+	BodyID *bodies_to_put_to_sleep = (BodyID *)JPH_STACK_ALLOC(cBodiesBatch * sizeof(BodyID));
+	int num_bodies_to_put_to_sleep = 0;
 
 	// Loop through active bodies
-	const BodyID *active_bodies = mBodyManager.GetActiveBodiesUnsafe(EBodyType::SoftBody);
-	const BodyID *active_bodies_end = active_bodies + mBodyManager.GetNumActiveBodies(EBodyType::SoftBody);
-	for (const BodyID *b = active_bodies; b < active_bodies_end; ++b)
+	BodyIDVector active_bodies;
+	mBodyManager.GetActiveBodies(EBodyType::SoftBody, active_bodies);
+	for (BodyID b : active_bodies)
 	{
-		Body &body = mBodyManager.GetBody(*b);
+		Body &body = mBodyManager.GetBody(b);
 		SoftBodyMotionProperties *mp = static_cast<SoftBodyMotionProperties *>(body.GetMotionProperties());
 
 		// Update the soft body
 		Vec3 delta_position;
-		mp->Update(ioContext->mStepDeltaTime, body, delta_position, *this);
-		body.SetPositionAndRotationInternal(body.GetPosition() + delta_position, body.GetRotation());
+		ECanSleep can_sleep = mp->Update(ioContext->mStepDeltaTime, body, delta_position, *this);
+		body.SetPositionAndRotationInternal(body.GetPosition() + delta_position, body.GetRotation(), false);
 
-		bodies_to_update_bounds[num_bodies_to_update_bounds++] = *b;
+		bodies_to_update_bounds[num_bodies_to_update_bounds++] = b;
 		if (num_bodies_to_update_bounds == cBodiesBatch)
 		{
 			// Buffer full, flush now
 			mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false);
 			num_bodies_to_update_bounds = 0;
 		}
+
+		if (can_sleep == ECanSleep::CanSleep)
+		{
+			// This body should go to sleep
+			bodies_to_put_to_sleep[num_bodies_to_put_to_sleep++] = b;
+			if (num_bodies_to_put_to_sleep == cBodiesBatch)
+			{
+				mBodyManager.DeactivateBodies(bodies_to_put_to_sleep, num_bodies_to_put_to_sleep);
+				num_bodies_to_put_to_sleep = 0;
+			}
+		}
 	}
 
 	// Notify change bounds on requested bodies
 	if (num_bodies_to_update_bounds > 0)
 		mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false);
+
+	// Notify bodies to go to sleep
+	if (num_bodies_to_put_to_sleep > 0)
+		mBodyManager.DeactivateBodies(bodies_to_put_to_sleep, num_bodies_to_put_to_sleep);
 }
 
 void PhysicsSystem::SaveState(StateRecorder &inStream, EStateRecorderState inState, const StateRecorderFilter *inFilter) const

Jolt/Physics/SoftBody/SoftBodyCreationSettings.cpp

@@ -20,6 +20,8 @@ JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodyCreationSettings)
 	JPH_ADD_ENUM_ATTRIBUTE(SoftBodyCreationSettings, mObjectLayer)
 	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mCollisionGroup)
 	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mNumIterations)
+	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mLinearDamping)
+	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mMaxLinearVelocity)
 	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mRestitution)
 	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mFriction)
 	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mPressure)
@@ -36,6 +38,8 @@ void SoftBodyCreationSettings::SaveBinaryState(StreamOut &inStream) const
 	inStream.Write(mObjectLayer);
 	mCollisionGroup.SaveBinaryState(inStream);
 	inStream.Write(mNumIterations);
+	inStream.Write(mLinearDamping);
+	inStream.Write(mMaxLinearVelocity);
 	inStream.Write(mRestitution);
 	inStream.Write(mFriction);
 	inStream.Write(mPressure);
@@ -52,6 +56,8 @@ void SoftBodyCreationSettings::RestoreBinaryState(StreamIn &inStream)
 	inStream.Read(mObjectLayer);
 	mCollisionGroup.RestoreBinaryState(inStream);
 	inStream.Read(mNumIterations);
+	inStream.Read(mLinearDamping);
+	inStream.Read(mMaxLinearVelocity);
 	inStream.Read(mRestitution);
 	inStream.Read(mFriction);
 	inStream.Read(mPressure);

Jolt/Physics/SoftBody/SoftBodyCreationSettings.h

@@ -59,7 +59,8 @@ public:
 	CollisionGroup		mCollisionGroup;					///< The collision group this body belongs to (determines if two objects can collide)
 
 	uint32				mNumIterations = 5;					///< Number of solver iterations
-	float				mLinearDamping = 0.05f;				///< Linear damping: dv/dt = -mLinearDamping * v
+	float				mLinearDamping = 0.1f;				///< Linear damping: dv/dt = -mLinearDamping * v
+	float				mMaxLinearVelocity = 500.0f;		///< Maximum linear velocity that a vertex can reach (m/s)
 	float				mRestitution = 0.0f;				///< Restitution when colliding
 	float				mFriction = 0.2f;					///< Friction coefficient when colliding
 	float				mPressure = 0.0f;					///< n * R * T, amount of substance * ideal gass constant * absolute temperature, see https://en.wikipedia.org/wiki/Pressure

Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp

@@ -91,7 +91,7 @@ float SoftBodyMotionProperties::GetVolumeTimesSix() const
 	return six_volume;
 }
 
-void SoftBodyMotionProperties::Update(float inDeltaTime, Body &inSoftBody, Vec3 &outDeltaPosition, PhysicsSystem &inSystem)
+ECanSleep SoftBodyMotionProperties::Update(float inDeltaTime, Body &inSoftBody, Vec3 &outDeltaPosition, PhysicsSystem &inSystem)
 {
 	// Based on: XPBD, Extended Position Based Dynamics, Matthias Muller, Ten Minute Physics
 	// See: https://matthias-research.github.io/pages/tenMinutePhysics/09-xpbd.pdf
@@ -419,10 +419,20 @@ void SoftBodyMotionProperties::Update(float inDeltaTime, Body &inSoftBody, Vec3
 		}
 
 	// Loop through vertices once more to update the global state
+	float max_linear_velocity_sq = Square(GetMaxLinearVelocity());
+	float max_v_sq = 0.0f;
 	Vec3 linear_velocity = Vec3::sZero(), angular_velocity = Vec3::sZero();
 	mLocalPredictedBounds = mLocalBounds = { };
 	for (Vertex &v : mVertices)
 	{
+		// Calculate max square velocity
+		float v_sq = v.mVelocity.LengthSq();
+		max_v_sq = max(max_v_sq, v_sq);
+
+		// Clamp if velocity is too high
+		if (v_sq > max_linear_velocity_sq)
+			v.mVelocity *= sqrt(max_linear_velocity_sq / v_sq);
+
 		// Calculate local linear/angular velocity
 		linear_velocity += v.mVelocity;
 		angular_velocity += v.mPosition.Cross(v.mVelocity);
@@ -463,6 +473,16 @@ void SoftBodyMotionProperties::Update(float inDeltaTime, Body &inSoftBody, Vec3
 	for (const CollidingShape &cs : collector.mHits)
 		if (cs.mUpdateVelocities)
 			body_interface.SetLinearAndAngularVelocity(cs.mBodyID, body_transform.Multiply3x3(cs.mLinearVelocity), body_transform.Multiply3x3(cs.mAngularVelocity));
+
+	// Test if we should go to sleep
+	const PhysicsSettings &physics_settings = inSystem.GetPhysicsSettings();
+	if (max_v_sq > physics_settings.mPointVelocitySleepThreshold)
+	{
+		ResetSleepTestTimer();
+		return ECanSleep::CannotSleep;
+	}
+
+	return AccumulateSleepTime(inDeltaTime, physics_settings.mTimeBeforeSleep);
 }
 
 #ifdef JPH_DEBUG_RENDERER

Jolt/Physics/SoftBody/SoftBodyMotionProperties.h

@@ -31,7 +31,7 @@ public:
 	void								Initialize(const SoftBodyCreationSettings &inSettings);
 
 	/// Update the soft body
-	void								Update(float inDeltaTime, Body &inSoftBody, Vec3 &outDeltaPosition, PhysicsSystem &inSystem);
+	ECanSleep							Update(float inDeltaTime, Body &inSoftBody, Vec3 &outDeltaPosition, PhysicsSystem &inSystem);
 
 	/// Get the shared settings of the soft body
 	const SoftBodySharedSettings *		GetSettings() const							{ return mSettings; }