Added error code return value to PhysicsSystem::Update (#504)

This allows checking if the simulation ran out of memory and a number of collisions have been ignored.
Also added unit test for out of bodies and out of constraint scenarios.

Jolt/Jolt.cmake

@@ -342,6 +342,7 @@ set(JOLT_PHYSICS_SRC_FILES
 	${JOLT_PHYSICS_ROOT}/Physics/DeterminismLog.cpp
 	${JOLT_PHYSICS_ROOT}/Physics/DeterminismLog.h
 	${JOLT_PHYSICS_ROOT}/Physics/EActivation.h
+	${JOLT_PHYSICS_ROOT}/Physics/EPhysicsUpdateError.h
 	${JOLT_PHYSICS_ROOT}/Physics/IslandBuilder.cpp
 	${JOLT_PHYSICS_ROOT}/Physics/IslandBuilder.h
 	${JOLT_PHYSICS_ROOT}/Physics/LargeIslandSplitter.cpp

Jolt/Physics/Constraints/ContactConstraintManager.cpp

@@ -263,7 +263,7 @@ ContactConstraintManager::MKeyValue *ContactConstraintManager::ManifoldCache::Cr
 	MKeyValue *kv = mCachedManifolds.Create(ioContactAllocator, inKey, inKeyHash, CachedManifold::sGetRequiredExtraSize(inNumContactPoints));
 	if (kv == nullptr)
 	{
-		JPH_ASSERT(false, "Out of cache space for manifold cache");
+		ioContactAllocator.mErrors |= EPhysicsUpdateError::ManifoldCacheFull;
 		return nullptr;
 	}
 	kv->GetValue().mNumContactPoints = uint16(inNumContactPoints);
@@ -304,7 +304,7 @@ ContactConstraintManager::BPKeyValue *ContactConstraintManager::ManifoldCache::C
 	BPKeyValue *kv = mCachedBodyPairs.Create(ioContactAllocator, inKey, inKeyHash, 0);
 	if (kv == nullptr)
 	{
-		JPH_ASSERT(false, "Out of cache space for body pair cache");
+		ioContactAllocator.mErrors |= EPhysicsUpdateError::BodyPairCacheFull;
 		return nullptr;
 	}
 	++ioContactAllocator.mNumBodyPairs;
@@ -620,6 +620,16 @@ void ContactConstraintManager::PrepareConstraintBuffer(PhysicsUpdateContext *inC
 	mConstraints = (ContactConstraint *)inContext->mTempAllocator->Allocate(mMaxConstraints * sizeof(ContactConstraint));
 }
 
+void ContactConstraintManager::sFinalizeContactAllocator(PhysicsUpdateContext::Step &ioStep, const ContactAllocator &inAllocator)
+{
+	// Atomically accumulate the number of found manifolds and body pairs
+	ioStep.mNumBodyPairs += inAllocator.mNumBodyPairs;
+	ioStep.mNumManifolds += inAllocator.mNumManifolds;
+
+	// Combine update errors
+	ioStep.mContext->mErrors.fetch_or((uint32)inAllocator.mErrors, memory_order_relaxed);
+}
+
 template <EMotionType Type1, EMotionType Type2>
 JPH_INLINE void ContactConstraintManager::TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, float inDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2, Mat44Arg inInvI1, Mat44Arg inInvI2)
 {
@@ -826,7 +836,7 @@ void ContactConstraintManager::GetContactsFromCache(ContactAllocator &ioContactA
 			uint32 constraint_idx = mNumConstraints++;
 			if (constraint_idx >= mMaxConstraints)
 			{
-				JPH_ASSERT(false, "Out of contact constraints!");
+				ioContactAllocator.mErrors |= EPhysicsUpdateError::ContactConstraintsFull;
 				break;
 			}
 			
@@ -1016,7 +1026,7 @@ bool ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &i
 		uint32 constraint_idx = mNumConstraints++;
 		if (constraint_idx >= mMaxConstraints)
 		{
-			JPH_ASSERT(false, "Out of contact constraints!");
+			ioContactAllocator.mErrors |= EPhysicsUpdateError::ContactConstraintsFull;
 
 			// 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.

Jolt/Physics/Constraints/ContactConstraintManager.h

@@ -6,6 +6,8 @@
 
 #include <Jolt/Core/StaticArray.h>
 #include <Jolt/Core/LockFreeHashMap.h>
+#include <Jolt/Physics/EPhysicsUpdateError.h>
+#include <Jolt/Physics/PhysicsUpdateContext.h>
 #include <Jolt/Physics/Body/BodyPair.h>
 #include <Jolt/Physics/Collision/Shape/SubShapeIDPair.h>
 #include <Jolt/Physics/Collision/ManifoldBetweenTwoFaces.h>
@@ -80,11 +82,15 @@ public:
 
 		uint					mNumBodyPairs = 0;													///< Total number of body pairs added using this allocator
 		uint					mNumManifolds = 0;													///< Total number of manifolds added using this allocator
+		EPhysicsUpdateError		mErrors = EPhysicsUpdateError::None;								///< Errors reported on this allocator
 	};
 
 	/// Get a new allocator context for storing contacts. Note that you should call this once and then add multiple contacts using the context.
 	ContactAllocator			GetContactAllocator()												{ return mCache[mCacheWriteIdx].GetContactAllocator(); }
 
+	/// Collect information from the contact allocator and accumulate it in the step.
+	static void					sFinalizeContactAllocator(PhysicsUpdateContext::Step &ioStep, const ContactAllocator &inAllocator);
+
 	/// Check if the contact points from the previous frame are reusable and if so copy them.
 	/// When the cache was usable and the pair has been handled: outPairHandled = true.
 	/// When a contact constraint was produced: outConstraintCreated = true.

Jolt/Physics/EPhysicsUpdateError.h

@@ -0,0 +1,37 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Enum used by PhysicsSystem to report error conditions during the PhysicsSystem::Update call. This is a bit field, multiple errors can trigger in the same update.
+enum class EPhysicsUpdateError : uint32
+{
+	None					= 0,			///< No errors
+	ManifoldCacheFull		= 1 << 0,		///< The manifold cache is full, this means that the total number of contacts between bodies is too high. Some contacts were ignored. Increase inMaxContactConstraints in PhysicsSystem::Init.
+	BodyPairCacheFull		= 1 << 1,		///< The body pair cache is full, this means that too many bodies contacted. Some contacts were ignored. Increase inMaxBodyPairs in PhysicsSystem::Init.
+	ContactConstraintsFull	= 1 << 2,		///< The contact constraints buffer is full. Some contacts were ignored. Increase inMaxContactConstraints in PhysicsSystem::Init.
+};
+
+/// OR operator for EPhysicsUpdateError
+inline EPhysicsUpdateError operator | (EPhysicsUpdateError inA, EPhysicsUpdateError inB)
+{
+	return static_cast<EPhysicsUpdateError>(static_cast<uint32>(inA) | static_cast<uint32>(inB));
+}
+
+/// OR operator for EPhysicsUpdateError
+inline EPhysicsUpdateError operator |= (EPhysicsUpdateError &ioA, EPhysicsUpdateError inB)
+{
+	ioA = ioA | inB;
+	return ioA;
+}
+
+/// AND operator for EPhysicsUpdateError
+inline EPhysicsUpdateError operator & (EPhysicsUpdateError inA, EPhysicsUpdateError inB)
+{
+	return static_cast<EPhysicsUpdateError>(static_cast<uint32>(inA) & static_cast<uint32>(inB));
+}
+
+JPH_NAMESPACE_END

Jolt/Physics/PhysicsSystem.cpp

@@ -115,7 +115,7 @@ void PhysicsSystem::RemoveStepListener(PhysicsStepListener *inListener)
 	mStepListeners.pop_back();
 }
 
-void PhysicsSystem::Update(float inDeltaTime, int inCollisionSteps, int inIntegrationSubSteps, TempAllocator *inTempAllocator, JobSystem *inJobSystem)
+EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionSteps, int inIntegrationSubSteps, TempAllocator *inTempAllocator, JobSystem *inJobSystem)
 {	
 	JPH_PROFILE_FUNCTION();
 
@@ -141,7 +141,7 @@ void PhysicsSystem::Update(float inDeltaTime, int inCollisionSteps, int inIntegr
 		mContactManager.FinalizeContactCacheAndCallContactPointRemovedCallbacks(0, 0);
 
 		mBodyManager.UnlockAllBodies();
-		return;
+		return EPhysicsUpdateError::None;
 	}
 
 	// Calculate ratio between current and previous frame delta time to scale initial constraint forces
@@ -623,6 +623,11 @@ void PhysicsSystem::Update(float inDeltaTime, int inCollisionSteps, int inIntegr
 
 	// Unlock step listeners
 	mStepListenersMutex.unlock();
+
+	// Return any errors
+	EPhysicsUpdateError errors = static_cast<EPhysicsUpdateError>(context.mErrors.load(memory_order_acquire));
+	JPH_ASSERT(errors == EPhysicsUpdateError::None, "An error occured during the physics update, see EPhysicsUpdateError for more information");
+	return errors;
 }
 
 void PhysicsSystem::JobStepListeners(PhysicsUpdateContext::Step *ioStep)
@@ -908,9 +913,8 @@ void PhysicsSystem::JobFindCollisions(PhysicsUpdateContext::Step *ioStep, int in
 					// If we're back at the first queue, we've looked at all of them and found nothing
 					if (read_queue_idx == first_read_queue_idx)
 					{
-						// Atomically accumulate the number of found manifolds and body pairs
-						ioStep->mNumBodyPairs += contact_allocator.mNumBodyPairs;
-						ioStep->mNumManifolds += contact_allocator.mNumManifolds;
+						// Collect information from the contact allocator and accumulate it in the step.
+						ContactConstraintManager::sFinalizeContactAllocator(*ioStep, contact_allocator);
 
 						// Mark this job as inactive
 						ioStep->mActiveFindCollisionJobs.fetch_and(~PhysicsUpdateContext::JobMask(1 << inJobIndex));
@@ -1914,9 +1918,8 @@ void PhysicsSystem::JobFindCCDContacts(const PhysicsUpdateContext *ioContext, Ph
 		}
 	}
 
-	// Atomically accumulate the number of found manifolds and body pairs
-	ioSubStep->mStep->mNumBodyPairs += contact_allocator.mNumBodyPairs;
-	ioSubStep->mStep->mNumManifolds += contact_allocator.mNumManifolds;
+	// Collect information from the contact allocator and accumulate it in the step.
+	ContactConstraintManager::sFinalizeContactAllocator(*ioSubStep->mStep, contact_allocator);
 }
 
 void PhysicsSystem::JobResolveCCDContacts(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::SubStep *ioSubStep)

Jolt/Physics/PhysicsSystem.h

@@ -103,7 +103,7 @@ public:
 	/// Simulate the system.
 	/// The world steps for a total of inDeltaTime seconds. This is divided in inCollisionSteps iterations. Each iteration
 	/// consists of collision detection followed by inIntegrationSubSteps integration steps.
-	void						Update(float inDeltaTime, int inCollisionSteps, int inIntegrationSubSteps, TempAllocator *inTempAllocator, JobSystem *inJobSystem);
+	EPhysicsUpdateError			Update(float inDeltaTime, int inCollisionSteps, int inIntegrationSubSteps, TempAllocator *inTempAllocator, JobSystem *inJobSystem);
 
 	/// Saving state for replay
 	void						SaveState(StateRecorder &inStream) const;

Jolt/Physics/PhysicsUpdateContext.h

@@ -161,6 +161,7 @@ public:
 	float					mSubStepDeltaTime;										///< Delta time for a simulation sub step (integration step)
 	float					mWarmStartImpulseRatio;									///< Ratio of this step delta time vs last step
 	bool					mUseLargeIslandSplitter;								///< If true, use large island splitting
+	atomic<uint32>			mErrors { 0 };											///< Errors that occurred during the update, actual type is EPhysicsUpdateError
 
 	Constraint **			mActiveConstraints = nullptr;							///< Constraints that were active at the start of the physics update step (activating bodies can activate constraints and we need a consistent snapshot). Only these constraints will be resolved.
 

UnitTests/Physics/PhysicsTests.cpp

@@ -8,6 +8,7 @@
 #include "LoggingBodyActivationListener.h"
 #include "LoggingContactListener.h"
 #include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
 #include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
 #include <Jolt/Physics/Body/BodyLockMulti.h>
 #include <Jolt/Physics/Constraints/PointConstraint.h>
@@ -1329,4 +1330,56 @@ TEST_SUITE("PhysicsTests")
 			CHECK_APPROX_EQUAL(lock2.GetBody().GetPosition(), cBox2Position + cBox2Velocity * cTime, 1.0e-5f);
 		}		
 	}
+
+	TEST_CASE("TestOutOfBodies")
+	{
+		// Create a context with space for a single body
+		PhysicsTestContext c(1.0f / 60.0f, 1, 1, 0, 1);
+
+		BodyInterface& bi = c.GetBodyInterface();
+
+		// First body
+		Body *b1 = bi.CreateBody(BodyCreationSettings(new SphereShape(1.0f), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING));
+		CHECK(b1 != nullptr);
+
+		// Second body should fail
+		Body *b2 = bi.CreateBody(BodyCreationSettings(new SphereShape(1.0f), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING));
+		CHECK(b2 == nullptr);
+
+		// Free first body
+		bi.DestroyBody(b1->GetID());
+
+		// Second body creation should succeed
+		b2 = bi.CreateBody(BodyCreationSettings(new SphereShape(1.0f), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING));
+		CHECK(b2 != nullptr);
+
+		// Clean up
+		bi.DestroyBody(b2->GetID());
+	}
+
+	TEST_CASE("TestOutOfContactConstraints")
+	{
+		// Create a context with space for 8 constraints
+		PhysicsTestContext c(1.0f / 60.0f, 1, 1, 0, 1024, 4096, 8);
+
+		c.CreateFloor();
+
+		// The first 8 boxes should be fine
+		for (int i = 0; i < 8; ++i)
+			c.CreateBox(RVec3(3.0_r * i, 0.9_r, 0), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(1.0f), EActivation::Activate);
+
+		// Step
+		EPhysicsUpdateError errors = c.SimulateSingleStep();
+		CHECK(errors == EPhysicsUpdateError::None);
+
+		// Adding one more box should introduce an error
+		c.CreateBox(RVec3(24.0_r, 0.9_r, 0), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(1.0f), EActivation::Activate);
+
+		// Step
+		{
+			JPH_IF_ENABLE_ASSERTS(ExpectAssert expect_assert(1);)
+			errors = c.SimulateSingleStep();
+		}
+		CHECK((errors & EPhysicsUpdateError::ContactConstraintsFull) != EPhysicsUpdateError::None);
+	}
 }

UnitTests/PhysicsTestContext.cpp

@@ -10,7 +10,7 @@
 #include <Jolt/Core/JobSystemThreadPool.h>
 #include <Jolt/Core/TempAllocator.h>
 
-PhysicsTestContext::PhysicsTestContext(float inDeltaTime, int inCollisionSteps, int inIntegrationSubSteps, int inWorkerThreads) :
+PhysicsTestContext::PhysicsTestContext(float inDeltaTime, int inCollisionSteps, int inIntegrationSubSteps, int inWorkerThreads, uint inMaxBodies, uint inMaxBodyPairs, uint inMaxContactConstraints) :
 #ifdef JPH_DISABLE_TEMP_ALLOCATOR
 	mTempAllocator(new TempAllocatorMalloc()),
 #else
@@ -23,7 +23,7 @@ PhysicsTestContext::PhysicsTestContext(float inDeltaTime, int inCollisionSteps,
 {
 	// Create physics system
 	mSystem = new PhysicsSystem();
-	mSystem->Init(1024, 0, 4096, 1024, mBroadPhaseLayerInterface, mObjectVsBroadPhaseLayerFilter, mObjectVsObjectLayerFilter);
+	mSystem->Init(inMaxBodies, 0, inMaxBodyPairs, inMaxContactConstraints, mBroadPhaseLayerInterface, mObjectVsBroadPhaseLayerFilter, mObjectVsObjectLayerFilter);
 }
 
 PhysicsTestContext::~PhysicsTestContext()
@@ -78,25 +78,30 @@ Body &PhysicsTestContext::CreateSphere(RVec3Arg inPosition, float inRadius, EMot
 	return CreateBody(new SphereShapeSettings(inRadius), inPosition, Quat::sIdentity(), inMotionType, inMotionQuality, inLayer, inActivation);
 }
 
-void PhysicsTestContext::Simulate(float inTotalTime, function<void()> inPreStepCallback)
+EPhysicsUpdateError PhysicsTestContext::Simulate(float inTotalTime, function<void()> inPreStepCallback)
 {
+	EPhysicsUpdateError errors = EPhysicsUpdateError::None;
+
 	const int cNumSteps = int(round(inTotalTime / mDeltaTime));
 	for (int s = 0; s < cNumSteps; ++s)
 	{
 		inPreStepCallback();
-		mSystem->Update(mDeltaTime, mCollisionSteps, mIntegrationSubSteps, mTempAllocator, mJobSystem);
+		errors |= mSystem->Update(mDeltaTime, mCollisionSteps, mIntegrationSubSteps, mTempAllocator, mJobSystem);
 	#ifndef JPH_DISABLE_TEMP_ALLOCATOR
 		JPH_ASSERT(static_cast<TempAllocatorImpl *>(mTempAllocator)->IsEmpty());
 	#endif // JPH_DISABLE_TEMP_ALLOCATOR
 	}
+
+	return errors;
 }
 
-void PhysicsTestContext::SimulateSingleStep()
+EPhysicsUpdateError PhysicsTestContext::SimulateSingleStep()
 {
-	mSystem->Update(mDeltaTime, mCollisionSteps, mIntegrationSubSteps, mTempAllocator, mJobSystem);
+	EPhysicsUpdateError errors = mSystem->Update(mDeltaTime, mCollisionSteps, mIntegrationSubSteps, mTempAllocator, mJobSystem);
 #ifndef JPH_DISABLE_TEMP_ALLOCATOR
 	JPH_ASSERT(static_cast<TempAllocatorImpl *>(mTempAllocator)->IsEmpty());
 #endif // JPH_DISABLE_TEMP_ALLOCATOR
+	return errors;
 }
 
 RVec3 PhysicsTestContext::PredictPosition(RVec3Arg inPosition, Vec3Arg inVelocity, Vec3Arg inAcceleration, float inTotalTime) const

UnitTests/PhysicsTestContext.h

@@ -19,7 +19,7 @@ class PhysicsTestContext
 {
 public:
 	// Constructor / destructor
-						PhysicsTestContext(float inDeltaTime = 1.0f / 60.0f, int inCollisionSteps = 1, int inIntegrationSubSteps = 1, int inWorkerThreads = 0);
+						PhysicsTestContext(float inDeltaTime = 1.0f / 60.0f, int inCollisionSteps = 1, int inIntegrationSubSteps = 1, int inWorkerThreads = 0, uint inMaxBodies = 1024, uint inMaxBodyPairs = 4096, uint inMaxContactConstraints = 1024);
 						~PhysicsTestContext();
 
 	// Set the gravity to zero
@@ -47,10 +47,10 @@ public:
 	}
 
 	// Simulate only for one delta time step
-	void				SimulateSingleStep();
+	EPhysicsUpdateError	SimulateSingleStep();
 
 	// Simulate the world for inTotalTime time
-	void				Simulate(float inTotalTime, function<void()> inPreStepCallback = []() { });
+	EPhysicsUpdateError	Simulate(float inTotalTime, function<void()> inPreStepCallback = []() { });
 
 	// Predict position assuming ballistic motion using initial position, velocity acceleration and time
 	RVec3				PredictPosition(RVec3Arg inPosition, Vec3Arg inVelocity, Vec3Arg inAcceleration, float inTotalTime) const;

UnitTests/UnitTestFramework.h

@@ -3,6 +3,7 @@
 // SPDX-License-Identifier: MIT
 
 #include <Jolt/Jolt.h>
+#include <Jolt/Core/Atomics.h>
 #include <Jolt/Math/DVec3.h>
 #include <Jolt/Math/Float2.h>
 
@@ -70,3 +71,40 @@ inline void CHECK_APPROX_EQUAL(const Float2 &inLHS, const Float2 &inRHS, float i
 
 // Define the exact random number generator we want to use across platforms for consistency (default_random_engine's implementation is platform specific)
 using UnitTestRandom = mt19937;
+
+#ifdef JPH_ENABLE_ASSERTS
+
+// Stack based object that tests for an assert
+class ExpectAssert
+{
+public:
+	/// Expect inCount asserts
+	explicit					ExpectAssert(int inCount)
+	{
+		CHECK(sCount == 0);
+		sCount = inCount;
+
+		mPrevAssertFailed = AssertFailed;
+		AssertFailed = [](const char*, const char*, const char*, uint)
+		{
+			--sCount;
+			return false;
+		};
+	}
+
+	/// Verifies that the expected number of asserts were triggered
+								~ExpectAssert()
+	{
+		AssertFailed = mPrevAssertFailed;
+		CHECK(sCount == 0);
+	}
+
+private:
+	// Keeps track of number of asserts that are expected
+	inline static atomic<int>	sCount { 0 };
+
+	// Previous assert function
+	AssertFailedFunction		mPrevAssertFailed;
+};
+
+#endif // JPH_ENABLE_ASSERTS