Added WereBodiesInContact function to detect if we're removing the last contact in ContactListener::OnContactRemoved (#451)

Jolt/Physics/Collision/ContactListener.h

@@ -100,6 +100,7 @@ public:
 	/// Body 1 and 2 will be sorted such that body 1 ID < body 2 ID, so body 1 may not be dynamic.
 	/// The sub shape ID were created in the previous simulation step too, so if the structure of a shape changes (e.g. by adding/removing a child shape of a compound shape),
 	/// the sub shape ID may not be valid / may not point to the same sub shape anymore.
+	/// If you want to know if this is the last contact between the two bodies, use PhysicsSystem::WereBodiesInContact.
 	virtual void			OnContactRemoved(const SubShapeIDPair &inSubShapePair) { /* Do nothing */ }
 };
 

Jolt/Physics/Constraints/ContactConstraintManager.cpp

@@ -358,6 +358,8 @@ void ContactConstraintManager::ManifoldCache::GetAllCCDManifoldsSorted(Array<con
 
 void ContactConstraintManager::ManifoldCache::ContactPointRemovedCallbacks(ContactListener *inListener)
 {
+	JPH_PROFILE_FUNCTION();
+
 	for (MKeyValue &kv : mCachedManifolds)
 		if ((kv.GetValue().mFlags & uint16(CachedManifold::EFlags::ContactPersisted)) == 0)
 			inListener->OnContactRemoved(kv.GetKey());
@@ -1300,7 +1302,7 @@ void ContactConstraintManager::SortContacts(uint32 *inConstraintIdxBegin, uint32
 	});
 }
 
-void ContactConstraintManager::FinalizeContactCache(uint inExpectedNumBodyPairs, uint inExpectedNumManifolds)
+void ContactConstraintManager::FinalizeContactCacheAndCallContactPointRemovedCallbacks(uint inExpectedNumBodyPairs, uint inExpectedNumManifolds)
 {
 	JPH_PROFILE_FUNCTION();
 
@@ -1317,23 +1319,32 @@ void ContactConstraintManager::FinalizeContactCache(uint inExpectedNumBodyPairs,
 	// Buffers are now complete, make write buffer the read buffer
 	mCacheWriteIdx ^= 1;
 
-	// Use the amount of contacts from the last iteration to determine the amount of buckets to use in the hash map for the next iteration
-	mCache[mCacheWriteIdx].Prepare(inExpectedNumBodyPairs, inExpectedNumManifolds);
-}
+	// Get the old read cache / new write cache
+	ManifoldCache &old_read_cache = mCache[mCacheWriteIdx];
 
-void ContactConstraintManager::ContactPointRemovedCallbacks()
-{
-	JPH_PROFILE_FUNCTION();
+	// Call the contact point removal callbacks
+	if (mContactListener != nullptr)
+		old_read_cache.ContactPointRemovedCallbacks(mContactListener);
 
-	// Get the read cache
-	ManifoldCache &read_cache = mCache[mCacheWriteIdx ^ 1];
+	// We're done with the old read cache now
+	old_read_cache.Clear();
 
-	// Call the actual callbacks
-	if (mContactListener != nullptr)
-		read_cache.ContactPointRemovedCallbacks(mContactListener);
+	// Use the amount of contacts from the last iteration to determine the amount of buckets to use in the hash map for the next iteration
+	old_read_cache.Prepare(inExpectedNumBodyPairs, inExpectedNumManifolds);
+}
 
-	// We're done with the cache now
-	read_cache.Clear();
+bool ContactConstraintManager::WereBodiesInContact(const BodyID &inBody1ID, const BodyID &inBody2ID) const
+{
+	// The body pair needs to be in the cache and it needs to have a manifold (otherwise it's just a record indicating that there are no collisions)
+	const ManifoldCache &read_cache = mCache[mCacheWriteIdx ^ 1];
+	BodyPair key;
+	if (inBody1ID < inBody2ID)
+		key = BodyPair(inBody1ID, inBody2ID);
+	else
+		key = BodyPair(inBody2ID, inBody1ID);
+	uint64 key_hash = key.GetHash();
+	const BPKeyValue *kv = read_cache.Find(key, key_hash);
+	return kv != nullptr && kv->GetValue().mFirstCachedManifold != ManifoldMap::cInvalidHandle;
 }
 
 void ContactConstraintManager::SetupVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inDeltaTime)

Jolt/Physics/Constraints/ContactConstraintManager.h

@@ -141,12 +141,14 @@ public:
 	bool						AddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPair, Body &inBody1, Body &inBody2, const ContactManifold &inManifold);
 
 	/// Finalizes the contact cache, the contact cache that was generated during the calls to AddContactConstraint in this update
-	/// will be used from now on to read from.
-	/// inExpectedNumBodyPairs / inExpectedNumManifolds are the amount of body pairs / manifolds found in the previous step and is used to determine the amount of buckets the contact cache hash map will use.
-	void						FinalizeContactCache(uint inExpectedNumBodyPairs, uint inExpectedNumManifolds);
-
-	/// Notifies the listener of any contact points that were removed. Needs to be callsed after FinalizeContactCache().
-	void						ContactPointRemovedCallbacks();
+	/// will be used from now on to read from. After finalizing the contact cache, the contact removed callbacks will be called.
+	/// inExpectedNumBodyPairs / inExpectedNumManifolds are the amount of body pairs / manifolds found in the previous step and is
+	/// used to determine the amount of buckets the contact cache hash map will use in the next update.
+	void						FinalizeContactCacheAndCallContactPointRemovedCallbacks(uint inExpectedNumBodyPairs, uint inExpectedNumManifolds);
+
+	/// Check if 2 bodies were in contact during the last simulation step. Since contacts are only detected between active bodies, at least one of the bodies must be active.
+	/// Uses the read collision cache to determine if 2 bodies are in contact.
+	bool						WereBodiesInContact(const BodyID &inBody1ID, const BodyID &inBody2ID) const;
 
 	/// Get the number of contact constraints that were found
 	uint32						GetNumConstraints() const											{ return min<uint32>(mNumConstraints, mMaxConstraints); }

Jolt/Physics/PhysicsSystem.cpp

@@ -137,8 +137,7 @@ void PhysicsSystem::Update(float inDeltaTime, int inCollisionSteps, int inIntegr
 		mBroadPhase->UnlockModifications();
 
 		// Call contact removal callbacks from contacts that existed in the previous update
-		mContactManager.ContactPointRemovedCallbacks();
-		mContactManager.FinalizeContactCache(0, 0);
+		mContactManager.FinalizeContactCacheAndCallContactPointRemovedCallbacks(0, 0);
 
 		mBodyManager.UnlockAllBodies();
 		return;
@@ -2056,11 +2055,9 @@ void PhysicsSystem::JobContactRemovedCallbacks(const PhysicsUpdateContext::Step
 	// Reset the Body::EFlags::InvalidateContactCache flag for all bodies
 	mBodyManager.ValidateContactCacheForAllBodies();
 
-	// Trigger all contact removed callbacks by looking at last step contact points that have not been flagged as reused
-	mContactManager.ContactPointRemovedCallbacks();
-
 	// Finalize the contact cache (this swaps the read and write versions of the contact cache)
-	mContactManager.FinalizeContactCache(ioStep->mNumBodyPairs, ioStep->mNumManifolds);
+	// Trigger all contact removed callbacks by looking at last step contact points that have not been flagged as reused
+	mContactManager.FinalizeContactCacheAndCallContactPointRemovedCallbacks(ioStep->mNumBodyPairs, ioStep->mNumManifolds);
 }
 
 void PhysicsSystem::JobSolvePositionConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::SubStep *ioSubStep)

Jolt/Physics/PhysicsSystem.h

@@ -166,6 +166,13 @@ public:
 	/// @param outBodyIDs On return, this will contain the list of BodyIDs
 	void						GetActiveBodies(BodyIDVector &outBodyIDs) const				{ return mBodyManager.GetActiveBodies(outBodyIDs); }
 
+	/// Check if 2 bodies were in contact during the last simulation step. Since contacts are only detected between active bodies, so at least one of the bodies must be active in order for this function to work.
+	/// It queries the state at the time of the last PhysicsSystem::Update and will return true if the bodies were in contact, even if one of the bodies was moved / removed afterwards.
+	/// This function can be called from any thread when the PhysicsSystem::Update is not running. During PhysicsSystem::Update this function is only valid during contact callbacks:
+	/// - During the ContactListener::OnContactAdded callback this function can be used to determine if a different contact pair between the bodies was active in the previous simulation step (function returns true) or if this is the first step that the bodies are touching (function returns false).
+	/// - During the ContactListener::OnContactRemoved callback this function can be used to determine if this is the last contact pair between the bodies (function returns false) or if there are other contacts still present (function returns true).
+	bool						WereBodiesInContact(const BodyID &inBody1ID, const BodyID &inBody2ID) const { return mContactManager.WereBodiesInContact(inBody1ID, inBody2ID); }
+
 #ifdef JPH_TRACK_BROADPHASE_STATS
 	/// Trace the accumulated broadphase stats to the TTY
 	void						ReportBroadphaseStats()										{ mBroadPhase->ReportStats(); }

UnitTests/Physics/ContactListenerTests.cpp

@@ -6,6 +6,8 @@
 #include "PhysicsTestContext.h"
 #include "Layers.h"
 #include "LoggingContactListener.h"
+#include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
 
 TEST_SUITE("ContactListenerTests")
 {
@@ -262,4 +264,104 @@ TEST_SUITE("ContactListenerTests")
 			CHECK(remove.mBody2 == body.GetID()); // Highest ID second
 		}		
 	}
+
+	TEST_CASE("TestWereBodiesInContact")
+	{
+		for (int sign = -1; sign <= 1; sign += 2)
+		{
+			PhysicsTestContext c(1.0f / 60.0f, 1, 1);
+
+			PhysicsSystem *s = c.GetSystem();
+			BodyInterface &bi = c.GetBodyInterface();
+
+			Body &floor = c.CreateFloor();
+
+			// Two spheres at a distance so that when one sphere leaves the floor the body can still be touching the floor with the other sphere
+			Ref<StaticCompoundShapeSettings> compound_shape = new StaticCompoundShapeSettings;
+			compound_shape->AddShape(Vec3(-2, 0, 0), Quat::sIdentity(), new SphereShape(1));
+			compound_shape->AddShape(Vec3(2, 0, 0), Quat::sIdentity(), new SphereShape(1));
+			Body &body = *bi.CreateBody(BodyCreationSettings(compound_shape, RVec3(0, 0.999f, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+			bi.AddBody(body.GetID(), EActivation::Activate);
+
+			class ContactListenerImpl : public ContactListener
+			{
+			public:
+								ContactListenerImpl(PhysicsSystem *inSystem) : mSystem(inSystem) { }
+
+				virtual void	OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+				{
+					++mAdded;
+				}
+
+				virtual void	OnContactRemoved(const SubShapeIDPair &inSubShapePair) override
+				{
+					++mRemoved;
+					mWasInContact = mSystem->WereBodiesInContact(inSubShapePair.GetBody1ID(), inSubShapePair.GetBody2ID());
+					CHECK(mWasInContact == mSystem->WereBodiesInContact(inSubShapePair.GetBody2ID(), inSubShapePair.GetBody1ID())); // Returned value should be the same regardless of order
+				}
+
+				int				GetAddCount() const
+				{
+					return mAdded - mRemoved;
+				}
+
+				void			Reset()
+				{
+					mAdded = 0;
+					mRemoved = 0;
+					mWasInContact = false;
+				}
+
+				PhysicsSystem *	mSystem;
+
+				int				mAdded = 0;
+
+				int				mRemoved = 0;
+				bool			mWasInContact = false;
+			};
+
+			// Set listener
+			ContactListenerImpl listener(s);
+			s->SetContactListener(&listener);
+
+			// If the simulation hasn't run yet, we can't be in contact
+			CHECK(!s->WereBodiesInContact(floor.GetID(), body.GetID()));
+
+			// Step the simulation to allow detecting the contact
+			c.SimulateSingleStep();
+
+			// Should be in contact now
+			CHECK(s->WereBodiesInContact(floor.GetID(), body.GetID()));
+			CHECK(s->WereBodiesInContact(body.GetID(), floor.GetID()));
+			CHECK(listener.GetAddCount() == 1);
+			listener.Reset();
+
+			// Impulse on one side
+			bi.AddImpulse(body.GetID(), Vec3(0, 10000, 0), RVec3(Real(-sign * 2), 0, 0));
+			c.SimulateSingleStep(); // One step to detach from the ground (but starts penetrating so will not send a remove callback)
+			CHECK(listener.GetAddCount() == 0);
+			c.SimulateSingleStep(); // One step to get contact remove callback
+
+			// Should still be in contact
+			// Note that we may get a remove and an add callback because manifold reduction has combined the collision with both spheres into 1 contact manifold.
+			// At that point it has to select one of the sub shapes for the contact and if that sub shape no longer collides we get a remove for this sub shape and then an add callback for the other sub shape.
+			CHECK(s->WereBodiesInContact(floor.GetID(), body.GetID()));
+			CHECK(s->WereBodiesInContact(body.GetID(), floor.GetID()));
+			CHECK(listener.GetAddCount() == 0); 
+			CHECK((listener.mRemoved == 0 || listener.mWasInContact));
+			listener.Reset();
+
+			// Impulse on the other side
+			bi.AddImpulse(body.GetID(), Vec3(0, 10000, 0), RVec3(Real(sign * 2), 0, 0));
+			c.SimulateSingleStep(); // One step to detach from the ground (but starts penetrating so will not send a remove callback)
+			CHECK(listener.GetAddCount() == 0);
+			c.SimulateSingleStep(); // One step to get contact remove callback
+
+			// Should no longer be in contact
+			CHECK(!s->WereBodiesInContact(floor.GetID(), body.GetID()));
+			CHECK(!s->WereBodiesInContact(body.GetID(), floor.GetID()));
+			CHECK(listener.GetAddCount() == -1);
+			CHECK((listener.mRemoved == 1 && !listener.mWasInContact));
+		}
+	}
 }