Ability to have a kinematic body colliding with a sensor (#7)

Jolt/Physics/Body/Body.inl

@@ -28,8 +28,11 @@ const Mat44 Body::GetInverseCenterOfMassTransform() const
 
 inline bool Body::sFindCollidingPairsCanCollide(const Body &inBody1, const Body &inBody2)
 {
-	// One of the bodies must be dynamic in order to collide
-	if (!inBody1.IsDynamic() && !inBody2.IsDynamic())
+	// One of these conditions must be true
+	// - One of the bodies must be dynamic to collide
+	// - A kinematic object can collide with a sensor
+	if ((!inBody1.IsDynamic() && !inBody2.IsDynamic()) 
+		&& !(inBody1.IsKinematic() && inBody2.IsSensor()))
 		return false;
 
 	// Check that body 1 is active

Jolt/Physics/Body/BodyManager.cpp

@@ -141,6 +141,8 @@ Body *BodyManager::CreateBody(const BodyCreationSettings &inBodyCreationSettings
 	
 	if (inBodyCreationSettings.HasMassProperties())
 	{
+		JPH_ASSERT(!inBodyCreationSettings.mIsSensor, "Sensors should be static and moved through BodyInterface::SetPosition/SetPositionAndRotation");
+
 		MotionProperties *mp = new MotionProperties();
 		mp->SetLinearDamping(inBodyCreationSettings.mLinearDamping);
 		mp->SetAngularDamping(inBodyCreationSettings.mAngularDamping);

Jolt/Physics/Collision/ContactListener.h

@@ -63,7 +63,7 @@ public:
 	/// This is a rather expensive time to reject a contact point since a lot of the collision detection has happened already, make sure you
 	/// filter out the majority of undesired body pairs through the ObjectLayerPairFilter that is registered on the PhysicsSystem.
 	/// Note that this callback is called when all bodies are locked, so don't use any locking functions!
-	/// Body 1 is always dynamic, otherwise there is no sorting between body 1 and 2
+	/// The order of body 1 and 2 is undefined, but when one of the two bodies is dynamic it will be body 1
 	virtual ValidateResult	OnContactValidate(const Body &inBody1, const Body &inBody2, const CollideShapeResult &inCollisionResult) { return ValidateResult::AcceptAllContactsForThisBodyPair; }
 
 	/// Called whenever a new contact point is detected.

Jolt/Physics/PhysicsSystem.cpp

@@ -904,9 +904,11 @@ void PhysicsSystem::ProcessBodyPair(const BodyPair &inBodyPair)
 
 	// Ensure that body1 is dynamic, this ensures that we do the collision detection in the space of a moving body, which avoids accuracy problems when testing a very large static object against a small dynamic object
 	// Ensure that body1 id < body2 id for dynamic vs dynamic
-	if (!body1->IsDynamic() || (body2->IsDynamic() && inBodyPair.mBodyB < inBodyPair.mBodyA))
+	// Keep body order unchanged when colliding with a sensor
+	if ((!body1->IsDynamic() || (body2->IsDynamic() && inBodyPair.mBodyB < inBodyPair.mBodyA)) 
+		&& !body2->IsSensor())
 		swap(body1, body2);
-	JPH_ASSERT(body1->IsDynamic());
+	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;
@@ -955,8 +957,10 @@ void PhysicsSystem::ProcessBodyPair(const BodyPair &inBodyPair)
 
 				virtual void	AddHit(const CollideShapeResult &inResult) override
 				{
-					// Body 1 should always be dynamic, body 2 may be static / kinematic
-					JPH_ASSERT(mBody1->IsDynamic());
+					// 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()));
 					JPH_ASSERT(!ShouldEarlyOut());
 
 					// Test if we want to accept this hit

Samples/Tests/General/SensorTest.cpp

@@ -67,10 +67,28 @@ void SensorTest::Initialize()
 	mRagdoll = ragdoll_settings->CreateRagdoll(1, nullptr, mPhysicsSystem);
 	mRagdoll->SetPose(ragdoll_pose);
 	mRagdoll->AddToPhysicsSystem(EActivation::Activate);
+
+	// Create kinematic body
+	BodyCreationSettings kinematic_settings(new BoxShape(Vec3(0.25f, 0.5f, 1.0f)), Vec3(-15, 10, 0), Quat::sIdentity(), EMotionType::Kinematic, Layers::MOVING);
+	Body &kinematic = *mBodyInterface->CreateBody(kinematic_settings);
+	mKinematicBodyID = kinematic.GetID();
+	mBodyInterface->AddBody(kinematic.GetID(), EActivation::Activate);
 }
 
 void SensorTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
 {
+	// Update time
+	mTime += inParams.mDeltaTime;
+
+	// Move kinematic body
+	Vec3 kinematic_pos = Vec3(-15.0f * cos(mTime), 10, 0);
+	mBodyInterface->MoveKinematic(mKinematicBodyID, kinematic_pos, Quat::sIdentity(), inParams.mDeltaTime);
+
+	// Draw if the kinematic body is in the sensor
+	if (mKinematicBodyInSensor)
+		mDebugRenderer->DrawWireBox(mBodyInterface->GetTransformedShape(mKinematicBodyID).GetWorldSpaceBounds(), Color::sRed);
+
+	// Apply forces to dynamic bodies in sensor
 	lock_guard lock(mMutex);
 
 	Vec3 center(0, 10, 0);
@@ -115,17 +133,26 @@ void SensorTest::OnContactAdded(const Body &inBody1, const Body &inBody2, const
 	else
 		return;
 
-	// Add to list and make sure that the list remains sorted for determinism (contacts can be added from multiple threads)
 	lock_guard lock(mMutex);
-	BodyAndCount body_and_count { body_id, 1 };
-	BodiesInSensor::iterator b = lower_bound(mBodiesInSensor.begin(), mBodiesInSensor.end(), body_and_count);
-	if (b != mBodiesInSensor.end() && b->mBodyID == body_id)
+
+	if (body_id == mKinematicBodyID)
 	{
-		// This is the right body, increment reference
-		b->mCount++;
-		return;
+		JPH_ASSERT(!mKinematicBodyInSensor);
+		mKinematicBodyInSensor = true;
+	}
+	else
+	{
+		// Add to list and make sure that the list remains sorted for determinism (contacts can be added from multiple threads)
+		BodyAndCount body_and_count { body_id, 1 };
+		BodiesInSensor::iterator b = lower_bound(mBodiesInSensor.begin(), mBodiesInSensor.end(), body_and_count);
+		if (b != mBodiesInSensor.end() && b->mBodyID == body_id)
+		{
+			// This is the right body, increment reference
+			b->mCount++;
+			return;
+		}
+		mBodiesInSensor.insert(b, body_and_count);
 	}
-	mBodiesInSensor.insert(b, body_and_count);
 }
 
 void SensorTest::OnContactRemoved(const SubShapeIDPair &inSubShapePair)
@@ -139,30 +166,43 @@ void SensorTest::OnContactRemoved(const SubShapeIDPair &inSubShapePair)
 	else
 		return;
 
-	// Remove from list
 	lock_guard lock(mMutex);
-	BodyAndCount body_and_count { body_id, 1 };
-	BodiesInSensor::iterator b = lower_bound(mBodiesInSensor.begin(), mBodiesInSensor.end(), body_and_count);
-	if (b != mBodiesInSensor.end() && b->mBodyID == body_id)
-	{
-		// This is the right body, increment reference
-		JPH_ASSERT(b->mCount > 0);
-		b->mCount--;
 
-		// When last reference remove from the list
-		if (b->mCount == 0)
-			mBodiesInSensor.erase(b);
-		return;
+	if (body_id == mKinematicBodyID)
+	{
+		JPH_ASSERT(mKinematicBodyInSensor);
+		mKinematicBodyInSensor = false;
+	}
+	else
+	{
+		// Remove from list
+		BodyAndCount body_and_count { body_id, 1 };
+		BodiesInSensor::iterator b = lower_bound(mBodiesInSensor.begin(), mBodiesInSensor.end(), body_and_count);
+		if (b != mBodiesInSensor.end() && b->mBodyID == body_id)
+		{
+			// This is the right body, increment reference
+			JPH_ASSERT(b->mCount > 0);
+			b->mCount--;
+
+			// When last reference remove from the list
+			if (b->mCount == 0)
+				mBodiesInSensor.erase(b);
+			return;
+		}
+		JPH_ASSERT(false, "Body pair not found");
 	}
-	JPH_ASSERT(false, "Body pair not found");
 }
 
 void SensorTest::SaveState(StateRecorder &inStream) const
 {
+	inStream.Write(mTime);
 	inStream.Write(mBodiesInSensor);
+	inStream.Write(mKinematicBodyInSensor);
 }
 
 void SensorTest::RestoreState(StateRecorder &inStream)
 {
+	inStream.Read(mTime);
 	inStream.Read(mBodiesInSensor);
+	inStream.Read(mKinematicBodyInSensor);
 }

Samples/Tests/General/SensorTest.h

@@ -33,11 +33,15 @@ public:
 	virtual void		RestoreState(StateRecorder &inStream) override;
 
 private:
+	float				mTime = 0.0f;						// Total elapsed time
+
 	BodyID				mSensorID;							// Body ID of the sensor
 
-	Ref<Ragdoll>		mRagdoll;
+	Ref<Ragdoll>		mRagdoll;							// Ragdoll that is falling into the sensor
+
+	BodyID				mKinematicBodyID;					// Body ID of a kinematic body that is animating in and out of the sensor
 
-	Mutex				mMutex;								// Mutex that protects mBodiesInSensor
+	Mutex				mMutex;								// Mutex that protects mBodiesInSensor and mKinematicBodyInSensor
 
 	// Structure that keeps track of how many contact point each body has with the sensor
 	struct BodyAndCount
@@ -49,5 +53,7 @@ private:
 	};
 
 	using BodiesInSensor = vector<BodyAndCount>;
-	BodiesInSensor		mBodiesInSensor;					// Bodies that are currently inside the sensor
+	BodiesInSensor		mBodiesInSensor;					// Dynamic bodies that are currently inside the sensor
+
+	bool				mKinematicBodyInSensor = false;		// Keeps track if the kinematic body is in the sensor
 };

UnitTests/LoggingContactListener.h

@@ -29,8 +29,9 @@ public:
 
 	virtual ValidateResult			OnContactValidate(const Body &inBody1, const Body &inBody2, const CollideShapeResult &inCollisionResult) override
 	{
-		// Check contract that body 1 is dynamic
-		CHECK(inBody1.IsDynamic());
+		// Check contract that body 1 is dynamic or that body2 is not dynamic
+		bool contract = inBody1.IsDynamic() || !inBody2.IsDynamic();
+		CHECK(contract);
 
 		lock_guard lock(mLogMutex);
 		mLog.push_back({ EType::Validate, inBody1.GetID(), inBody2.GetID(), ContactManifold() });

UnitTests/Physics/SensorTests.cpp

@@ -0,0 +1,87 @@
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include "Layers.h"
+#include "LoggingContactListener.h"
+#include <Physics/Collision/Shape/BoxShape.h>
+
+TEST_SUITE("SensorTests")
+{
+	using LogEntry = LoggingContactListener::LogEntry;
+	using EType = LoggingContactListener::EType;
+
+	TEST_CASE("TestDynamicVsSensor")
+	{
+		PhysicsTestContext c;
+		c.ZeroGravity();
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		// Sensor
+		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(1)), Vec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+		sensor_settings.mIsSensor = true;
+		BodyID sensor_id = c.GetBodyInterface().CreateAndAddBody(sensor_settings, EActivation::DontActivate);
+
+		// Dynamic body moving downwards
+		Body &dynamic = c.CreateBox(Vec3(0, 2, 0), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(0.5f));
+		dynamic.SetLinearVelocity(Vec3(0, -1, 0));
+
+		// After a single step the dynamic object should not have touched the sensor yet
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() == 0);
+
+		// After half a second we should be touching the sensor
+		c.Simulate(0.5f);
+		CHECK(listener.Contains(EType::Add, dynamic.GetID(), sensor_id));
+		listener.Clear();
+
+		// The next step we require that the contact persists
+		c.SimulateSingleStep();
+		CHECK(listener.Contains(EType::Persist, 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));
+	}
+
+	TEST_CASE("TestKinematicVsSensor")
+	{
+		PhysicsTestContext c;
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		// Sensor
+		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(1)), Vec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+		sensor_settings.mIsSensor = true;
+		BodyID sensor_id = c.GetBodyInterface().CreateAndAddBody(sensor_settings, EActivation::DontActivate);
+
+		// Kinematic body moving downwards
+		Body &kinematic = c.CreateBox(Vec3(0, 2, 0), Quat::sIdentity(), EMotionType::Kinematic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(0.5f));
+		kinematic.SetLinearVelocity(Vec3(0, -1, 0));
+
+		// After a single step the kinematic object should not have touched the sensor yet
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() == 0);
+
+		// After half a second we should be touching the sensor
+		c.Simulate(0.5f);
+		CHECK(listener.Contains(EType::Add, kinematic.GetID(), sensor_id));
+		listener.Clear();
+
+		// The next step we require that the contact persists
+		c.SimulateSingleStep();
+		CHECK(listener.Contains(EType::Persist, 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));
+	}
+}

UnitTests/UnitTests.cmake

@@ -35,6 +35,7 @@ set(UNIT_TESTS_SRC_FILES
 	${UNIT_TESTS_ROOT}/Physics/PhysicsDeterminismTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/PhysicsStepListenerTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/RayShapeTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/SensorTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/ShapeTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/SliderConstraintTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/SubShapeIDTest.cpp