Allowed sensors to collide with one another (#432)

* Allowed sensors to collide with one another
* Added unit test for sensor vs sensor

Jolt/Physics/Body/Body.inl

@@ -31,15 +31,9 @@ inline bool Body::sFindCollidingPairsCanCollide(const Body &inBody1, const Body
 	// One of these conditions must be true
 	// - One of the bodies must be dynamic to collide
 	// - A kinematic object can collide with a sensor
-	bool body1_sensor = inBody1.IsSensor();
-	bool body2_sensor = inBody2.IsSensor();
 	if ((!inBody1.IsDynamic() && !inBody2.IsDynamic()) 
-		&& !(inBody1.IsKinematic() && body2_sensor)
-		&& !(inBody2.IsKinematic() && body1_sensor))
-		return false;
-
-	// If both bodies are sensors, there's no collision
-	if (body1_sensor && body2_sensor)
+		&& !(inBody1.IsKinematic() && inBody2.IsSensor())
+		&& !(inBody2.IsKinematic() && inBody1.IsSensor()))
 		return false;
 
 	// Check that body 1 is active

UnitTests/Layers.h

@@ -18,7 +18,8 @@ namespace Layers
 	static constexpr uint8 MOVING = 6;
 	static constexpr uint8 HQ_DEBRIS = 7; // High quality debris collides with MOVING and NON_MOVING but not with any debris
 	static constexpr uint8 LQ_DEBRIS = 8; // Low quality debris only collides with NON_MOVING
-	static constexpr uint8 NUM_LAYERS = 9;
+	static constexpr uint8 SENSOR = 9; // Sensors only collide with MOVING objects
+	static constexpr uint8 NUM_LAYERS = 10;
 };
 
 /// Class that determines if two object layers can collide
@@ -38,11 +39,13 @@ public:
 		case Layers::NON_MOVING:
 			return inObject2 == Layers::MOVING || inObject2 == Layers::HQ_DEBRIS || inObject2 == Layers::LQ_DEBRIS;
 		case Layers::MOVING:
-			return inObject2 == Layers::NON_MOVING || inObject2 == Layers::MOVING || inObject2 == Layers::HQ_DEBRIS;
+			return inObject2 == Layers::NON_MOVING || inObject2 == Layers::MOVING || inObject2 == Layers::HQ_DEBRIS || inObject2 == Layers::SENSOR;
 		case Layers::HQ_DEBRIS:
 			return inObject2 == Layers::NON_MOVING || inObject2 == Layers::MOVING;
 		case Layers::LQ_DEBRIS:
 			return inObject2 == Layers::NON_MOVING;
+		case Layers::SENSOR:
+			return inObject2 == Layers::MOVING;
 		default:
 			JPH_ASSERT(false);
 			return false;
@@ -57,7 +60,8 @@ namespace BroadPhaseLayers
 	static constexpr BroadPhaseLayer MOVING(1); 
 	static constexpr BroadPhaseLayer LQ_DEBRIS(2);
 	static constexpr BroadPhaseLayer UNUSED(3);
-	static constexpr uint NUM_LAYERS(4);
+	static constexpr BroadPhaseLayer SENSOR(4);
+	static constexpr uint NUM_LAYERS(5);
 };
 
 /// BroadPhaseLayerInterface implementation
@@ -76,6 +80,7 @@ public:
 		mObjectToBroadPhase[Layers::MOVING] = BroadPhaseLayers::MOVING;
 		mObjectToBroadPhase[Layers::HQ_DEBRIS] = BroadPhaseLayers::MOVING; // HQ_DEBRIS is also in the MOVING layer as an example on how to map multiple layers onto the same broadphase layer
 		mObjectToBroadPhase[Layers::LQ_DEBRIS] = BroadPhaseLayers::LQ_DEBRIS;
+		mObjectToBroadPhase[Layers::SENSOR] = BroadPhaseLayers::SENSOR;
 	}
 
 	virtual uint					GetNumBroadPhaseLayers() const override
@@ -98,6 +103,7 @@ public:
 		case (BroadPhaseLayer::Type)BroadPhaseLayers::MOVING:		return "MOVING";
 		case (BroadPhaseLayer::Type)BroadPhaseLayers::LQ_DEBRIS:	return "LQ_DEBRIS";
 		case (BroadPhaseLayer::Type)BroadPhaseLayers::UNUSED:		return "UNUSED";
+		case (BroadPhaseLayer::Type)BroadPhaseLayers::SENSOR:		return "SENSOR";
 		default:													JPH_ASSERT(false); return "INVALID";
 		}
 	}
@@ -119,15 +125,17 @@ public:
 			return inLayer2 == BroadPhaseLayers::MOVING;
 		case Layers::MOVING:
 		case Layers::HQ_DEBRIS:
-			return inLayer2 == BroadPhaseLayers::NON_MOVING || inLayer2 == BroadPhaseLayers::MOVING;
+			return inLayer2 == BroadPhaseLayers::NON_MOVING || inLayer2 == BroadPhaseLayers::MOVING || inLayer2 == BroadPhaseLayers::SENSOR;
 		case Layers::LQ_DEBRIS:
 			return inLayer2 == BroadPhaseLayers::NON_MOVING;
+		case Layers::SENSOR:
+			return inLayer2 == BroadPhaseLayers::MOVING;
 		case Layers::UNUSED1:
 		case Layers::UNUSED2:
 		case Layers::UNUSED3:
 		case Layers::UNUSED4:
 		case Layers::UNUSED5:
-			return false;			
+			return false;
 		default:
 			JPH_ASSERT(false);
 			return false;

UnitTests/Physics/SensorTests.cpp

@@ -23,7 +23,7 @@ TEST_SUITE("SensorTests")
 		c.GetSystem()->SetContactListener(&listener);
 
 		// Sensor
-		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(1)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(1)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::SENSOR);
 		sensor_settings.mIsSensor = true;
 		BodyID sensor_id = c.GetBodyInterface().CreateAndAddBody(sensor_settings, EActivation::DontActivate);
 
@@ -61,7 +61,7 @@ TEST_SUITE("SensorTests")
 		c.GetSystem()->SetContactListener(&listener);
 
 		// Sensor
-		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(1)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(1)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::SENSOR);
 		sensor_settings.mIsSensor = true;
 		BodyID sensor_id = c.GetBodyInterface().CreateAndAddBody(sensor_settings, EActivation::DontActivate);
 
@@ -101,7 +101,7 @@ TEST_SUITE("SensorTests")
 		c.GetSystem()->SetContactListener(&listener);
 
 		// Kinematic sensor
-		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(1)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Kinematic, Layers::NON_MOVING);
+		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(1)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Kinematic, Layers::SENSOR);
 		sensor_settings.mIsSensor = true;
 		BodyID sensor_id = c.GetBodyInterface().CreateAndAddBody(sensor_settings, EActivation::Activate);
 
@@ -145,7 +145,7 @@ TEST_SUITE("SensorTests")
 		kinematic.SetLinearVelocity(Vec3(0, -1, 0));
 
 		// Kinematic sensor
-		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(1)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Kinematic, Layers::NON_MOVING);
+		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(1)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Kinematic, Layers::SENSOR);
 		sensor_settings.mIsSensor = true;
 		BodyID sensor_id = c.GetBodyInterface().CreateAndAddBody(sensor_settings, EActivation::Activate);
 
@@ -179,7 +179,7 @@ TEST_SUITE("SensorTests")
 		c.GetSystem()->SetContactListener(&listener);
 
 		// Sensor
-		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(1)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(1)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::SENSOR);
 		sensor_settings.mIsSensor = true;
 		Body &sensor = *c.GetBodyInterface().CreateBody(sensor_settings);
 		c.GetBodyInterface().AddBody(sensor.GetID(), EActivation::DontActivate);
@@ -234,7 +234,7 @@ TEST_SUITE("SensorTests")
 		c.GetSystem()->SetContactListener(&listener);
 
 		// Kinematic sensor that is active (so will keep detecting contacts with sleeping bodies)
-		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(1)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Kinematic, Layers::NON_MOVING);
+		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(1)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Kinematic, Layers::SENSOR);
 		sensor_settings.mIsSensor = true;
 		Body &sensor = *c.GetBodyInterface().CreateBody(sensor_settings);
 		c.GetBodyInterface().AddBody(sensor.GetID(), EActivation::Activate);
@@ -303,6 +303,67 @@ TEST_SUITE("SensorTests")
 		CHECK(!listener.Contains(EType::Remove, sensor.GetID(), dynamic.GetID()));
 	}
 
+	TEST_CASE("TestSensorVsSensor")
+	{
+		for (int test = 0; test < 2; ++test)
+		{
+			bool sensor_detects_sensor = test == 1;
+
+			PhysicsTestContext c;
+
+			// Register listener
+			LoggingContactListener listener;
+			c.GetSystem()->SetContactListener(&listener);
+
+			// Depending on the iteration we either place the sensor in the moving layer which means it will collide with other sensors
+			// or we put it in the sensor layer which means it won't collide with other sensors
+			ObjectLayer layer = sensor_detects_sensor? Layers::MOVING : Layers::SENSOR;
+
+			// Sensor 1
+			BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(1)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, layer);
+			sensor_settings.mIsSensor = true;
+			BodyID sensor_id1 = c.GetBodyInterface().CreateAndAddBody(sensor_settings, EActivation::DontActivate);
+
+			// Sensor 2 moving downwards
+			sensor_settings.mMotionType = EMotionType::Kinematic;
+			sensor_settings.mPosition = RVec3(0, 3, 0);
+			sensor_settings.mIsSensor = true;
+			sensor_settings.mLinearVelocity = Vec3(0, -2, 0);
+			BodyID sensor_id2 = c.GetBodyInterface().CreateAndAddBody(sensor_settings, EActivation::Activate);
+
+			// After a single step the sensors should not touch yet
+			c.SimulateSingleStep();
+			CHECK(listener.GetEntryCount() == 0);
+
+			// After half a second the sensors should be touching
+			c.Simulate(0.5f);
+			if (sensor_detects_sensor)
+				CHECK(listener.Contains(EType::Add, sensor_id1, sensor_id2));
+			else
+				CHECK(listener.GetEntryCount() == 0);
+			listener.Clear();
+
+			// The next step we require that the contact persists
+			c.SimulateSingleStep();
+			if (sensor_detects_sensor)
+			{
+				CHECK(listener.Contains(EType::Persist, sensor_id1, sensor_id2));
+				CHECK(!listener.Contains(EType::Remove, sensor_id1, sensor_id2));
+			}
+			else
+				CHECK(listener.GetEntryCount() == 0);
+			listener.Clear();
+
+			// After 2 more seconds we should have left the sensor at the bottom side
+			c.Simulate(2.0f + c.GetDeltaTime());
+			if (sensor_detects_sensor)
+				CHECK(listener.Contains(EType::Remove, sensor_id1, sensor_id2));
+			else
+				CHECK(listener.GetEntryCount() == 0);
+			CHECK_APPROX_EQUAL(c.GetBodyInterface().GetPosition(sensor_id2), sensor_settings.mPosition + sensor_settings.mLinearVelocity * (2.5f + 3.0f * c.GetDeltaTime()), 1.0e-4f);
+		}
+	}
+
 	TEST_CASE("TestContactListenerMakesSensor")
 	{
 		PhysicsTestContext c;
@@ -387,7 +448,7 @@ TEST_SUITE("SensorTests")
 		c.GetSystem()->SetContactListener(&listener);
 
 		// Create sensor
-		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(5.0f)), RVec3(0, 10, 0), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(5.0f)), RVec3(0, 10, 0), Quat::sIdentity(), EMotionType::Static, Layers::SENSOR);
 		sensor_settings.mIsSensor = true;
 		BodyID sensor_id = bi.CreateAndAddBody(sensor_settings, EActivation::DontActivate);