Sensors now can detect static objects (#581)

* Enable the BodyCreationSettings::mSensorDetectsStatic flag to have sensors interact with static objects (note the sensor must be kinematic and active for this to work)
* Added test case for sensor detecting static

Fixes #574 and allows godot-jolt/godot-jolt#391 to be fixed.

Jolt/Physics/Body/Body.cpp

@@ -324,6 +324,7 @@ BodyCreationSettings Body::GetBodyCreationSettings() const
 	result.mMotionType = GetMotionType();
 	result.mAllowDynamicOrKinematic = mMotionProperties != nullptr;
 	result.mIsSensor = IsSensor();
+	result.mSensorDetectsStatic = SensorDetectsStatic();
 	result.mUseManifoldReduction = GetUseManifoldReduction();
 	result.mMotionQuality = mMotionProperties != nullptr? mMotionProperties->GetMotionQuality() : EMotionQuality::Discrete;
 	result.mAllowSleeping = mMotionProperties != nullptr? GetAllowSleeping() : true;

Jolt/Physics/Body/Body.h

@@ -68,6 +68,12 @@ public:
 	/// Check if this body is a sensor.
 	inline bool				IsSensor() const												{ return (mFlags.load(memory_order_relaxed) & uint8(EFlags::IsSensor)) != 0; }
 
+	// If this sensor detects static objects entering it. Note that the sensor must be kinematic and active for it to detect static objects.
+	inline void				SetSensorDetectsStatic(bool inDetectsStatic)					{ if (inDetectsStatic) mFlags.fetch_or(uint8(EFlags::SensorDetectsStatic), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::SensorDetectsStatic)), memory_order_relaxed); }
+
+	/// Check if this sensor detects static objects entering it.
+	inline bool				SensorDetectsStatic() const										{ return (mFlags.load(memory_order_relaxed) & uint8(EFlags::SensorDetectsStatic)) != 0; }
+
 	/// If PhysicsSettings::mUseManifoldReduction is true, this allows turning off manifold reduction for this specific body. Manifold reduction by default will combine contacts that come from different SubShapeIDs (e.g. different triangles or different compound shapes).
 	/// If the application requires tracking exactly which SubShapeIDs are in contact, you can turn off manifold reduction. Note that this comes at a performance cost.
 	inline void				SetUseManifoldReduction(bool inUseReduction)					{ if (inUseReduction) mFlags.fetch_or(uint8(EFlags::UseManifoldReduction), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::UseManifoldReduction)), memory_order_relaxed); }
@@ -304,9 +310,10 @@ private:
 	enum class EFlags : uint8
 	{
 		IsSensor				= 1 << 0,													///< If this object is a sensor. A sensor will receive collision callbacks, but will not cause any collision responses and can be used as a trigger volume.
-		IsInBroadPhase			= 1 << 1,													///< Set this bit to indicate that the body is in the broadphase
-		InvalidateContactCache	= 1 << 2,													///< Set this bit to indicate that all collision caches for this body are invalid, will be reset the next simulation step.
-		UseManifoldReduction	= 1 << 3,													///< Set this bit to indicate that this body can use manifold reduction (if PhysicsSettings::mUseManifoldReduction is true)
+		SensorDetectsStatic		= 1 << 1,													///< If this sensor detects static objects entering it.
+		IsInBroadPhase			= 1 << 2,													///< Set this bit to indicate that the body is in the broadphase
+		InvalidateContactCache	= 1 << 3,													///< Set this bit to indicate that all collision caches for this body are invalid, will be reset the next simulation step.
+		UseManifoldReduction	= 1 << 4,													///< Set this bit to indicate that this body can use manifold reduction (if PhysicsSettings::mUseManifoldReduction is true)
 	};
 
 	// 16 byte aligned

Jolt/Physics/Body/Body.inl

@@ -27,14 +27,26 @@ RMat44 Body::GetInverseCenterOfMassTransform() const
 	return RMat44::sInverseRotationTranslation(mRotation, mPosition);
 }
 
+inline static bool sIsValidSensorBodyPair(const Body &inSensor, const Body &inOther)
+{
+	// If the sensor is not an actual sensor then this is not a valid pair
+	if (!inSensor.IsSensor())
+		return false;
+	
+	if (inSensor.SensorDetectsStatic())
+		return !inOther.IsDynamic(); // If the other body is dynamic, the pair will be handled when the bodies are swapped, otherwise we'll detect the collision twice
+	else	
+		return inOther.IsKinematic(); // Only kinematic bodies are valid
+}
+
 inline bool Body::sFindCollidingPairsCanCollide(const Body &inBody1, const Body &inBody2)
 {
 	// One of these conditions must be true
 	// - One of the bodies must be dynamic to collide
-	// - A kinematic object can collide with a sensor
+	// - A sensor can collide with non-dynamic bodies
 	if ((!inBody1.IsDynamic() && !inBody2.IsDynamic()) 
-		&& !(inBody1.IsKinematic() && inBody2.IsSensor())
-		&& !(inBody2.IsKinematic() && inBody1.IsSensor()))
+		&& !sIsValidSensorBodyPair(inBody1, inBody2)
+		&& !sIsValidSensorBodyPair(inBody2, inBody1))
 		return false;
 
 	// Check that body 1 is active

Jolt/Physics/Body/BodyCreationSettings.cpp

@@ -24,6 +24,7 @@ JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(BodyCreationSettings)
 	JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mMotionType)
 	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mAllowDynamicOrKinematic)
 	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mIsSensor)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mSensorDetectsStatic)
 	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mUseManifoldReduction)
 	JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mMotionQuality)
 	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mAllowSleeping)
@@ -50,6 +51,7 @@ void BodyCreationSettings::SaveBinaryState(StreamOut &inStream) const
 	inStream.Write(mMotionType);
 	inStream.Write(mAllowDynamicOrKinematic);
 	inStream.Write(mIsSensor);
+	inStream.Write(mSensorDetectsStatic);
 	inStream.Write(mUseManifoldReduction);
 	inStream.Write(mMotionQuality);
 	inStream.Write(mAllowSleeping);
@@ -76,6 +78,7 @@ void BodyCreationSettings::RestoreBinaryState(StreamIn &inStream)
 	inStream.Read(mMotionType);
 	inStream.Read(mAllowDynamicOrKinematic);
 	inStream.Read(mIsSensor);
+	inStream.Read(mSensorDetectsStatic);
 	inStream.Read(mUseManifoldReduction);
 	inStream.Read(mMotionQuality);
 	inStream.Read(mAllowSleeping);

Jolt/Physics/Body/BodyCreationSettings.h

@@ -91,6 +91,7 @@ public:
 	EMotionType				mMotionType = EMotionType::Dynamic;								///< Motion type, determines if the object is static, dynamic or kinematic
 	bool					mAllowDynamicOrKinematic = false;								///< When this body is created as static, this setting tells the system to create a MotionProperties object so that the object can be switched to kinematic or dynamic
 	bool					mIsSensor = false;												///< If this body is a sensor. A sensor will receive collision callbacks, but will not cause any collision responses and can be used as a trigger volume. See description at Body::SetIsSensor.
+	bool					mSensorDetectsStatic = false;									///< If this sensor detects static objects entering it. Note that the sensor must be kinematic and active for it to detect static objects.
 	bool					mUseManifoldReduction = true;									///< If this body should use manifold reduction (see description at Body::SetUseManifoldReduction)
 	EMotionQuality			mMotionQuality = EMotionQuality::Discrete;						///< Motion quality, or how well it detects collisions when it has a high velocity
 	bool					mAllowSleeping = true;											///< If this body can go to sleep or not

Jolt/Physics/Body/BodyManager.cpp

@@ -162,6 +162,8 @@ Body *BodyManager::AllocateBody(const BodyCreationSettings &inBodyCreationSettin
 	body->mMotionType = inBodyCreationSettings.mMotionType;
 	if (inBodyCreationSettings.mIsSensor)
 		body->SetIsSensor(true);
+	if (inBodyCreationSettings.mSensorDetectsStatic)
+		body->SetSensorDetectsStatic(true);
 	if (inBodyCreationSettings.mUseManifoldReduction)
 		body->SetUseManifoldReduction(true);
 	SetBodyObjectLayerInternal(*body, inBodyCreationSettings.mObjectLayer);

Jolt/Physics/PhysicsSystem.cpp

@@ -973,7 +973,7 @@ void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const
 	if ((!body1->IsDynamic() || (body2->IsDynamic() && inBodyPair.mBodyB < inBodyPair.mBodyA)) 
 		&& !body2->IsSensor())
 		swap(body1, body2);
-	JPH_ASSERT(body1->IsDynamic() || (body1->IsKinematic() && body2->IsSensor()));
+	JPH_ASSERT(body1->IsDynamic() || body2->IsSensor());
 
 	// Check if the contact points from the previous frame are reusable and if so copy them
 	bool pair_handled = false, constraint_created = false;
@@ -1030,8 +1030,8 @@ void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const
 				{
 					// One of the following should be true:
 					// - Body 1 is dynamic and body 2 may be dynamic, static or kinematic
-					// - Body 1 is kinematic in which case body 2 should be a sensor
-					JPH_ASSERT(mBody1->IsDynamic() || (mBody1->IsKinematic() && mBody2->IsSensor()));
+					// - Body 1 is not dynamic in which case body 2 should be a sensor
+					JPH_ASSERT(mBody1->IsDynamic() || mBody2->IsSensor());
 					JPH_ASSERT(!ShouldEarlyOut());
 
 					// Test if we want to accept this hit

Samples/Tests/General/SensorTest.cpp

@@ -29,7 +29,7 @@ SensorTest::~SensorTest()
 void SensorTest::Initialize()
 {
 	// Floor
-	CreateFloor();
+	CreateFloor(400.0f);
 
 	{
 		// A static sensor that attrects dynamic bodies that enter its area
@@ -40,22 +40,35 @@ void SensorTest::Initialize()
 	
 	{
 		// A static sensor that only detects active bodies
-		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(5.0f)), RVec3(-10, 5, 0), Quat::sIdentity(), EMotionType::Static, Layers::SENSOR);
+		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(5.0f)), RVec3(-10, 5.1f, 0), Quat::sIdentity(), EMotionType::Static, Layers::SENSOR);
 		sensor_settings.mIsSensor = true;
 		mSensorID[StaticSensor] = mBodyInterface->CreateAndAddBody(sensor_settings, EActivation::DontActivate);
 	}
 
 	{
 		// A kinematic sensor that also detects sleeping bodies
-		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(5.0f)), RVec3(10, 5, 0), Quat::sIdentity(), EMotionType::Kinematic, Layers::SENSOR);
+		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(5.0f)), RVec3(10, 5.1f, 0), Quat::sIdentity(), EMotionType::Kinematic, Layers::SENSOR);
 		sensor_settings.mIsSensor = true;
 		mSensorID[KinematicSensor] = mBodyInterface->CreateAndAddBody(sensor_settings, EActivation::Activate);
 	}
 
+	{
+		// A kinematic sensor that also detects static bodies
+		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(5.0f)), RVec3(25, 5.1f, 0), Quat::sIdentity(), EMotionType::Kinematic, Layers::MOVING); // Put in a layer that collides with static
+		sensor_settings.mIsSensor = true;
+		sensor_settings.mSensorDetectsStatic = true;
+		mSensorID[SensorDetectingStatic] = mBodyInterface->CreateAndAddBody(sensor_settings, EActivation::Activate);
+	}
+
 	// Dynamic bodies
-	for (int i = 0; i < 10; ++i)
+	for (int i = 0; i < 15; ++i)
 		mBodyInterface->CreateAndAddBody(BodyCreationSettings(new BoxShape(Vec3(0.1f, 0.5f, 0.2f)), RVec3(-15.0f + i * 3.0f, 25, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING), EActivation::Activate);
 
+	// Static bodies
+	RVec3 static_positions[] = { RVec3(-14, 1, 4), RVec3(6, 1, 4), RVec3(21, 1, 4) };
+	for (const RVec3 &p : static_positions)
+		mBodyInterface->CreateAndAddBody(BodyCreationSettings(new BoxShape(Vec3::sReplicate(0.5f)), p, Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::Activate);
+
 	// Load ragdoll
 	Ref<RagdollSettings> ragdoll_settings = RagdollLoader::sLoad("Assets/Human.tof", EMotionType::Dynamic);
 	if (ragdoll_settings == nullptr)
@@ -95,7 +108,7 @@ void SensorTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
 	mBodyInterface->MoveKinematic(mKinematicBodyID, kinematic_pos, Quat::sIdentity(), inParams.mDeltaTime);
 
 	// Draw if body is in sensor
-	Color sensor_color[] = { Color::sRed, Color::sGreen, Color::sBlue };
+	Color sensor_color[] = { Color::sRed, Color::sGreen, Color::sBlue, Color::sPurple };
 	for (int sensor = 0; sensor < NumSensors; ++sensor)
 		for (const BodyAndCount &body_and_count : mBodiesInSensor[sensor])
 		{

Samples/Tests/General/SensorTest.h

@@ -41,6 +41,7 @@ private:
 		StaticAttractor,									// A static sensor that attrects dynamic bodies that enter its area
 		StaticSensor,										// A static sensor that only detects active bodies
 		KinematicSensor,									// A kinematic sensor that also detects sleeping bodies
+		SensorDetectingStatic,								// A kinematic sensor that detects static bodies
 		NumSensors
 	};
 

UnitTests/Physics/SensorTests.cpp

@@ -529,4 +529,51 @@ TEST_SUITE("SensorTests")
 		// Check all expected events received
 		CHECK(next == end);
 	}
+
+	TEST_CASE("TestSensorVsStatic")
+	{
+		PhysicsTestContext c;
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		// Static body 1
+		Body &static1 = c.CreateSphere(RVec3::sZero(), 1.0f, EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+
+		// Sensor
+		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(1)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Kinematic, Layers::MOVING); // Put in layer that collides with static
+		sensor_settings.mIsSensor = true;
+		Body &sensor = *c.GetBodyInterface().CreateBody(sensor_settings);
+		BodyID sensor_id = sensor.GetID();
+		c.GetBodyInterface().AddBody(sensor_id, EActivation::Activate);
+
+		// Static body 2 (created after sensor to force higher body ID)
+		Body &static2 = c.CreateSphere(RVec3::sZero(), 1.0f, EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+
+		// After a step we should not detect the static bodies
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() == 0);
+		listener.Clear();
+
+		// Start detecting static
+		sensor.SetSensorDetectsStatic(true);
+
+		// After a single step we should detect both static bodies
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() == 4); // Should also contain validates
+		CHECK(listener.Contains(EType::Add, static1.GetID(), sensor_id));
+		CHECK(listener.Contains(EType::Add, static2.GetID(), sensor_id));
+		listener.Clear();
+
+		// Stop detecting static
+		sensor.SetSensorDetectsStatic(false);
+
+		// After a single step we should stop detecting both static bodies
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() == 2);
+		CHECK(listener.Contains(EType::Remove, static1.GetID(), sensor_id));
+		CHECK(listener.Contains(EType::Remove, static2.GetID(), sensor_id));
+		listener.Clear();
+	}
 }