Fixed mass scaling not applied for CCD objects & during solve position constraints (#755)

Fixes #753

Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h

@@ -148,17 +148,18 @@ class AxisConstraintPart
 		case EMotionType::Dynamic:
 			{
 				const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked();
+				float invm1 = mp1->GetInverseMass();
 				Mat44 invi1 = inBody1.GetInverseInertia();
 				switch (inBody2.GetMotionType())
 				{
 				case EMotionType::Dynamic:
-					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Dynamic>(mp1->GetInverseMass(), invi1, inR1PlusU, inBody2.GetMotionPropertiesUnchecked()->GetInverseMass(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
+					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Dynamic>(invm1, invi1, inR1PlusU, inBody2.GetMotionPropertiesUnchecked()->GetInverseMass(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
 
 				case EMotionType::Kinematic:
-					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Kinematic>(mp1->GetInverseMass(), invi1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis);
+					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Kinematic>(invm1, invi1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis);
 
 				case EMotionType::Static:
-					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Static>(mp1->GetInverseMass(), invi1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis);
+					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Static>(invm1, invi1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis);
 
 				default:
 					break;
@@ -182,6 +183,50 @@ class AxisConstraintPart
 		return 0.0f;
 	}
 
+	/// Internal helper function to calculate the inverse effective mass, version that supports mass scaling
+	JPH_INLINE float			CalculateInverseEffectiveMassWithMassScale(const Body &inBody1, float inInvMassScale1, float inInvInertiaScale1, Vec3Arg inR1PlusU, const Body &inBody2, float inInvMassScale2, float inInvInertiaScale2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis)
+	{
+		// Dispatch to the correct templated form
+		switch (inBody1.GetMotionType())
+		{
+		case EMotionType::Dynamic:
+			{
+				const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked();
+				float invm1 = inInvMassScale1 * mp1->GetInverseMass();
+				Mat44 invi1 = inInvInertiaScale1 * inBody1.GetInverseInertia();
+				switch (inBody2.GetMotionType())
+				{
+				case EMotionType::Dynamic:
+					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Dynamic>(invm1, invi1, inR1PlusU, inInvMassScale2 * inBody2.GetMotionPropertiesUnchecked()->GetInverseMass(), inInvInertiaScale2 * inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
+
+				case EMotionType::Kinematic:
+					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Kinematic>(invm1, invi1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis);
+
+				case EMotionType::Static:
+					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Static>(invm1, invi1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis);
+
+				default:
+					break;
+				}
+				break;
+			}
+
+		case EMotionType::Kinematic:
+			JPH_ASSERT(inBody2.IsDynamic());
+			return TemplatedCalculateInverseEffectiveMass<EMotionType::Kinematic, EMotionType::Dynamic>(0 /* Will not be used */, Mat44() /* Will not be used */, inR1PlusU, inInvMassScale2 * inBody2.GetMotionPropertiesUnchecked()->GetInverseMass(), inInvInertiaScale2 * inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
+
+		case EMotionType::Static:
+			JPH_ASSERT(inBody2.IsDynamic());
+			return TemplatedCalculateInverseEffectiveMass<EMotionType::Static, EMotionType::Dynamic>(0 /* Will not be used */, Mat44() /* Will not be used */, inR1PlusU, inInvMassScale2 * inBody2.GetMotionPropertiesUnchecked()->GetInverseMass(), inInvInertiaScale2 * inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
+
+		default:
+			break;
+		}
+
+		JPH_ASSERT(false);
+		return 0.0f;
+	}
+
 public:
 	/// Templated form of CalculateConstraintProperties with the motion types baked in
 	template <EMotionType Type1, EMotionType Type2>
@@ -214,6 +259,30 @@ public:
 		}
 	}
 
+	/// Calculate properties used during the functions below, version that supports mass scaling
+	/// @param inBody1 The first body that this constraint is attached to
+	/// @param inBody2 The second body that this constraint is attached to
+	/// @param inInvMassScale1 Scale factor for the inverse mass of body 1
+	/// @param inInvMassScale2 Scale factor for the inverse mass of body 2
+	/// @param inInvInertiaScale1 Scale factor for the inverse inertia of body 1
+	/// @param inInvInertiaScale2 Scale factor for the inverse inertia of body 2
+	/// @param inR1PlusU See equations above (r1 + u)
+	/// @param inR2 See equations above (r2)
+	/// @param inWorldSpaceAxis Axis along which the constraint acts (normalized, pointing from body 1 to 2)
+	/// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b
+	inline void					CalculateConstraintPropertiesWithMassScale(const Body &inBody1, float inInvMassScale1, float inInvInertiaScale1, Vec3Arg inR1PlusU, const Body &inBody2, float inInvMassScale2, float inInvInertiaScale2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f)
+	{
+		float inv_effective_mass = CalculateInverseEffectiveMassWithMassScale(inBody1, inInvMassScale1, inInvInertiaScale1, inR1PlusU, inBody2, inInvMassScale2, inInvInertiaScale2, inR2, inWorldSpaceAxis);
+
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else
+		{
+			mEffectiveMass = 1.0f / inv_effective_mass;
+			mSpringPart.CalculateSpringPropertiesWithBias(inBias);
+		}
+	}
+
 	/// Calculate properties used during the functions below
 	/// @param inDeltaTime Time step
 	/// @param inBody1 The first body that this constraint is attached to

Jolt/Physics/Constraints/ContactConstraintManager.cpp

@@ -33,14 +33,14 @@ bool ContactConstraintManager::sDrawContactManifolds = false;
 // ContactConstraintManager::WorldContactPoint
 ////////////////////////////////////////////////////////////////////////////////////////////////////////
 
-void ContactConstraintManager::WorldContactPoint::CalculateNonPenetrationConstraintProperties(const Body &inBody1, const Body &inBody2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal)
+void ContactConstraintManager::WorldContactPoint::CalculateNonPenetrationConstraintProperties(const Body &inBody1, float inInvMassScale1, float inInvInertiaScale1, const Body &inBody2, float inInvMassScale2, float inInvInertiaScale2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal)
 {
 	// Calculate collision points relative to body
 	RVec3 p = 0.5_r * (inWorldSpacePosition1 + inWorldSpacePosition2);
 	Vec3 r1 = Vec3(p - inBody1.GetCenterOfMassPosition());
 	Vec3 r2 = Vec3(p - inBody2.GetCenterOfMassPosition());
 
-	mNonPenetrationConstraint.CalculateConstraintProperties(inBody1, r1, inBody2, r2, inWorldSpaceNormal);
+	mNonPenetrationConstraint.CalculateConstraintPropertiesWithMassScale(inBody1, inInvMassScale1, inInvInertiaScale1, r1, inBody2, inInvMassScale2, inInvInertiaScale2, r2, inWorldSpaceNormal);
 }
 
 template <EMotionType Type1, EMotionType Type2>
@@ -894,6 +894,10 @@ void ContactConstraintManager::GetContactsFromCache(ContactAllocator &ioContactA
 			constraint.mSortKey = input_hash;
 			world_space_normal.StoreFloat3(&constraint.mWorldSpaceNormal);
 			constraint.mCombinedFriction = settings.mCombinedFriction;
+			constraint.mInvMassScale1 = settings.mInvMassScale1;
+			constraint.mInvInertiaScale1 = settings.mInvInertiaScale1;
+			constraint.mInvMassScale2 = settings.mInvMassScale2;
+			constraint.mInvInertiaScale2 = settings.mInvInertiaScale2;
 			constraint.mContactPoints.resize(output_cm->mNumContactPoints);
 			for (uint32 i = 0; i < output_cm->mNumContactPoints; ++i)
 			{
@@ -1085,6 +1089,10 @@ bool ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &i
 		constraint.mSortKey = key_hash;
 		inManifold.mWorldSpaceNormal.StoreFloat3(&constraint.mWorldSpaceNormal);
 		constraint.mCombinedFriction = settings.mCombinedFriction;
+		constraint.mInvMassScale1 = settings.mInvMassScale1;
+		constraint.mInvInertiaScale1 = settings.mInvInertiaScale1;
+		constraint.mInvMassScale2 = settings.mInvMassScale2;
+		constraint.mInvInertiaScale2 = settings.mInvInertiaScale2;
 
 		JPH_DET_LOG("TemplatedAddContactConstraint: id1: " << constraint.mBody1->GetID() << " id2: " << constraint.mBody2->GetID() << " key: " << constraint.mSortKey);
 
@@ -1350,6 +1358,14 @@ void ContactConstraintManager::OnCCDContactAdded(ContactAllocator &ioContactAllo
 			// Note that we can trigger OnContactPersisted multiple times per physics update, but otherwise we have no way of obtaining the settings
 			mContactListener->OnContactPersisted(*body1, *body2, *manifold, outSettings);
 		}
+
+		// If we swapped body1 and body2 we need to swap the mass scales back
+		if (manifold == &temp)
+		{
+			swap(outSettings.mInvMassScale1, outSettings.mInvMassScale2);
+			swap(outSettings.mInvInertiaScale1, outSettings.mInvInertiaScale2);
+			// Note we do not need to negate the relative surface velocity as it is not applied by the CCD collision constraint
+		}
 	}
 
 	JPH_ASSERT(outSettings.mIsSensor || !(inBody1.IsSensor() || inBody2.IsSensor()), "Sensors cannot be converted into regular bodies by a contact callback!");
@@ -1650,7 +1666,7 @@ bool ContactConstraintManager::SolvePositionConstraints(const uint32 *inConstrai
 			if (separation < 0.0f)
 			{
 				// Update constraint properties (bodies may have moved)
-				wcp.CalculateNonPenetrationConstraintProperties(body1, body2, p1, p2, ws_normal);
+				wcp.CalculateNonPenetrationConstraintProperties(body1, constraint.mInvMassScale1, constraint.mInvInertiaScale1, body2, constraint.mInvMassScale2, constraint.mInvInertiaScale2, p1, p2, ws_normal);
 
 				// Solve position errors
 				if (wcp.mNonPenetrationConstraint.SolvePositionConstraint(body1, body2, ws_normal, separation, mPhysicsSettings.mBaumgarte))

Jolt/Physics/Constraints/ContactConstraintManager.h

@@ -420,7 +420,7 @@ private:
 	{
 	public:
 		/// Calculate constraint properties below
-		void					CalculateNonPenetrationConstraintProperties(const Body &inBody1, const Body &inBody2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal);
+		void					CalculateNonPenetrationConstraintProperties(const Body &inBody1, float inInvMassScale1, float inInvInertiaScale1, const Body &inBody2, float inInvMassScale2, float inInvInertiaScale2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal);
 
 		template <EMotionType Type1, EMotionType Type2>
 		JPH_INLINE void			TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(float inDeltaTime, const Body &inBody1, const Body &inBody2, float inInvM1, float inInvM2, Mat44Arg inInvI1, Mat44Arg inInvI2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal, Vec3Arg inWorldSpaceTangent1, Vec3Arg inWorldSpaceTangent2, const ContactSettings &inSettings, float inMinVelocityForRestitution);
@@ -464,6 +464,10 @@ private:
 		uint64					mSortKey;
 		Float3					mWorldSpaceNormal;
 		float					mCombinedFriction;
+		float					mInvMassScale1;
+		float					mInvInertiaScale1;
+		float					mInvMassScale2;
+		float					mInvInertiaScale2;
 		WorldContactPoints		mContactPoints;
 	};
 

Jolt/Physics/PhysicsSystem.cpp

@@ -1984,18 +1984,19 @@ void PhysicsSystem::JobResolveCCDContacts(PhysicsUpdateContext *ioContext, Physi
 
 					// Calculate velocity bias due to restitution
 					float normal_velocity_bias;
-					if (ccd_body->mContactSettings.mCombinedRestitution > 0.0f && normal_velocity < -mPhysicsSettings.mMinVelocityForRestitution)
-						normal_velocity_bias = ccd_body->mContactSettings.mCombinedRestitution * normal_velocity;
+					const ContactSettings &contact_settings = ccd_body->mContactSettings;
+					if (contact_settings.mCombinedRestitution > 0.0f && normal_velocity < -mPhysicsSettings.mMinVelocityForRestitution)
+						normal_velocity_bias = contact_settings.mCombinedRestitution * normal_velocity;
 					else
 						normal_velocity_bias = 0.0f;
 
 					// Solve contact constraint
 					AxisConstraintPart contact_constraint;
-					contact_constraint.CalculateConstraintProperties(body1, r1_plus_u, body2, r2, ccd_body->mContactNormal, normal_velocity_bias);
+					contact_constraint.CalculateConstraintPropertiesWithMassScale(body1, contact_settings.mInvMassScale1, contact_settings.mInvInertiaScale1, r1_plus_u, body2, contact_settings.mInvMassScale2, contact_settings.mInvInertiaScale2, r2, ccd_body->mContactNormal, normal_velocity_bias);
 					contact_constraint.SolveVelocityConstraint(body1, body2, ccd_body->mContactNormal, -FLT_MAX, FLT_MAX);
 
 					// Apply friction
-					if (ccd_body->mContactSettings.mCombinedFriction > 0.0f)
+					if (contact_settings.mCombinedFriction > 0.0f)
 					{
 						// Calculate friction direction by removing normal velocity from the relative velocity
 						Vec3 friction_direction = relative_velocity - normal_velocity * ccd_body->mContactNormal;
@@ -2006,10 +2007,10 @@ void PhysicsSystem::JobResolveCCDContacts(PhysicsUpdateContext *ioContext, Physi
 							friction_direction /= sqrt(friction_direction_len_sq);
 
 							// Calculate max friction impulse
-							float max_lambda_f = ccd_body->mContactSettings.mCombinedFriction * contact_constraint.GetTotalLambda();
+							float max_lambda_f = contact_settings.mCombinedFriction * contact_constraint.GetTotalLambda();
 
 							AxisConstraintPart friction;
-							friction.CalculateConstraintProperties(body1, r1_plus_u, body2, r2, friction_direction);
+							friction.CalculateConstraintPropertiesWithMassScale(body1, contact_settings.mInvMassScale1, contact_settings.mInvInertiaScale1, r1_plus_u, body2, contact_settings.mInvMassScale2, contact_settings.mInvInertiaScale2, r2, friction_direction);
 							friction.SolveVelocityConstraint(body1, body2, friction_direction, -max_lambda_f, max_lambda_f);
 						}
 					}

UnitTests/Physics/ContactListenerTests.cpp

@@ -518,4 +518,88 @@ TEST_SUITE("ContactListenerTests")
 						CHECK_APPROX_EQUAL(body2.GetLinearVelocity(), Vec3(v2, 0, 0));
 					}
 	}
+
+	TEST_CASE("TestInfiniteMassOverride")
+	{
+		for (bool do_swap : { false, true })
+			for (EMotionQuality quality : { EMotionQuality::Discrete, EMotionQuality::LinearCast })
+			{
+				// A contact listener that makes a body have infinite mass
+				class ContactListenerImpl : public ContactListener
+				{
+				public:
+									ContactListenerImpl(const BodyID &inBodyID) : mBodyID(inBodyID) { }
+
+					virtual void	OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+					{
+						if (mBodyID == inBody1.GetID())
+						{
+							ioSettings.mInvInertiaScale1 = 0.0f;
+							ioSettings.mInvMassScale1 = 0.0f;
+						}
+						else if (mBodyID == inBody2.GetID())
+						{
+							ioSettings.mInvInertiaScale2 = 0.0f;
+							ioSettings.mInvMassScale2 = 0.0f;
+						}
+					}
+
+					virtual void	OnContactPersisted(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+					{
+						OnContactAdded(inBody1, inBody2, inManifold, ioSettings);
+					}
+
+				private:
+					BodyID			mBodyID;
+				};
+
+				PhysicsTestContext c;
+				c.ZeroGravity();
+
+				// Create a box
+				const RVec3 cInitialBoxPos(0, 2, 0);
+				BodyCreationSettings box_settings(new BoxShape(Vec3::sReplicate(2)), cInitialBoxPos, Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+				box_settings.mOverrideMassProperties = EOverrideMassProperties::CalculateInertia;
+				box_settings.mMassPropertiesOverride.mMass = 1.0f;
+
+				// Create a sphere
+				BodyCreationSettings sphere_settings(new SphereShape(2), RVec3(30, 2, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+				sphere_settings.mLinearVelocity = Vec3(-100, 0, 0);
+				sphere_settings.mOverrideMassProperties = EOverrideMassProperties::CalculateInertia;
+				sphere_settings.mMassPropertiesOverride.mMass = 10.0f;
+				sphere_settings.mRestitution = 0.1f;
+				sphere_settings.mLinearDamping = 0.0f;
+				sphere_settings.mMotionQuality = quality;
+
+				BodyID box_id, sphere_id;
+				if (do_swap)
+				{
+					// Swap the bodies so that the contact listener will receive the bodies in the opposite order
+					sphere_id = c.GetBodyInterface().CreateAndAddBody(sphere_settings, EActivation::Activate);
+					box_id = c.GetBodyInterface().CreateAndAddBody(box_settings, EActivation::Activate);
+				}
+				else
+				{
+					box_id = c.GetBodyInterface().CreateAndAddBody(box_settings, EActivation::Activate);
+					sphere_id = c.GetBodyInterface().CreateAndAddBody(sphere_settings, EActivation::Activate);
+				}
+
+				// Add listener that will make the box have infinite mass
+				ContactListenerImpl listener(box_id);
+				c.GetSystem()->SetContactListener(&listener);
+
+				// Simulate
+				const float cSimulationTime = 0.3f;
+				c.Simulate(cSimulationTime);
+
+				// Check that the box didn't move
+				BodyInterface &bi = c.GetBodyInterface();
+				CHECK(bi.GetPosition(box_id) == cInitialBoxPos);
+				CHECK(bi.GetLinearVelocity(box_id) == Vec3::sZero());
+				CHECK(bi.GetAngularVelocity(box_id) == Vec3::sZero());
+
+				// Check that the sphere bounced off the box
+				CHECK_APPROX_EQUAL(bi.GetLinearVelocity(sphere_id), -sphere_settings.mLinearVelocity * sphere_settings.mRestitution);
+			}
+		}
 }