Improvements to activation logic (#1114)

- BodyInterface::AddForce/Torque functions now take an additional EActivation parameter that indicates if you want to forcefully activate the body. If you are just applying a custom gravity using AddForce, you want the body to be able to go to sleep, while if you're actively controlling a body through forces you want to be able to wake it up.
- BodyInterface::ActivateBody/Bodies will now reset the sleep timer of an already active body so that it cannot go to sleep the frame after activation.

See: godot-jolt/godot-jolt#866

Jolt/Physics/Body/BodyInterface.cpp

@@ -17,6 +17,16 @@
 
 JPH_NAMESPACE_BEGIN
 
+void BodyInterface::ActivateBodyInternal(Body &ioBody) const
+{
+	// Activate body or reset its sleep timer.
+	// Note that BodyManager::ActivateBodies also resets the sleep timer internally, but we avoid a mutex lock if the body is already active by calling ResetSleepTimer directly.
+	if (!ioBody.IsActive())
+		mBodyManager->ActivateBodies(&ioBody.GetID(), 1);
+	else
+		ioBody.ResetSleepTimer();
+}
+
 Body *BodyInterface::CreateBody(const BodyCreationSettings &inSettings)
 {
 	Body *body = mBodyManager->AllocateBody(inSettings);
@@ -205,10 +215,8 @@ void BodyInterface::ActivateBody(const BodyID &inBodyID)
 	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
 	if (lock.Succeeded())
 	{
-		const Body &body = lock.GetBody();
-
-		if (!body.IsActive())
-			mBodyManager->ActivateBodies(&inBodyID, 1);
+		Body &body = lock.GetBody();
+		ActivateBodyInternal(body);
 	}
 }
 
@@ -312,7 +320,7 @@ void BodyInterface::SetShape(const BodyID &inBodyID, const Shape *inShape, bool
 
 			// Optionally activate body
 			if (inActivationMode == EActivation::Activate && !body.IsStatic())
-				mBodyManager->ActivateBodies(&inBodyID, 1);
+				ActivateBodyInternal(body);
 		}
 	}
 }
@@ -342,7 +350,7 @@ void BodyInterface::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inPreviou
 
 		// Optionally activate body
 		if (inActivationMode == EActivation::Activate && !body.IsStatic())
-			mBodyManager->ActivateBodies(&inBodyID, 1);
+			ActivateBodyInternal(body);
 	}
 }
 
@@ -397,7 +405,7 @@ void BodyInterface::SetPositionAndRotation(const BodyID &inBodyID, RVec3Arg inPo
 
 		// Optionally activate body
 		if (inActivationMode == EActivation::Activate && !body.IsStatic())
-			mBodyManager->ActivateBodies(&inBodyID, 1);
+			ActivateBodyInternal(body);
 	}
 }
 
@@ -424,7 +432,7 @@ void BodyInterface::SetPositionAndRotationWhenChanged(const BodyID &inBodyID, RV
 
 			// Optionally activate body
 			if (inActivationMode == EActivation::Activate && !body.IsStatic())
-				mBodyManager->ActivateBodies(&inBodyID, 1);
+				ActivateBodyInternal(body);
 		}
 	}
 }
@@ -464,7 +472,7 @@ void BodyInterface::SetPosition(const BodyID &inBodyID, RVec3Arg inPosition, EAc
 
 		// Optionally activate body
 		if (inActivationMode == EActivation::Activate && !body.IsStatic())
-			mBodyManager->ActivateBodies(&inBodyID, 1);
+			ActivateBodyInternal(body);
 	}
 }
 
@@ -505,7 +513,7 @@ void BodyInterface::SetRotation(const BodyID &inBodyID, QuatArg inRotation, EAct
 
 		// Optionally activate body
 		if (inActivationMode == EActivation::Activate && !body.IsStatic())
-			mBodyManager->ActivateBodies(&inBodyID, 1);
+			ActivateBodyInternal(body);
 	}
 }
 
@@ -688,67 +696,67 @@ Vec3 BodyInterface::GetPointVelocity(const BodyID &inBodyID, RVec3Arg inPoint) c
 	return Vec3::sZero();
 }
 
-void BodyInterface::AddForce(const BodyID &inBodyID, Vec3Arg inForce)
+void BodyInterface::AddForce(const BodyID &inBodyID, Vec3Arg inForce, EActivation inActivationMode)
 {
 	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
 	if (lock.Succeeded())
 	{
 		Body &body = lock.GetBody();
-		if (body.IsDynamic())
+		if (body.IsDynamic() && (inActivationMode == EActivation::Activate || body.IsActive()))
 		{
 			body.AddForce(inForce);
 
-			if (!body.IsActive())
-				mBodyManager->ActivateBodies(&inBodyID, 1);
+			if (inActivationMode == EActivation::Activate)
+				ActivateBodyInternal(body);
 		}
 	}
 }
 
-void BodyInterface::AddForce(const BodyID &inBodyID, Vec3Arg inForce, RVec3Arg inPoint)
+void BodyInterface::AddForce(const BodyID &inBodyID, Vec3Arg inForce, RVec3Arg inPoint, EActivation inActivationMode)
 {
 	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
 	if (lock.Succeeded())
 	{
 		Body &body = lock.GetBody();
-		if (body.IsDynamic())
+		if (body.IsDynamic() && (inActivationMode == EActivation::Activate || body.IsActive()))
 		{
 			body.AddForce(inForce, inPoint);
 
-			if (!body.IsActive())
-				mBodyManager->ActivateBodies(&inBodyID, 1);
+			if (inActivationMode == EActivation::Activate)
+				ActivateBodyInternal(body);
 		}
 	}
 }
 
-void BodyInterface::AddTorque(const BodyID &inBodyID, Vec3Arg inTorque)
+void BodyInterface::AddTorque(const BodyID &inBodyID, Vec3Arg inTorque, EActivation inActivationMode)
 {
 	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
 	if (lock.Succeeded())
 	{
 		Body &body = lock.GetBody();
-		if (body.IsDynamic())
+		if (body.IsDynamic() && (inActivationMode == EActivation::Activate || body.IsActive()))
 		{
 			body.AddTorque(inTorque);
 
-			if (!body.IsActive())
-				mBodyManager->ActivateBodies(&inBodyID, 1);
+			if (inActivationMode == EActivation::Activate)
+				ActivateBodyInternal(body);
 		}
 	}
 }
 
-void BodyInterface::AddForceAndTorque(const BodyID &inBodyID, Vec3Arg inForce, Vec3Arg inTorque)
+void BodyInterface::AddForceAndTorque(const BodyID &inBodyID, Vec3Arg inForce, Vec3Arg inTorque, EActivation inActivationMode)
 {
 	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
 	if (lock.Succeeded())
 	{
 		Body &body = lock.GetBody();
-		if (body.IsDynamic())
+		if (body.IsDynamic() && (inActivationMode == EActivation::Activate || body.IsActive()))
 		{
 			body.AddForce(inForce);
 			body.AddTorque(inTorque);
 
-			if (!body.IsActive())
-				mBodyManager->ActivateBodies(&inBodyID, 1);
+			if (inActivationMode == EActivation::Activate)
+				ActivateBodyInternal(body);
 		}
 	}
 }
@@ -807,14 +815,11 @@ bool BodyInterface::ApplyBuoyancyImpulse(const BodyID &inBodyID, RVec3Arg inSurf
 	if (lock.Succeeded())
 	{
 		Body &body = lock.GetBody();
-		if (body.IsDynamic())
+		if (body.IsDynamic()
+			&& body.ApplyBuoyancyImpulse(inSurfacePosition, inSurfaceNormal, inBuoyancy, inLinearDrag, inAngularDrag, inFluidVelocity, inGravity, inDeltaTime))
 		{
-			bool ret = body.ApplyBuoyancyImpulse(inSurfacePosition, inSurfaceNormal, inBuoyancy, inLinearDrag, inAngularDrag, inFluidVelocity, inGravity, inDeltaTime);
-
-			if (ret && !body.IsActive())
-				mBodyManager->ActivateBodies(&inBodyID, 1);
-
-			return ret;
+			ActivateBodyInternal(body);
+			return true;
 		}
 	}
 
@@ -864,8 +869,8 @@ void BodyInterface::SetMotionType(const BodyID &inBodyID, EMotionType inMotionTy
 		body.SetMotionType(inMotionType);
 
 		// Activate body if requested
-		if (inMotionType != EMotionType::Static && inActivationMode == EActivation::Activate && !body.IsActive())
-			mBodyManager->ActivateBodies(&inBodyID, 1);
+		if (inMotionType != EMotionType::Static && inActivationMode == EActivation::Activate)
+			ActivateBodyInternal(body);
 	}
 }
 

Jolt/Physics/Body/BodyInterface.h

@@ -200,10 +200,10 @@ public:
 
 	///@name Add forces to the body
 	///@{
-	void						AddForce(const BodyID &inBodyID, Vec3Arg inForce); ///< See Body::AddForce
-	void						AddForce(const BodyID &inBodyID, Vec3Arg inForce, RVec3Arg inPoint); ///< Applied at inPoint
-	void						AddTorque(const BodyID &inBodyID, Vec3Arg inTorque); ///< See Body::AddTorque
-	void						AddForceAndTorque(const BodyID &inBodyID, Vec3Arg inForce, Vec3Arg inTorque); ///< A combination of Body::AddForce and Body::AddTorque
+	void						AddForce(const BodyID &inBodyID, Vec3Arg inForce, EActivation inActivationMode = EActivation::Activate); ///< See Body::AddForce
+	void						AddForce(const BodyID &inBodyID, Vec3Arg inForce, RVec3Arg inPoint, EActivation inActivationMode = EActivation::Activate); ///< Applied at inPoint
+	void						AddTorque(const BodyID &inBodyID, Vec3Arg inTorque, EActivation inActivationMode = EActivation::Activate); ///< See Body::AddTorque
+	void						AddForceAndTorque(const BodyID &inBodyID, Vec3Arg inForce, Vec3Arg inTorque, EActivation inActivationMode = EActivation::Activate); ///< A combination of Body::AddForce and Body::AddTorque
 	///@}
 
 	///@name Add an impulse to the body
@@ -272,6 +272,9 @@ public:
 	void						InvalidateContactCache(const BodyID &inBodyID);
 
 private:
+	/// Helper function to activate a single body
+	JPH_INLINE void				ActivateBodyInternal(Body &ioBody) const;
+
 	BodyLockInterface *			mBodyLockInterface = nullptr;
 	BodyManager *				mBodyManager = nullptr;
 	BroadPhase *				mBroadPhase = nullptr;

Jolt/Physics/Body/BodyManager.cpp

@@ -556,17 +556,20 @@ void BodyManager::ActivateBodies(const BodyID *inBodyIDs, int inNumber)
 			JPH_ASSERT(body.GetID() == body_id);
 			JPH_ASSERT(body.IsInBroadPhase());
 
-			if (!body.IsStatic()
-				&& body.mMotionProperties->mIndexInActiveBodies == Body::cInactiveIndex)
+			if (!body.IsStatic())
 			{
-				// Reset sleeping
+				// Reset sleeping timer so that we don't immediately go to sleep again
 				body.ResetSleepTimer();
 
-				AddBodyToActiveBodies(body);
+				// Check if we're sleeping
+				if (body.mMotionProperties->mIndexInActiveBodies == Body::cInactiveIndex)
+				{
+					AddBodyToActiveBodies(body);
 
-				// Call activation listener
-				if (mActivationListener != nullptr)
-					mActivationListener->OnBodyActivated(body_id, body.GetUserData());
+					// Call activation listener
+					if (mActivationListener != nullptr)
+						mActivationListener->OnBodyActivated(body_id, body.GetUserData());
+				}
 			}
 		}
 }