Bugfix: Kinematic vs dynamic body with inverse mass scale of 0 were still colliding (#582)

Also updated unit test to test for this behavior

Should fix issue reported in godot-jolt/godot-jolt#457

Jolt/Physics/Constraints/ContactConstraintManager.cpp

@@ -856,7 +856,8 @@ void ContactConstraintManager::GetContactsFromCache(ContactAllocator &ioContactA
 
 		JPH_ASSERT(settings.mIsSensor || !(body1->IsSensor() || body2->IsSensor()), "Sensors cannot be converted into regular bodies by a contact callback!");
 		if (!settings.mIsSensor // If one of the bodies is a sensor, don't actually create the constraint
-			&& (settings.mInvMassScale1 != 0.0f || settings.mInvMassScale2 != 0.0f)) // One of the bodies must have mass to be able to create a contact constraint
+			&& ((body1->IsDynamic() && settings.mInvMassScale1 != 0.0f) // One of the bodies must have mass to be able to create a contact constraint
+				|| (body2->IsDynamic() && settings.mInvMassScale2 != 0.0f))) 
 		{
 			// Add contact constraint in world space for the solver
 			uint32 constraint_idx = mNumConstraints++;
@@ -1044,7 +1045,8 @@ bool ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &i
 			cp.mFrictionLambda[1] = 0.0f;
 		}
 	}
-	else if (settings.mInvMassScale1 != 0.0f || settings.mInvMassScale2 != 0.0f) // One of the bodies must have mass to be able to create a contact constraint
+	else if ((inBody1.IsDynamic() && settings.mInvMassScale1 != 0.0f) // One of the bodies must have mass to be able to create a contact constraint
+			|| (inBody2.IsDynamic() && settings.mInvMassScale2 != 0.0f)) 
 	{
 		// Add contact constraint
 		uint32 constraint_idx = mNumConstraints++;

UnitTests/Physics/ContactListenerTests.cpp

@@ -423,85 +423,90 @@ TEST_SUITE("ContactListenerTests")
 
 	TEST_CASE("TestMassOverride")
 	{
-		const float cInitialVelocity1 = 2.0f;
-		const float cInitialVelocity2 = -3.0f;
-
-		for (int i = 0; i < 16; ++i)
-		{
-			PhysicsTestContext c;
-			c.ZeroGravity();
-
-			// Create two spheres on a collision course
-			BodyCreationSettings bcs(new SphereShape(1.0f), RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
-			bcs.mOverrideMassProperties = EOverrideMassProperties::CalculateInertia;
-			bcs.mMassPropertiesOverride.mMass = 1.0f;
-			bcs.mRestitution = 1.0f;
-			bcs.mLinearDamping = 0.0f;
-			bcs.mPosition = RVec3(-2, 0, 0);
-			bcs.mLinearVelocity = Vec3(cInitialVelocity1, 0, 0);
-			bcs.mUserData = i << 1;
-			Body &body1 = *c.GetBodyInterface().CreateBody(bcs);
-			c.GetBodyInterface().AddBody(body1.GetID(), EActivation::Activate);
-
-			bcs.mMassPropertiesOverride.mMass = 2.0f;
-			bcs.mPosition = RVec3(2, 0, 0);
-			bcs.mLinearVelocity = Vec3(cInitialVelocity2, 0, 0);
-			bcs.mUserData++;
-			Body &body2 = *c.GetBodyInterface().CreateBody(bcs);
-			c.GetBodyInterface().AddBody(body2.GetID(), EActivation::Activate);
-
-			// Contact listener that modifies mass
-			class ContactListenerImpl : public ContactListener
-			{
-			public:
-				virtual void	OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
-				{
-					// Override the mass of body 1
-					float scale1 = sGetInvMassScale(inBody1);
-					ioSettings.mInvMassScale1 = scale1;
-					ioSettings.mInvInertiaScale1 = scale1;
-
-					// Override the mass of body 2
-					float scale2 = sGetInvMassScale(inBody2);
-					ioSettings.mInvMassScale2 = scale2;
-					ioSettings.mInvInertiaScale2 = scale2;
-				}
-
-				virtual void	OnContactPersisted(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
-				{
-					OnContactAdded(inBody1, inBody2, inManifold, ioSettings);
-				}
-			};
-
-			// Set listener
-			ContactListenerImpl listener;
-			c.GetSystem()->SetContactListener(&listener);
-
-			// Simulate
-			c.Simulate(1.0f);
-
-			// Calculate resulting inverse mass
-			float inv_m1 = sGetInvMassScale(body1) * body1.GetMotionProperties()->GetInverseMass();
-			float inv_m2 = sGetInvMassScale(body2) * body2.GetMotionProperties()->GetInverseMass();
-
-			float v1, v2;
-			if (inv_m1 == 0.0f && inv_m2 == 0.0f)
-			{
-				// If both bodies became kinematic they will pass through each other
-				v1 = cInitialVelocity1;
-				v2 = cInitialVelocity2;
-			}
-			else
-			{
-				// Calculate resulting velocity using conservation of momentum and energy
-				// See: https://en.wikipedia.org/wiki/Elastic_collision where m1 = 1 / inv_m1 and m2 = 1 / inv_m2
-				v1 = (2.0f * inv_m1 * cInitialVelocity2 + (inv_m2 - inv_m1) * cInitialVelocity1) / (inv_m1 + inv_m2);
-				v2 = (2.0f * inv_m2 * cInitialVelocity1 + (inv_m1 - inv_m2) * cInitialVelocity2) / (inv_m1 + inv_m2);
-			}
-
-			// Check that the spheres move according to their overridden masses
-			CHECK_APPROX_EQUAL(body1.GetLinearVelocity(), Vec3(v1, 0, 0));
-			CHECK_APPROX_EQUAL(body2.GetLinearVelocity(), Vec3(v2, 0, 0));
-		}
+		for (EMotionType m1 = EMotionType::Static; m1 <= EMotionType::Dynamic; m1 = EMotionType((int)m1 + 1))
+			for (EMotionType m2 = EMotionType::Static; m2 <= EMotionType::Dynamic; m2 = EMotionType((int)m2 + 1))
+				if (m1 != EMotionType::Static || m2 != EMotionType::Static)
+					for (int i = 0; i < 16; ++i)
+					{
+						PhysicsTestContext c;
+						c.ZeroGravity();
+
+						const float cInitialVelocity1 = m1 != EMotionType::Static? 3.0f : 0.0f;
+						const float cInitialVelocity2 = m2 != EMotionType::Static? -4.0f : 0.0f;
+
+						// Create two spheres on a collision course
+						BodyCreationSettings bcs(new SphereShape(1.0f), RVec3::sZero(), Quat::sIdentity(), m1, m1 != EMotionType::Static? Layers::MOVING : Layers::NON_MOVING);
+						bcs.mOverrideMassProperties = EOverrideMassProperties::CalculateInertia;
+						bcs.mMassPropertiesOverride.mMass = 1.0f;
+						bcs.mRestitution = 1.0f;
+						bcs.mLinearDamping = 0.0f;
+						bcs.mPosition = RVec3(-2, 0, 0);
+						bcs.mLinearVelocity = Vec3(cInitialVelocity1, 0, 0);
+						bcs.mUserData = i << 1;
+						Body &body1 = *c.GetBodyInterface().CreateBody(bcs);
+						c.GetBodyInterface().AddBody(body1.GetID(), EActivation::Activate);
+
+						bcs.mMotionType = m2;
+						bcs.mObjectLayer = m2 != EMotionType::Static? Layers::MOVING : Layers::NON_MOVING;
+						bcs.mMassPropertiesOverride.mMass = 2.0f;
+						bcs.mPosition = RVec3(2, 0, 0);
+						bcs.mLinearVelocity = Vec3(cInitialVelocity2, 0, 0);
+						bcs.mUserData++;
+						Body &body2 = *c.GetBodyInterface().CreateBody(bcs);
+						c.GetBodyInterface().AddBody(body2.GetID(), EActivation::Activate);
+
+						// Contact listener that modifies mass
+						class ContactListenerImpl : public ContactListener
+						{
+						public:
+							virtual void	OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+							{
+								// Override the mass of body 1
+								float scale1 = sGetInvMassScale(inBody1);
+								ioSettings.mInvMassScale1 = scale1;
+								ioSettings.mInvInertiaScale1 = scale1;
+
+								// Override the mass of body 2
+								float scale2 = sGetInvMassScale(inBody2);
+								ioSettings.mInvMassScale2 = scale2;
+								ioSettings.mInvInertiaScale2 = scale2;
+							}
+
+							virtual void	OnContactPersisted(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+							{
+								OnContactAdded(inBody1, inBody2, inManifold, ioSettings);
+							}
+						};
+
+						// Set listener
+						ContactListenerImpl listener;
+						c.GetSystem()->SetContactListener(&listener);
+
+						// Simulate
+						c.Simulate(1.0f);
+
+						// Calculate resulting inverse mass
+						float inv_m1 = body1.GetMotionType() == EMotionType::Dynamic? sGetInvMassScale(body1) * body1.GetMotionProperties()->GetInverseMass() : 0.0f;
+						float inv_m2 = body2.GetMotionType() == EMotionType::Dynamic? sGetInvMassScale(body2) * body2.GetMotionProperties()->GetInverseMass() : 0.0f;
+
+						float v1, v2;
+						if (inv_m1 == 0.0f && inv_m2 == 0.0f)
+						{
+							// If both bodies became kinematic they will pass through each other
+							v1 = cInitialVelocity1;
+							v2 = cInitialVelocity2;
+						}
+						else
+						{
+							// Calculate resulting velocity using conservation of momentum and energy
+							// See: https://en.wikipedia.org/wiki/Elastic_collision where m1 = 1 / inv_m1 and m2 = 1 / inv_m2
+							v1 = (2.0f * inv_m1 * cInitialVelocity2 + (inv_m2 - inv_m1) * cInitialVelocity1) / (inv_m1 + inv_m2);
+							v2 = (2.0f * inv_m2 * cInitialVelocity1 + (inv_m1 - inv_m2) * cInitialVelocity2) / (inv_m1 + inv_m2);
+						}
+
+						// Check that the spheres move according to their overridden masses
+						CHECK_APPROX_EQUAL(body1.GetLinearVelocity(), Vec3(v1, 0, 0));
+						CHECK_APPROX_EQUAL(body2.GetLinearVelocity(), Vec3(v2, 0, 0));
+					}
 	}
 }