Ability in the OnContactPointAdded/Persisted callbacks to change a body into a sensor (#158)

ContactSettings adds a mIsSensor property that you can set to true whenever you want a body to report contacts but not to have a collision response.

Idea and initial implementation by @Joshua-Ashton.

Jolt/Physics/Collision/ContactListener.h

@@ -36,6 +36,7 @@ class ContactSettings
 public:
 	float					mCombinedFriction;					///< Combined friction for the body pair (usually calculated by sCombineFriction)
 	float					mCombinedRestitution;				///< Combined restitution for the body pair (usually calculated by sCombineRestitution)
+	bool					mIsSensor;							///< If the contact should be treated as a sensor vs body contact (no collision response)
 };
 
 /// Return value for the OnContactValidate callback. Determines if the contact is being processed or not.

Jolt/Physics/Constraints/ContactConstraintManager.cpp

@@ -622,7 +622,7 @@ JPH_INLINE void ContactConstraintManager::TemplatedCalculateFrictionAndNonPenetr
 	{
 		Vec3 p1 = inTransformBody1 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition1);
 		Vec3 p2 = inTransformBody2 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition2);
-		wcp.CalculateFrictionAndNonPenetrationConstraintProperties<Type1, Type2>(inDeltaTime, inBody1, inBody2, inInvI1, inInvI2, p1, p2, ioConstraint.mWorldSpaceNormal, t1, t2, ioConstraint.mSettings.mCombinedRestitution, ioConstraint.mSettings.mCombinedFriction, min_velocity_for_restitution);
+		wcp.CalculateFrictionAndNonPenetrationConstraintProperties<Type1, Type2>(inDeltaTime, inBody1, inBody2, inInvI1, inInvI2, p1, p2, ioConstraint.mWorldSpaceNormal, t1, t2, ioConstraint.mCombinedRestitution, ioConstraint.mCombinedFriction, min_velocity_for_restitution);
 	}
 }
 
@@ -671,12 +671,12 @@ inline void ContactConstraintManager::CalculateFrictionAndNonPenetrationConstrai
 	}
 }
 
-void ContactConstraintManager::GetContactsFromCache(ContactAllocator &ioContactAllocator, Body &inBody1, Body &inBody2, bool &outPairHandled, bool &outContactFound)
+void ContactConstraintManager::GetContactsFromCache(ContactAllocator &ioContactAllocator, Body &inBody1, Body &inBody2, bool &outPairHandled, bool &outConstraintCreated)
 {
 	JPH_PROFILE_FUNCTION();
 
 	// Start with nothing found and not handled
-	outContactFound = false;
+	outConstraintCreated = false;
 	outPairHandled = false;
 
 	// Swap bodies so that body 1 id < body 2 id
@@ -740,9 +740,6 @@ void ContactConstraintManager::GetContactsFromCache(ContactAllocator &ioContactA
 	if (input_cbp.mFirstCachedManifold == ManifoldMap::cInvalidHandle)
 		return;
 
-	// A contact is available, start creating constraints
-	outContactFound = true;
-
 	// Get body transforms
 	Mat44 transform_body1 = body1->GetCenterOfMassTransform();
 	Mat44 transform_body2 = body2->GetCenterOfMassTransform();
@@ -779,6 +776,7 @@ void ContactConstraintManager::GetContactsFromCache(ContactAllocator &ioContactA
 		ContactSettings settings;
 		settings.mCombinedFriction = mCombineFriction(*body1, input_key.GetSubShapeID1(), *body2, input_key.GetSubShapeID2());
 		settings.mCombinedRestitution = mCombineRestitution(*body1, input_key.GetSubShapeID1(), *body2, input_key.GetSubShapeID2());
+		settings.mIsSensor = body1->IsSensor() || body2->IsSensor();
 
 		// Calculate world space contact normal
 		Vec3 world_space_normal = transform_body2.Multiply3x3(Vec3::sLoadFloat3Unsafe(output_cm->mContactNormal)).Normalized();
@@ -808,7 +806,8 @@ void ContactConstraintManager::GetContactsFromCache(ContactAllocator &ioContactA
 		}
 
 		// If one of the bodies is a sensor, don't actually create the constraint
-		if (!body1->IsSensor() && !body2->IsSensor())
+		JPH_ASSERT(settings.mIsSensor || !(body1->IsSensor() || body2->IsSensor()), "Sensors cannot be converted into regular bodies by a contact callback!");
+		if (!settings.mIsSensor)
 		{
 			// Add contact constraint in world space for the solver
 			uint32 constraint_idx = mNumConstraints++;
@@ -818,13 +817,17 @@ void ContactConstraintManager::GetContactsFromCache(ContactAllocator &ioContactA
 				break;
 			}
 			
+			// A constraint will be created
+			outConstraintCreated = true;
+
 			ContactConstraint &constraint = mConstraints[constraint_idx];
 			new (&constraint) ContactConstraint();
 			constraint.mBody1 = body1;
 			constraint.mBody2 = body2;
 			constraint.mSortKey = input_hash;
 			constraint.mWorldSpaceNormal = world_space_normal;
-			constraint.mSettings = settings;
+			constraint.mCombinedFriction = settings.mCombinedFriction;
+			constraint.mCombinedRestitution = settings.mCombinedRestitution;
 			constraint.mContactPoints.resize(output_cm->mNumContactPoints);
 			for (uint32 i = 0; i < output_cm->mNumContactPoints; ++i)
 			{
@@ -902,7 +905,7 @@ ContactConstraintManager::BodyPairHandle ContactConstraintManager::AddBodyPair(C
 }
 
 template <EMotionType Type1, EMotionType Type2>
-void ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPairHandle, Body &inBody1, Body &inBody2, const ContactManifold &inManifold, Mat44Arg inInvI1, Mat44Arg inInvI2)
+bool ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPairHandle, Body &inBody1, Body &inBody2, const ContactManifold &inManifold, Mat44Arg inInvI1, Mat44Arg inInvI2)
 {
 	// Calculate hash
 	SubShapeIDPair key { inBody1.GetID(), inManifold.mSubShapeID1, inBody2.GetID(), inManifold.mSubShapeID2 };
@@ -919,7 +922,7 @@ void ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &i
 	ManifoldCache &write_cache = mCache[mCacheWriteIdx];
 	MKeyValue *new_manifold_kv = write_cache.Create(ioContactAllocator, key, key_hash, num_contact_points);
 	if (new_manifold_kv == nullptr)
-		return; // Out of cache space
+		return false; // Out of cache space
 	CachedManifold *new_manifold = &new_manifold_kv->GetValue();
 
 	// Transform the world space normal to the space of body 2 (this is usually the static body)
@@ -930,6 +933,7 @@ void ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &i
 	ContactSettings settings;
 	settings.mCombinedFriction = mCombineFriction(inBody1, inManifold.mSubShapeID1, inBody2, inManifold.mSubShapeID2);
 	settings.mCombinedRestitution = mCombineRestitution(inBody1, inManifold.mSubShapeID1, inBody2, inManifold.mSubShapeID2);
+	settings.mIsSensor = inBody1.IsSensor() || inBody2.IsSensor();
 
 	// Get the contact points for the old cache entry
 	const ManifoldCache &read_cache = mCache[mCacheWriteIdx ^ 1];
@@ -964,8 +968,11 @@ void ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &i
 	// Get inverse transform for body 1
 	Mat44 inverse_transform_body1 = inBody1.GetInverseCenterOfMassTransform();
 
+	bool contact_constraint_created;
+
 	// If one of the bodies is a sensor, don't actually create the constraint
-	if (inBody1.IsSensor() || inBody2.IsSensor())
+	JPH_ASSERT(settings.mIsSensor || !(inBody1.IsSensor() || inBody2.IsSensor()), "Sensors cannot be converted into regular bodies by a contact callback!");
+	if (settings.mIsSensor)
 	{
 		// Store the contact manifold in the cache
 		for (int i = 0; i < num_contact_points; ++i)
@@ -984,6 +991,9 @@ void ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &i
 			cp.mFrictionLambda[0] = 0.0f;
 			cp.mFrictionLambda[1] = 0.0f;
 		}
+
+		// No constraint created
+		contact_constraint_created = false;
 	}
 	else
 	{
@@ -996,8 +1006,11 @@ void ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &i
 			// 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.
 			new_manifold->mNumContactPoints = 0; 
-			return;
+			return false;
 		}
+
+		// We will create a contact constraint
+		contact_constraint_created = true;
 		
 		ContactConstraint &constraint = mConstraints[constraint_idx];
 		new (&constraint) ContactConstraint();
@@ -1005,7 +1018,8 @@ void ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &i
 		constraint.mBody1 = &inBody1;
 		constraint.mBody2 = &inBody2;
 		constraint.mSortKey = key_hash;
-		constraint.mSettings = settings;
+		constraint.mCombinedFriction = settings.mCombinedFriction;
+		constraint.mCombinedRestitution = settings.mCombinedRestitution;
 
 		// Notify island builder
 		mUpdateContext->mIslandBuilder->LinkContact(constraint_idx, inBody1.GetIndexInActiveBodiesInternal(), inBody2.GetIndexInActiveBodiesInternal());
@@ -1070,9 +1084,12 @@ void ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &i
 	CachedBodyPair *cbp = reinterpret_cast<CachedBodyPair *>(inBodyPairHandle);
 	new_manifold->mNextWithSameBodyPair = cbp->mFirstCachedManifold;
 	cbp->mFirstCachedManifold = write_cache.ToHandle(new_manifold_kv);
+
+	// A contact constraint was added
+	return contact_constraint_created;
 }
 
-void ContactConstraintManager::AddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPairHandle, Body &inBody1, Body &inBody2, const ContactManifold &inManifold)
+bool ContactConstraintManager::AddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPairHandle, Body &inBody1, Body &inBody2, const ContactManifold &inManifold)
 {
 	JPH_PROFILE_FUNCTION();
 
@@ -1106,16 +1123,13 @@ void ContactConstraintManager::AddContactConstraint(ContactAllocator &ioContactA
 			switch (body2->GetMotionType())
 			{
 			case EMotionType::Dynamic:
-				TemplatedAddContactConstraint<EMotionType::Dynamic, EMotionType::Dynamic>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold, invi1, body2->GetInverseInertia());
-				break;
+				return TemplatedAddContactConstraint<EMotionType::Dynamic, EMotionType::Dynamic>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold, invi1, body2->GetInverseInertia());
 
 			case EMotionType::Kinematic:
-				TemplatedAddContactConstraint<EMotionType::Dynamic, EMotionType::Kinematic>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold, invi1, Mat44() /* Will not be used */);
-				break;
+				return TemplatedAddContactConstraint<EMotionType::Dynamic, EMotionType::Kinematic>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold, invi1, Mat44() /* Will not be used */);
 
 			case EMotionType::Static:
-				TemplatedAddContactConstraint<EMotionType::Dynamic, EMotionType::Static>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold, invi1, Mat44() /* Will not be used */);
-				break;
+				return TemplatedAddContactConstraint<EMotionType::Dynamic, EMotionType::Static>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold, invi1, Mat44() /* Will not be used */);
 
 			default:
 				JPH_ASSERT(false);
@@ -1128,16 +1142,13 @@ void ContactConstraintManager::AddContactConstraint(ContactAllocator &ioContactA
 		switch (body2->GetMotionType())
 		{
 		case EMotionType::Dynamic:
-			TemplatedAddContactConstraint<EMotionType::Kinematic, EMotionType::Dynamic>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold, Mat44() /* Will not be used */, body2->GetInverseInertia());
-			break;
+			return TemplatedAddContactConstraint<EMotionType::Kinematic, EMotionType::Dynamic>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold, Mat44() /* Will not be used */, body2->GetInverseInertia());
 
 		case EMotionType::Kinematic:
-			TemplatedAddContactConstraint<EMotionType::Kinematic, EMotionType::Kinematic>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold, Mat44() /* Will not be used */, Mat44() /* Will not be used */);
-			break;
+			return TemplatedAddContactConstraint<EMotionType::Kinematic, EMotionType::Kinematic>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold, Mat44() /* Will not be used */, Mat44() /* Will not be used */);
 
 		case EMotionType::Static:
-			TemplatedAddContactConstraint<EMotionType::Kinematic, EMotionType::Static>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold, Mat44() /* Will not be used */, Mat44() /* Will not be used */);
-			break;
+			return TemplatedAddContactConstraint<EMotionType::Kinematic, EMotionType::Static>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold, Mat44() /* Will not be used */, Mat44() /* Will not be used */);
 
 		default:
 			JPH_ASSERT(false);
@@ -1149,12 +1160,10 @@ void ContactConstraintManager::AddContactConstraint(ContactAllocator &ioContactA
 		switch (body2->GetMotionType())
 		{
 		case EMotionType::Dynamic:
-			TemplatedAddContactConstraint<EMotionType::Static, EMotionType::Dynamic>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold, Mat44() /* Will not be used */, body2->GetInverseInertia());
-			break;
+			return TemplatedAddContactConstraint<EMotionType::Static, EMotionType::Dynamic>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold, Mat44() /* Will not be used */, body2->GetInverseInertia());
 
 		case EMotionType::Kinematic:
-			TemplatedAddContactConstraint<EMotionType::Static, EMotionType::Kinematic>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold, Mat44() /* Will not be used */, Mat44() /* Will not be used */);
-			break;
+			return TemplatedAddContactConstraint<EMotionType::Static, EMotionType::Kinematic>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold, Mat44() /* Will not be used */, Mat44() /* Will not be used */);
 
 		case EMotionType::Static: // Static vs static not possible
 		default: 
@@ -1167,6 +1176,8 @@ void ContactConstraintManager::AddContactConstraint(ContactAllocator &ioContactA
 		JPH_ASSERT(false);
 		break;
 	}
+
+	return false;
 }
 
 void ContactConstraintManager::OnCCDContactAdded(ContactAllocator &ioContactAllocator, const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &outSettings)
@@ -1176,6 +1187,7 @@ void ContactConstraintManager::OnCCDContactAdded(ContactAllocator &ioContactAllo
 	// Calculate contact settings
 	outSettings.mCombinedFriction = mCombineFriction(inBody1, inManifold.mSubShapeID1, inBody2, inManifold.mSubShapeID2);
 	outSettings.mCombinedRestitution = mCombineRestitution(inBody1, inManifold.mSubShapeID1, inBody2, inManifold.mSubShapeID2);
+	outSettings.mIsSensor = false; // For now, no sensors are supported during CCD
 
 	// The remainder of this function only deals with calling contact callbacks, if there's no contact callback we also don't need to do this work
 	if (mContactListener != nullptr)
@@ -1240,6 +1252,8 @@ void ContactConstraintManager::OnCCDContactAdded(ContactAllocator &ioContactAllo
 			mContactListener->OnContactPersisted(*body1, *body2, *manifold, outSettings);
 		}
 	}
+
+	JPH_ASSERT(!outSettings.mIsSensor, "CCD bodies cannot currently act as sensors");
 }
 
 void ContactConstraintManager::SortContacts(uint32 *inConstraintIdxBegin, uint32 *inConstraintIdxEnd) const
@@ -1385,7 +1399,7 @@ JPH_INLINE bool ContactConstraintManager::sSolveVelocityConstraint(ContactConstr
 			// Calculate max impulse that can be applied. Note that we're using the non-penetration impulse from the previous iteration here.
 			// We do this because non-penetration is more important so is solved last (the last things that are solved in an iterative solver
 			// contribute the most).
-			float max_lambda_f = ioConstraint.mSettings.mCombinedFriction * wcp.mNonPenetrationConstraint.GetTotalLambda();
+			float max_lambda_f = ioConstraint.mCombinedFriction * wcp.mNonPenetrationConstraint.GetTotalLambda();
 
 			// Solve friction velocities
 			// Note that what we're doing is not fully correct since the max force we can apply is 2 * max_lambda_f instead of max_lambda_f since we're solving axis independently

Jolt/Physics/Constraints/ContactConstraintManager.h

@@ -83,8 +83,8 @@ public:
 
 	/// 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 was produced: outContactFound = true.
-	void						GetContactsFromCache(ContactAllocator &ioContactAllocator, Body &inBody1, Body &inBody2, bool &outPairHandled, bool &outContactFound);
+	/// When a contact constraint was produced: outConstraintCreated = true.
+	void						GetContactsFromCache(ContactAllocator &ioContactAllocator, Body &inBody1, Body &inBody2, bool &outPairHandled, bool &outConstraintCreated);
 
 	/// Handle used to keep track of the current body pair
 	using BodyPairHandle = void *;
@@ -100,6 +100,7 @@ public:
 	/// @param inBody1 The first body that is colliding
 	/// @param inBody2 The second body that is colliding
 	/// @param inManifold The manifold that describes the collision
+	/// @return true if a contact constraint was created (can be false in the case of a sensor)
 	///
 	/// This is using the approach described in 'Modeling and Solving Constraints' by Erin Catto presented at GDC 2009 (and later years with slight modifications).
 	/// We're using the formulas from slide 50 - 53 combined.
@@ -133,7 +134,7 @@ public:
 	/// r1, r2 = contact point relative to center of mass of body 1 and body 2 (r1 = p1 - x1, r2 = p2 - x2).
 	/// v1, v2 = (linear velocity, angular velocity): 6 vectors containing linear and angular velocity for body 1 and 2.
 	/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].
-	void						AddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPair, Body &inBody1, Body &inBody2, const ContactManifold &inManifold);
+	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.
@@ -439,7 +440,8 @@ private:
 		Body *					mBody1;
 		Body *					mBody2;
 		size_t					mSortKey;
-		ContactSettings			mSettings;
+		float					mCombinedFriction;
+		float					mCombinedRestitution;
 		WorldContactPoints		mContactPoints;
 	};
 
@@ -452,7 +454,7 @@ private:
 
 	/// Internal helper function to add a contact constraint. Templated to the motion type to reduce the amount of branches and calculations.
 	template <EMotionType Type1, EMotionType Type2>
-	void						TemplatedAddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPairHandle, Body &inBody1, Body &inBody2, const ContactManifold &inManifold, Mat44Arg inInvI1, Mat44Arg inInvI2);
+	bool						TemplatedAddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPairHandle, Body &inBody1, Body &inBody2, const ContactManifold &inManifold, Mat44Arg inInvI1, Mat44Arg inInvI2);
 
 	/// Internal helper function to warm start contact constraint. Templated to the motion type to reduce the amount of branches and calculations.
 	template <EMotionType Type1, EMotionType Type2>

Jolt/Physics/PhysicsSystem.cpp

@@ -945,9 +945,9 @@ void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const
 	JPH_ASSERT(body1->IsDynamic() || (body1->IsKinematic() && body2->IsSensor()));
 
 	// Check if the contact points from the previous frame are reusable and if so copy them
-	bool pair_handled = false, contact_found = false;
+	bool pair_handled = false, constraint_created = false;
 	if (mPhysicsSettings.mUseBodyPairContactCache && !(body1->IsCollisionCacheInvalid() || body2->IsCollisionCacheInvalid()))
-		mContactManager.GetContactsFromCache(ioContactAllocator, *body1, *body2, pair_handled, contact_found);
+		mContactManager.GetContactsFromCache(ioContactAllocator, *body1, *body2, pair_handled, constraint_created);
 
 	// If the cache hasn't handled this body pair do actual collision detection
 	if (!pair_handled)
@@ -1094,10 +1094,8 @@ void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const
 					PruneContactPoints(body1->GetCenterOfMassPosition(), manifold.mWorldSpaceNormal, manifold.mWorldSpaceContactPointsOn1, manifold.mWorldSpaceContactPointsOn2);
 
 				// Actually add the contact points to the manager
-				mContactManager.AddContactConstraint(ioContactAllocator, body_pair_handle, *body1, *body2, manifold);
+				constraint_created |= mContactManager.AddContactConstraint(ioContactAllocator, body_pair_handle, *body1, *body2, manifold);
 			}
-
-			contact_found = !collector.mManifolds.empty();
 		}
 		else
 		{
@@ -1167,10 +1165,7 @@ void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const
 					manifold.mSubShapeID2 = inResult.mSubShapeID2;
 
 					// Actually add the contact points to the manager
-					mSystem->mContactManager.AddContactConstraint(mContactAllocator, mBodyPairHandle, *mBody1, *mBody2, manifold);
-
-					// Mark contact found
-					mContactFound = true;
+					mConstraintCreated |= mSystem->mContactManager.AddContactConstraint(mContactAllocator, mBodyPairHandle, *mBody1, *mBody2, manifold);
 				}
 
 				PhysicsSystem *		mSystem;
@@ -1179,7 +1174,7 @@ void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const
 				Body *				mBody2;
 				ContactConstraintManager::BodyPairHandle mBodyPairHandle;
 				bool				mValidateBodyPair = true;
-				bool				mContactFound = false;
+				bool				mConstraintCreated = false;
 			};
 			NonReductionCollideShapeCollector collector(this, ioContactAllocator, body1, body2, body_pair_handle);
 
@@ -1187,12 +1182,12 @@ void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const
 			SubShapeIDCreator part1, part2;
 			CollisionDispatch::sCollideShapeVsShape(body1->GetShape(), body2->GetShape(), Vec3::sReplicate(1.0f), Vec3::sReplicate(1.0f), body1->GetCenterOfMassTransform(), body2->GetCenterOfMassTransform(), part1, part2, settings, collector);
 
-			contact_found = collector.mContactFound;
+			constraint_created = collector.mConstraintCreated;
 		}
 	}
 
-	// If an actual contact is present we need to do some extra work
-	if (contact_found && !body1->IsSensor() && !body2->IsSensor())
+	// If a contact constraint was created, we need to do some extra work
+	if (constraint_created)
 	{
 		// Wake up sleeping bodies
 		BodyID body_ids[2];

UnitTests/Physics/SensorTests.cpp

@@ -42,11 +42,13 @@ TEST_SUITE("SensorTests")
 		// The next step we require that the contact persists
 		c.SimulateSingleStep();
 		CHECK(listener.Contains(EType::Persist, dynamic.GetID(), sensor_id));
+		CHECK(!listener.Contains(EType::Remove, dynamic.GetID(), sensor_id));
 		listener.Clear();
 
 		// After 3 more seconds we should have left the sensor at the bottom side
 		c.Simulate(3.0f + c.GetDeltaTime());
 		CHECK(listener.Contains(EType::Remove, dynamic.GetID(), sensor_id));
+		CHECK_APPROX_EQUAL(dynamic.GetPosition(), Vec3(0, -1.5f - 3.0f * c.GetDeltaTime(), 0), 1.0e-4f);
 	}
 
 	TEST_CASE("TestKinematicVsSensor")
@@ -78,11 +80,13 @@ TEST_SUITE("SensorTests")
 		// The next step we require that the contact persists
 		c.SimulateSingleStep();
 		CHECK(listener.Contains(EType::Persist, kinematic.GetID(), sensor_id));
+		CHECK(!listener.Contains(EType::Remove, kinematic.GetID(), sensor_id));
 		listener.Clear();
 
 		// After 3 more seconds we should have left the sensor at the bottom side
 		c.Simulate(3.0f + c.GetDeltaTime());
 		CHECK(listener.Contains(EType::Remove, kinematic.GetID(), sensor_id));
+		CHECK_APPROX_EQUAL(kinematic.GetPosition(), Vec3(0, -1.5f - 3.0f * c.GetDeltaTime(), 0), 1.0e-4f);
 	}
 
 	TEST_CASE("TestDynamicSleepingVsStaticSensor")
@@ -109,6 +113,10 @@ TEST_SUITE("SensorTests")
 		c.SimulateSingleStep();
 		CHECK(listener.GetEntryCount() == 0);
 
+		// The dynamic object should not be part of an island
+		CHECK(!sensor.IsActive());
+		CHECK(dynamic.GetMotionProperties()->GetIslandIndexInternal() == Body::cInactiveIndex);
+
 		// Activate the body
 		c.GetBodyInterface().ActivateBody(dynamic.GetID());
 
@@ -121,11 +129,19 @@ TEST_SUITE("SensorTests")
 		CHECK(listener.Contains(EType::Add, sensor.GetID(), dynamic.GetID()));
 		listener.Clear();
 
+		// The dynamic object should be part of an island now
+		CHECK(!sensor.IsActive());
+		CHECK(dynamic.GetMotionProperties()->GetIslandIndexInternal() != Body::cInactiveIndex);
+
 		// After a second the body should have gone to sleep and the contacts should have been removed
 		c.Simulate(1.0f);
 		CHECK(!dynamic.IsActive());
 		CHECK(listener.Contains(EType::Remove, floor.GetID(), dynamic.GetID()));
 		CHECK(listener.Contains(EType::Remove, sensor.GetID(), dynamic.GetID()));
+
+		// The dynamic object should not be part of an island
+		CHECK(!sensor.IsActive());
+		CHECK(dynamic.GetMotionProperties()->GetIslandIndexInternal() == Body::cInactiveIndex);
 	}
 
 	TEST_CASE("TestDynamicSleepingVsKinematicSensor")
@@ -159,6 +175,16 @@ TEST_SUITE("SensorTests")
 		CHECK(sensor.GetMotionProperties()->GetIslandIndexInternal() != Body::cInactiveIndex);
 		CHECK(dynamic.GetMotionProperties()->GetIslandIndexInternal() == Body::cInactiveIndex);
 
+		// The second step, the contact with the sensor should have persisted
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() == 1);
+		CHECK(listener.Contains(EType::Persist, sensor.GetID(), dynamic.GetID()));
+		listener.Clear();
+
+		// The sensor should still be in its own island
+		CHECK(sensor.GetMotionProperties()->GetIslandIndexInternal() != Body::cInactiveIndex);
+		CHECK(dynamic.GetMotionProperties()->GetIslandIndexInternal() == Body::cInactiveIndex);
+
 		// Activate the body
 		c.GetBodyInterface().ActivateBody(dynamic.GetID());
 
@@ -175,10 +201,98 @@ TEST_SUITE("SensorTests")
 		CHECK(dynamic.GetMotionProperties()->GetIslandIndexInternal() != Body::cInactiveIndex);
 		CHECK(sensor.GetMotionProperties()->GetIslandIndexInternal() != dynamic.GetMotionProperties()->GetIslandIndexInternal());
 
+		// After another step we should have persisted the collision with the floor and sensor
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() >= 2); // Depending on if we used the contact cache or not there will be validate callbacks too
+		CHECK(listener.Contains(EType::Persist, floor.GetID(), dynamic.GetID()));
+		CHECK(!listener.Contains(EType::Remove, floor.GetID(), dynamic.GetID()));
+		CHECK(listener.Contains(EType::Persist, sensor.GetID(), dynamic.GetID()));
+		CHECK(!listener.Contains(EType::Remove, sensor.GetID(), dynamic.GetID()));
+		listener.Clear();
+
+		// The same islands as the previous step should have been created
+		CHECK(sensor.GetMotionProperties()->GetIslandIndexInternal() != Body::cInactiveIndex);
+		CHECK(dynamic.GetMotionProperties()->GetIslandIndexInternal() != Body::cInactiveIndex);
+		CHECK(sensor.GetMotionProperties()->GetIslandIndexInternal() != dynamic.GetMotionProperties()->GetIslandIndexInternal());
+
 		// After a second the body should have gone to sleep and the contacts with the floor should have been removed, but not with the sensor
 		c.Simulate(1.0f);
 		CHECK(!dynamic.IsActive());
 		CHECK(listener.Contains(EType::Remove, floor.GetID(), dynamic.GetID()));
 		CHECK(!listener.Contains(EType::Remove, sensor.GetID(), dynamic.GetID()));
 	}
+
+	TEST_CASE("TestContactListenerMakesSensor")
+	{
+		PhysicsTestContext c;
+		c.ZeroGravity();
+
+		class SensorOverridingListener : public LoggingContactListener
+		{
+		public:
+			virtual void		OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+			{
+				LoggingContactListener::OnContactAdded(inBody1, inBody2, inManifold, ioSettings);
+
+				JPH_ASSERT(ioSettings.mIsSensor == false);
+				if (inBody1.GetID() == mBodyThatSeesSensorID || inBody2.GetID() == mBodyThatSeesSensorID)
+					ioSettings.mIsSensor = true;
+			}
+
+			virtual void		OnContactPersisted(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+			{
+				LoggingContactListener::OnContactPersisted(inBody1, inBody2, inManifold, ioSettings);
+
+				JPH_ASSERT(ioSettings.mIsSensor == false);
+				if (inBody1.GetID() == mBodyThatSeesSensorID || inBody2.GetID() == mBodyThatSeesSensorID)
+					ioSettings.mIsSensor = true;
+			}
+
+			BodyID				mBodyThatSeesSensorID;
+		};
+
+		// Register listener
+		SensorOverridingListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		// Body that will appear as a sensor to one object and as static to another
+		BodyID static_id = c.GetBodyInterface().CreateAndAddBody(BodyCreationSettings(new BoxShape(Vec3(5, 1, 5)), Vec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+
+		// Dynamic body moving down that will do a normal collision
+		Body &dynamic1 = c.CreateBox(Vec3(-2, 2, 0), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(0.5f));
+		dynamic1.SetAllowSleeping(false);
+		dynamic1.SetLinearVelocity(Vec3(0, -1, 0));
+
+		// Dynamic body moving down that will only see the static object as a sensor
+		Body &dynamic2 = c.CreateBox(Vec3(2, 2, 0), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(0.5f));
+		dynamic2.SetAllowSleeping(false);
+		dynamic2.SetLinearVelocity(Vec3(0, -1, 0));
+		listener.mBodyThatSeesSensorID = dynamic2.GetID();
+
+		// After a single step the dynamic object should not have touched the sensor yet
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() == 0);
+
+		// After half a second both bodies should be touching the sensor
+		c.Simulate(0.5f);
+		CHECK(listener.Contains(EType::Add, dynamic1.GetID(), static_id));
+		CHECK(listener.Contains(EType::Add, dynamic2.GetID(), static_id));
+		listener.Clear();
+
+		// The next step we require that the contact persists
+		c.SimulateSingleStep();
+		CHECK(listener.Contains(EType::Persist, dynamic1.GetID(), static_id));
+		CHECK(!listener.Contains(EType::Remove, dynamic1.GetID(), static_id));
+		CHECK(listener.Contains(EType::Persist, dynamic2.GetID(), static_id));
+		CHECK(!listener.Contains(EType::Remove, dynamic2.GetID(), static_id));
+		listener.Clear();
+
+		// After 3 more seconds one body should be resting on the static body, the other should have fallen through
+		c.Simulate(3.0f + c.GetDeltaTime());
+		CHECK(listener.Contains(EType::Persist, dynamic1.GetID(), static_id));
+		CHECK(!listener.Contains(EType::Remove, dynamic1.GetID(), static_id));
+		CHECK(listener.Contains(EType::Remove, dynamic2.GetID(), static_id));
+		CHECK_APPROX_EQUAL(dynamic1.GetPosition(), Vec3(-2, 1.5f, 0), 5.0e-3f);
+		CHECK_APPROX_EQUAL(dynamic2.GetPosition(), Vec3(2, -1.5f - 3.0f * c.GetDeltaTime(), 0), 1.0e-4f);
+	}
 }