Ability to override mass and inertia of bodies from the ContactListener (#580)

Added mInvMassScale1/2 and mInvInertiaScale1/2 to ContactSettings which allow overriding the masses for two bodies when they collide.

Note that this currently does not work if you use multiple integration sub steps in PhysicsSystem::Update and warm starting the constraint will produce an incorrect result if the mass changes while the bodies are in contact, which may result in simulation artifacts.

Fixes #580

Jolt/Math/DMat44.h

@@ -114,6 +114,9 @@ public:
 	JPH_INLINE Vec4				GetColumn4(uint inCol) const							{ JPH_ASSERT(inCol < 3); return mCol[inCol]; }
 	JPH_INLINE void				SetColumn4(uint inCol, Vec4Arg inV)						{ JPH_ASSERT(inCol < 3); mCol[inCol] = inV; }
 
+	/// Transpose 3x3 subpart of matrix
+	JPH_INLINE Mat44			Transposed3x3() const									{ return GetRotation().Transposed3x3(); }
+
 	/// Inverse 4x4 matrix
 	JPH_INLINE DMat44			Inversed() const;
 

Jolt/Physics/Body/BodyInterface.cpp

@@ -898,6 +898,13 @@ uint64 BodyInterface::GetUserData(const BodyID &inBodyID) const
 		return 0;
 }
 
+void BodyInterface::SetUserData(const BodyID &inBodyID, uint64 inUserData) const
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+		lock.GetBody().SetUserData(inUserData);
+}
+
 const PhysicsMaterial *BodyInterface::GetMaterial(const BodyID &inBodyID, const SubShapeID &inSubShapeID) const
 {
 	BodyLockRead lock(*mBodyLockInterface, inBodyID);

Jolt/Physics/Body/BodyInterface.h

@@ -231,6 +231,7 @@ public:
 
 	/// Get the user data for a body
 	uint64						GetUserData(const BodyID &inBodyID) const;
+	void						SetUserData(const BodyID &inBodyID, uint64 inUserData) const;
 
 	/// Get the material for a particular sub shape
 	const PhysicsMaterial *		GetMaterial(const BodyID &inBodyID, const SubShapeID &inSubShapeID) const;

Jolt/Physics/Collision/ContactListener.h

@@ -42,6 +42,10 @@ class ContactSettings
 public:
 	float					mCombinedFriction;					///< Combined friction for the body pair (see: PhysicsSystem::SetCombineFriction)
 	float					mCombinedRestitution;				///< Combined restitution for the body pair (see: PhysicsSystem::SetCombineRestitution)
+	float					mInvMassScale1 = 1.0f;				///< Scale factor for the inverse mass of body 1 (0 = infinite mass, 1 = use original mass, 2 = body has half the mass). For the same contact pair, you should strive to keep the value the same over time.
+	float					mInvInertiaScale1 = 1.0f;			///< Scale factor for the inverse inertia of body 1 (usually same as mInvMassScale1)
+	float					mInvMassScale2 = 1.0f;				///< Scale factor for the inverse mass of body 2 (0 = infinite mass, 1 = use original mass, 2 = body has half the mass). For the same contact pair, you should strive to keep the value the same over time.
+	float					mInvInertiaScale2 = 1.0f;			///< Scale factor for the inverse inertia of body 2 (usually same as mInvMassScale2)
 	bool					mIsSensor;							///< If the contact should be treated as a sensor vs body contact (no collision response)
 	Vec3					mRelativeSurfaceVelocity = Vec3::sZero(); ///< Relative surface velocity between the bodies (world space surface velocity of body 2 - world space surface velocity of body 1), can be used to create a conveyor belt effect
 };

Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h

@@ -57,12 +57,12 @@ class AxisConstraintPart
 			// v' = v + M^-1 P
 			if constexpr (Type1 == EMotionType::Dynamic)
 			{
-				ioMotionProperties1->SubLinearVelocityStep((inLambda * ioMotionProperties1->GetInverseMass()) * inWorldSpaceAxis);
+				ioMotionProperties1->SubLinearVelocityStep((inLambda * mInverseMass1) * inWorldSpaceAxis);
 				ioMotionProperties1->SubAngularVelocityStep(inLambda * Vec3::sLoadFloat3Unsafe(mInvI1_R1PlusUxAxis));
 			}
 			if constexpr (Type2 == EMotionType::Dynamic)
 			{
-				ioMotionProperties2->AddLinearVelocityStep((inLambda * ioMotionProperties2->GetInverseMass()) * inWorldSpaceAxis);
+				ioMotionProperties2->AddLinearVelocityStep((inLambda * mInverseMass2) * inWorldSpaceAxis);
 				ioMotionProperties2->AddAngularVelocityStep(inLambda * Vec3::sLoadFloat3Unsafe(mInvI2_R2xAxis));
 			}
 			return true;
@@ -73,7 +73,7 @@ class AxisConstraintPart
 
 	/// Internal helper function to calculate the inverse effective mass
 	template <EMotionType Type1, EMotionType Type2>
-	JPH_INLINE float			TemplatedCalculateInverseEffectiveMass(const MotionProperties *inMotionProperties1, Mat44Arg inInvI1, Vec3Arg inR1PlusU, const MotionProperties *inMotionProperties2, Mat44Arg inInvI2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis)
+	JPH_INLINE float			TemplatedCalculateInverseEffectiveMass(float inInvMass1, Mat44Arg inInvI1, Vec3Arg inR1PlusU, float inInvMass2, Mat44Arg inInvI2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis)
 	{
 		JPH_ASSERT(inWorldSpaceAxis.IsNormalized(1.0e-5f));
 
@@ -81,18 +81,32 @@ class AxisConstraintPart
 		Vec3 r1_plus_u_x_axis;
 		if constexpr (Type1 != EMotionType::Static)
 		{
+			mInverseMass1 = inInvMass1;
 			r1_plus_u_x_axis = inR1PlusU.Cross(inWorldSpaceAxis);
 			r1_plus_u_x_axis.StoreFloat3(&mR1PlusUxAxis);
 		}
-			JPH_IF_DEBUG(else Vec3::sNaN().StoreFloat3(&mR1PlusUxAxis);)
+		else
+		{
+		#ifdef _DEBUG
+			Vec3::sNaN().StoreFloat3(&mR1PlusUxAxis);
+			mInverseMass1 = numeric_limits<float>::quiet_NaN();
+		#endif
+		}
 
 		Vec3 r2_x_axis;
 		if constexpr (Type2 != EMotionType::Static)
 		{
+			mInverseMass2 = inInvMass2;
 			r2_x_axis = inR2.Cross(inWorldSpaceAxis);
 			r2_x_axis.StoreFloat3(&mR2xAxis);
 		}		
-			JPH_IF_DEBUG(else Vec3::sNaN().StoreFloat3(&mR2xAxis);)
+		else
+		{
+		#ifdef _DEBUG
+			Vec3::sNaN().StoreFloat3(&mR2xAxis);
+			mInverseMass2 = numeric_limits<float>::quiet_NaN();
+		#endif
+		}
 
 		// Calculate inverse effective mass: K = J M^-1 J^T
 		float inv_effective_mass;
@@ -101,7 +115,7 @@ class AxisConstraintPart
 		{
 			Vec3 invi1_r1_plus_u_x_axis = inInvI1.Multiply3x3(r1_plus_u_x_axis);
 			invi1_r1_plus_u_x_axis.StoreFloat3(&mInvI1_R1PlusUxAxis);
-			inv_effective_mass = inMotionProperties1->GetInverseMass() + invi1_r1_plus_u_x_axis.Dot(r1_plus_u_x_axis);
+			inv_effective_mass = mInverseMass1 + invi1_r1_plus_u_x_axis.Dot(r1_plus_u_x_axis);
 		}
 		else
 		{
@@ -114,7 +128,7 @@ class AxisConstraintPart
 		{
 			Vec3 invi2_r2_x_axis = inInvI2.Multiply3x3(r2_x_axis);
 			invi2_r2_x_axis.StoreFloat3(&mInvI2_R2xAxis);
-			inv_effective_mass += inMotionProperties2->GetInverseMass() + invi2_r2_x_axis.Dot(r2_x_axis);
+			inv_effective_mass += mInverseMass2 + invi2_r2_x_axis.Dot(r2_x_axis);
 		}
 		else
 		{
@@ -138,13 +152,13 @@ class AxisConstraintPart
 				switch (inBody2.GetMotionType())
 				{
 				case EMotionType::Dynamic:
-					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Dynamic>(mp1, invi1, inR1PlusU, inBody2.GetMotionPropertiesUnchecked(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
+					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Dynamic>(mp1->GetInverseMass(), invi1, inR1PlusU, inBody2.GetMotionPropertiesUnchecked()->GetInverseMass(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
 
 				case EMotionType::Kinematic:
-					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Kinematic>(mp1, invi1, inR1PlusU, nullptr, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis);
+					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Kinematic>(mp1->GetInverseMass(), invi1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis);
 
 				case EMotionType::Static:
-					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Static>(mp1, invi1, inR1PlusU, nullptr, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis);
+					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Static>(mp1->GetInverseMass(), invi1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis);
 
 				default:
 					break;
@@ -154,11 +168,11 @@ class AxisConstraintPart
 
 		case EMotionType::Kinematic:
 			JPH_ASSERT(inBody2.IsDynamic());
-			return TemplatedCalculateInverseEffectiveMass<EMotionType::Kinematic, EMotionType::Dynamic>(nullptr, Mat44() /* Will not be used */, inR1PlusU, inBody2.GetMotionPropertiesUnchecked(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
+			return TemplatedCalculateInverseEffectiveMass<EMotionType::Kinematic, EMotionType::Dynamic>(0 /* Will not be used */, Mat44() /* Will not be used */, inR1PlusU, inBody2.GetMotionPropertiesUnchecked()->GetInverseMass(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
 
 		case EMotionType::Static:
 			JPH_ASSERT(inBody2.IsDynamic());
-			return TemplatedCalculateInverseEffectiveMass<EMotionType::Static, EMotionType::Dynamic>(nullptr, Mat44() /* Will not be used */, inR1PlusU, inBody2.GetMotionPropertiesUnchecked(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
+			return TemplatedCalculateInverseEffectiveMass<EMotionType::Static, EMotionType::Dynamic>(0 /* Will not be used */, Mat44() /* Will not be used */, inR1PlusU, inBody2.GetMotionPropertiesUnchecked()->GetInverseMass(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
 
 		default:
 			break;
@@ -171,13 +185,13 @@ class AxisConstraintPart
 public:
 	/// Templated form of CalculateConstraintProperties with the motion types baked in
 	template <EMotionType Type1, EMotionType Type2>
-	JPH_INLINE void				TemplatedCalculateConstraintProperties(const MotionProperties *inMotionProperties1, Mat44Arg inInvI1, Vec3Arg inR1PlusU, const MotionProperties *inMotionProperties2, Mat44Arg inInvI2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f)
+	JPH_INLINE void				TemplatedCalculateConstraintProperties(float inInvMass1, Mat44Arg inInvI1, Vec3Arg inR1PlusU, float inInvMass2, Mat44Arg inInvI2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f)
 	{
-		mEffectiveMass = 1.0f / TemplatedCalculateInverseEffectiveMass<Type1, Type2>(inMotionProperties1, inInvI1, inR1PlusU, inMotionProperties2, inInvI2, inR2, inWorldSpaceAxis);
+		mEffectiveMass = 1.0f / TemplatedCalculateInverseEffectiveMass<Type1, Type2>(inInvMass1, inInvI1, inR1PlusU, inInvMass2, inInvI2, inR2, inWorldSpaceAxis);
 
 		mSpringPart.CalculateSpringPropertiesWithBias(inBias);
 
-		JPH_DET_LOG("TemplatedCalculateConstraintProperties: invI1: " << inInvI1 << " r1PlusU: " << inR1PlusU << " invI2: " << inInvI2 << " r2: " << inR2 << " bias: " << inBias << " r1PlusUxAxis: " << mR1PlusUxAxis << " r2xAxis: " << mR2xAxis << " invI1_R1PlusUxAxis: " << mInvI1_R1PlusUxAxis << " invI2_R2xAxis: " << mInvI2_R2xAxis << " effectiveMass: " << mEffectiveMass << " totalLambda: " << mTotalLambda);
+		JPH_DET_LOG("TemplatedCalculateConstraintProperties: invM1: " << inInvMass1 << " invI1: " << inInvI1 << " r1PlusU: " << inR1PlusU << " invM2: " << inInvMass2 << " invI2: " << inInvI2 << " r2: " << inR2 << " bias: " << inBias << " r1PlusUxAxis: " << mR1PlusUxAxis << " r2xAxis: " << mR2xAxis << " invI1_R1PlusUxAxis: " << mInvI1_R1PlusUxAxis << " invI2_R2xAxis: " << mInvI2_R2xAxis << " effectiveMass: " << mEffectiveMass << " totalLambda: " << mTotalLambda);
 	}
 
 	/// Calculate properties used during the functions below
@@ -430,12 +444,12 @@ public:
 			// integrate + a position integrate and then discard the velocity change.
 			if (ioBody1.IsDynamic())
 			{
-				ioBody1.SubPositionStep((lambda * ioBody1.GetMotionPropertiesUnchecked()->GetInverseMass()) * inWorldSpaceAxis);
+				ioBody1.SubPositionStep((lambda * mInverseMass1) * inWorldSpaceAxis);
 				ioBody1.SubRotationStep(lambda * Vec3::sLoadFloat3Unsafe(mInvI1_R1PlusUxAxis));
 			}
 			if (ioBody2.IsDynamic())
 			{
-				ioBody2.AddPositionStep((lambda * ioBody2.GetMotionPropertiesUnchecked()->GetInverseMass()) * inWorldSpaceAxis);
+				ioBody2.AddPositionStep((lambda * mInverseMass2) * inWorldSpaceAxis);
 				ioBody2.AddRotationStep(lambda * Vec3::sLoadFloat3Unsafe(mInvI2_R2xAxis));
 			}
 			return true;
@@ -473,6 +487,8 @@ private:
 	Float3						mR2xAxis;
 	Float3						mInvI1_R1PlusUxAxis;
 	Float3						mInvI2_R2xAxis;
+	float						mInverseMass1;
+	float						mInverseMass2;
 	float						mEffectiveMass = 0.0f;
 	SpringPart					mSpringPart;
 	float						mTotalLambda = 0.0f;

Jolt/Physics/Constraints/ContactConstraintManager.cpp

@@ -44,9 +44,9 @@ void ContactConstraintManager::WorldContactPoint::CalculateNonPenetrationConstra
 }
 
 template <EMotionType Type1, EMotionType Type2>
-JPH_INLINE void ContactConstraintManager::WorldContactPoint::CalculateFrictionAndNonPenetrationConstraintProperties(float inDeltaTime, const Body &inBody1, const Body &inBody2, Mat44Arg inInvI1, Mat44Arg inInvI2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal, Vec3Arg inWorldSpaceTangent1, Vec3Arg inWorldSpaceTangent2, float inCombinedRestitution, float inCombinedFriction, float inMinVelocityForRestitution, float inSurfaceVelocity1, float inSurfaceVelocity2)
+JPH_INLINE void ContactConstraintManager::WorldContactPoint::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, float inCombinedRestitution, float inCombinedFriction, float inMinVelocityForRestitution, float inSurfaceVelocity1, float inSurfaceVelocity2)
 {
-	JPH_DET_LOG("CalculateFrictionAndNonPenetrationConstraintProperties: p1: " << inWorldSpacePosition1 << " p2: " << inWorldSpacePosition2
+	JPH_DET_LOG("TemplatedCalculateFrictionAndNonPenetrationConstraintProperties: p1: " << inWorldSpacePosition1 << " p2: " << inWorldSpacePosition2
 		<< " normal: " << inWorldSpaceNormal << " tangent1: " << inWorldSpaceTangent1 << " tangent2: " << inWorldSpaceTangent2
 		<< " restitution: " << inCombinedRestitution << " friction: " << inCombinedFriction << " minv: " << inMinVelocityForRestitution
 		<< " surf1: " << inSurfaceVelocity1 << " surf2: " << inSurfaceVelocity2);
@@ -107,14 +107,14 @@ JPH_INLINE void ContactConstraintManager::WorldContactPoint::CalculateFrictionAn
 		normal_velocity_bias = speculative_contact_velocity_bias;
 	}
 
-	mNonPenetrationConstraint.TemplatedCalculateConstraintProperties<Type1, Type2>(mp1, inInvI1, r1, mp2, inInvI2, r2, inWorldSpaceNormal, normal_velocity_bias);
+	mNonPenetrationConstraint.TemplatedCalculateConstraintProperties<Type1, Type2>(inInvM1, inInvI1, r1, inInvM2, inInvI2, r2, inWorldSpaceNormal, normal_velocity_bias);
 
 	// Calculate friction part
 	if (inCombinedFriction > 0.0f)
 	{
 		// Implement friction as 2 AxisContraintParts
-		mFrictionConstraint1.TemplatedCalculateConstraintProperties<Type1, Type2>(mp1, inInvI1, r1, mp2, inInvI2, r2, inWorldSpaceTangent1, inSurfaceVelocity1);
-		mFrictionConstraint2.TemplatedCalculateConstraintProperties<Type1, Type2>(mp1, inInvI1, r1, mp2, inInvI2, r2, inWorldSpaceTangent2, inSurfaceVelocity2);
+		mFrictionConstraint1.TemplatedCalculateConstraintProperties<Type1, Type2>(inInvM1, inInvI1, r1, inInvM2, inInvI2, r2, inWorldSpaceTangent1, inSurfaceVelocity1);
+		mFrictionConstraint2.TemplatedCalculateConstraintProperties<Type1, Type2>(inInvM1, inInvI1, r1, inInvM2, inInvI2, r2, inWorldSpaceTangent2, inSurfaceVelocity2);
 	}
 	else
 	{
@@ -624,8 +624,37 @@ void ContactConstraintManager::PrepareConstraintBuffer(PhysicsUpdateContext *inC
 }
 
 template <EMotionType Type1, EMotionType Type2>
-JPH_INLINE void ContactConstraintManager::TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, float inDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2, Mat44Arg inInvI1, Mat44Arg inInvI2)
+JPH_INLINE void ContactConstraintManager::TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2)
 {
+	// Calculate scaled mass and inertia
+	float inv_m1;
+	Mat44 inv_i1;
+	if constexpr (Type1 == EMotionType::Dynamic)
+	{
+		const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked();
+		inv_m1 = inSettings.mInvMassScale1 * mp1->GetInverseMass();
+		inv_i1 = inSettings.mInvInertiaScale1 * mp1->GetInverseInertiaForRotation(inTransformBody1.GetRotation());
+	}
+	else
+	{
+		inv_m1 = 0.0f;
+		inv_i1 = Mat44::sZero();
+	}
+
+	float inv_m2;
+	Mat44 inv_i2;
+	if constexpr (Type2 == EMotionType::Dynamic)
+	{
+		const MotionProperties *mp2 = inBody2.GetMotionPropertiesUnchecked();
+		inv_m2 = inSettings.mInvMassScale2 * mp2->GetInverseMass();
+		inv_i2 = inSettings.mInvInertiaScale2 * mp2->GetInverseInertiaForRotation(inTransformBody2.GetRotation());
+	}
+	else
+	{
+		inv_m2 = 0.0f;
+		inv_i2 = Mat44::sZero();
+	}
+
 	// Calculate tangents
 	Vec3 t1, t2;
 	ioConstraint.GetTangents(t1, t2);
@@ -643,47 +672,44 @@ JPH_INLINE void ContactConstraintManager::TemplatedCalculateFrictionAndNonPenetr
 	{
 		RVec3 p1 = inTransformBody1 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition1);
 		RVec3 p2 = inTransformBody2 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition2);
-		wcp.CalculateFrictionAndNonPenetrationConstraintProperties<Type1, Type2>(inDeltaTime, inBody1, inBody2, inInvI1, inInvI2, p1, p2, ws_normal, t1, t2, ioConstraint.mCombinedRestitution, ioConstraint.mCombinedFriction, min_velocity_for_restitution, surface_velocity1, surface_velocity2);
+		wcp.TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<Type1, Type2>(inDeltaTime, inBody1, inBody2, inv_m1, inv_m2, inv_i1, inv_i2, p1, p2, ws_normal, t1, t2, ioConstraint.mCombinedRestitution, ioConstraint.mCombinedFriction, min_velocity_for_restitution, surface_velocity1, surface_velocity2);
 	}
 }
 
-inline void ContactConstraintManager::CalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, float inDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2)
+inline void ContactConstraintManager::CalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2)
 {
 	// Dispatch to the correct templated form
 	switch (inBody1.GetMotionType())
 	{
 	case EMotionType::Dynamic:
+		switch (inBody2.GetMotionType())
 		{
-			Mat44 invi1 = inBody1.GetInverseInertia();
-			switch (inBody2.GetMotionType())
-			{
-			case EMotionType::Dynamic:
-				TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Dynamic, EMotionType::Dynamic>(ioConstraint, inDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2, invi1, inBody2.GetInverseInertia());
-				break;
+		case EMotionType::Dynamic:
+			TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Dynamic, EMotionType::Dynamic>(ioConstraint, inSettings, inDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
+			break;
 
-			case EMotionType::Kinematic:
-				TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Dynamic, EMotionType::Kinematic>(ioConstraint, inDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2, invi1, Mat44() /* Will not be used */);
-				break;
+		case EMotionType::Kinematic:
+			TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Dynamic, EMotionType::Kinematic>(ioConstraint, inSettings, inDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
+			break;
 
-			case EMotionType::Static:
-				TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Dynamic, EMotionType::Static>(ioConstraint, inDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2, invi1, Mat44() /* Will not be used */);
-				break;
+		case EMotionType::Static:
+			TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Dynamic, EMotionType::Static>(ioConstraint, inSettings, inDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
+			break;
 
-			default:
-				JPH_ASSERT(false);
-				break;
-			}
+		default:
+			JPH_ASSERT(false);
 			break;
 		}
+		break;
 
 	case EMotionType::Kinematic:
 		JPH_ASSERT(inBody2.IsDynamic());
-		TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Kinematic, EMotionType::Dynamic>(ioConstraint, inDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2, Mat44() /* Will not be used */, inBody2.GetInverseInertia());
+		TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Kinematic, EMotionType::Dynamic>(ioConstraint, inSettings, inDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
 		break;
 
 	case EMotionType::Static:
 		JPH_ASSERT(inBody2.IsDynamic());
-		TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Static, EMotionType::Dynamic>(ioConstraint, inDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2, Mat44() /* Will not be used */, inBody2.GetInverseInertia());
+		TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Static, EMotionType::Dynamic>(ioConstraint, inSettings, inDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
 		break;
 
 	default:
@@ -828,9 +854,9 @@ void ContactConstraintManager::GetContactsFromCache(ContactAllocator &ioContactA
 			mContactListener->OnContactPersisted(*body1, *body2, manifold, settings);
 		}
 
-		// If one of the bodies is a sensor, don't actually create the constraint
 		JPH_ASSERT(settings.mIsSensor || !(body1->IsSensor() || body2->IsSensor()), "Sensors cannot be converted into regular bodies by a contact callback!");
-		if (!settings.mIsSensor)
+		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
 		{
 			// Add contact constraint in world space for the solver
 			uint32 constraint_idx = mNumConstraints++;
@@ -866,7 +892,7 @@ void ContactConstraintManager::GetContactsFromCache(ContactAllocator &ioContactA
 			JPH_DET_LOG("GetContactsFromCache: id1: " << constraint.mBody1->GetID() << " id2: " << constraint.mBody2->GetID() << " key: " << constraint.mSortKey);
 
 			// Calculate friction and non-penetration constraint properties for all contact points
-			CalculateFrictionAndNonPenetrationConstraintProperties(constraint, delta_time, transform_body1, transform_body2, *body1, *body2);
+			CalculateFrictionAndNonPenetrationConstraintProperties(constraint, settings, delta_time, transform_body1, transform_body2, *body1, *body2);
 
 			// Notify island builder
 			mUpdateContext->mIslandBuilder->LinkContact(constraint_idx, body1->GetIndexInActiveBodiesInternal(), body2->GetIndexInActiveBodiesInternal());
@@ -931,7 +957,7 @@ ContactConstraintManager::BodyPairHandle ContactConstraintManager::AddBodyPair(C
 }
 
 template <EMotionType Type1, EMotionType Type2>
-bool ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPairHandle, Body &inBody1, Body &inBody2, const ContactManifold &inManifold, Mat44Arg inInvI1, Mat44Arg inInvI2)
+bool ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPairHandle, Body &inBody1, Body &inBody2, const ContactManifold &inManifold)
 {
 	// Calculate hash
 	SubShapeIDPair key { inBody1.GetID(), inManifold.mSubShapeID1, inBody2.GetID(), inManifold.mSubShapeID2 };
@@ -994,7 +1020,7 @@ bool ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &i
 	// Get inverse transform for body 1
 	RMat44 inverse_transform_body1 = inBody1.GetInverseCenterOfMassTransform();
 
-	bool contact_constraint_created;
+	bool contact_constraint_created = false;
 
 	// If one of the bodies is a sensor, don't actually create the constraint
 	JPH_ASSERT(settings.mIsSensor || !(inBody1.IsSensor() || inBody2.IsSensor()), "Sensors cannot be converted into regular bodies by a contact callback!");
@@ -1017,11 +1043,8 @@ bool ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &i
 			cp.mFrictionLambda[0] = 0.0f;
 			cp.mFrictionLambda[1] = 0.0f;
 		}
-
-		// No constraint created
-		contact_constraint_created = false;
 	}
-	else
+	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
 	{
 		// Add contact constraint
 		uint32 constraint_idx = mNumConstraints++;
@@ -1056,6 +1079,35 @@ bool ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &i
 		// Get time step
 		float delta_time = mUpdateContext->mSubStepDeltaTime;
 
+		// Calculate scaled mass and inertia
+		float inv_m1;
+		Mat44 inv_i1;
+		if constexpr (Type1 == EMotionType::Dynamic)
+		{
+			const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked();
+			inv_m1 = settings.mInvMassScale1 * mp1->GetInverseMass();
+			inv_i1 = settings.mInvInertiaScale1 * mp1->GetInverseInertiaForRotation(inverse_transform_body1.Transposed3x3());
+		}
+		else
+		{
+			inv_m1 = 0.0f;
+			inv_i1 = Mat44::sZero();
+		}
+
+		float inv_m2;
+		Mat44 inv_i2;
+		if constexpr (Type2 == EMotionType::Dynamic)
+		{
+			const MotionProperties *mp2 = inBody2.GetMotionPropertiesUnchecked();
+			inv_m2 = settings.mInvMassScale2 * mp2->GetInverseMass();
+			inv_i2 = settings.mInvInertiaScale2 * mp2->GetInverseInertiaForRotation(inverse_transform_body2.Transposed3x3());
+		}
+		else
+		{
+			inv_m2 = 0.0f;
+			inv_i2 = Mat44::sZero();
+		}
+
 		// Calculate tangents
 		Vec3 t1, t2;
 		constraint.GetTangents(t1, t2);
@@ -1103,7 +1155,7 @@ bool ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &i
 			wcp.mContactPoint = &cp;
 
 			// Setup velocity constraint
-			wcp.CalculateFrictionAndNonPenetrationConstraintProperties<Type1, Type2>(delta_time, inBody1, inBody2, inInvI1, inInvI2, p1_ws, p2_ws, inManifold.mWorldSpaceNormal, t1, t2, settings.mCombinedRestitution, settings.mCombinedFriction, mPhysicsSettings.mMinVelocityForRestitution, surface_velocity1, surface_velocity2);
+			wcp.TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<Type1, Type2>(delta_time, inBody1, inBody2, inv_m1, inv_m2, inv_i1, inv_i2, p1_ws, p2_ws, inManifold.mWorldSpaceNormal, t1, t2, settings.mCombinedRestitution, settings.mCombinedFriction, mPhysicsSettings.mMinVelocityForRestitution, surface_velocity1, surface_velocity2);
 		}
 
 	#ifdef JPH_DEBUG_RENDERER
@@ -1156,17 +1208,16 @@ bool ContactConstraintManager::AddContactConstraint(ContactAllocator &ioContactA
 	{
 	case EMotionType::Dynamic:
 		{
-			Mat44 invi1 = body1->GetInverseInertia();
 			switch (body2->GetMotionType())
 			{
 			case EMotionType::Dynamic:
-				return TemplatedAddContactConstraint<EMotionType::Dynamic, EMotionType::Dynamic>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold, invi1, body2->GetInverseInertia());
+				return TemplatedAddContactConstraint<EMotionType::Dynamic, EMotionType::Dynamic>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold);
 
 			case EMotionType::Kinematic:
-				return TemplatedAddContactConstraint<EMotionType::Dynamic, EMotionType::Kinematic>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold, invi1, Mat44() /* Will not be used */);
+				return TemplatedAddContactConstraint<EMotionType::Dynamic, EMotionType::Kinematic>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold);
 
 			case EMotionType::Static:
-				return TemplatedAddContactConstraint<EMotionType::Dynamic, EMotionType::Static>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold, invi1, Mat44() /* Will not be used */);
+				return TemplatedAddContactConstraint<EMotionType::Dynamic, EMotionType::Static>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold);
 
 			default:
 				JPH_ASSERT(false);
@@ -1179,13 +1230,13 @@ bool ContactConstraintManager::AddContactConstraint(ContactAllocator &ioContactA
 		switch (body2->GetMotionType())
 		{
 		case EMotionType::Dynamic:
-			return TemplatedAddContactConstraint<EMotionType::Kinematic, EMotionType::Dynamic>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold, Mat44() /* Will not be used */, body2->GetInverseInertia());
+			return TemplatedAddContactConstraint<EMotionType::Kinematic, EMotionType::Dynamic>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold);
 
 		case EMotionType::Kinematic:
-			return TemplatedAddContactConstraint<EMotionType::Kinematic, EMotionType::Kinematic>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold, Mat44() /* Will not be used */, Mat44() /* Will not be used */);
+			return TemplatedAddContactConstraint<EMotionType::Kinematic, EMotionType::Kinematic>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold);
 
 		case EMotionType::Static:
-			return TemplatedAddContactConstraint<EMotionType::Kinematic, EMotionType::Static>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold, Mat44() /* Will not be used */, Mat44() /* Will not be used */);
+			return TemplatedAddContactConstraint<EMotionType::Kinematic, EMotionType::Static>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold);
 
 		default:
 			JPH_ASSERT(false);
@@ -1197,10 +1248,10 @@ bool ContactConstraintManager::AddContactConstraint(ContactAllocator &ioContactA
 		switch (body2->GetMotionType())
 		{
 		case EMotionType::Dynamic:
-			return TemplatedAddContactConstraint<EMotionType::Static, EMotionType::Dynamic>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold, Mat44() /* Will not be used */, body2->GetInverseInertia());
+			return TemplatedAddContactConstraint<EMotionType::Static, EMotionType::Dynamic>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold);
 
 		case EMotionType::Kinematic:
-			return TemplatedAddContactConstraint<EMotionType::Static, EMotionType::Kinematic>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold, Mat44() /* Will not be used */, Mat44() /* Will not be used */);
+			return TemplatedAddContactConstraint<EMotionType::Static, EMotionType::Kinematic>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold);
 
 		case EMotionType::Static: // Static vs static not possible
 		default: 
@@ -1367,6 +1418,9 @@ void ContactConstraintManager::SetupVelocityConstraints(const uint32 *inConstrai
 {
 	JPH_PROFILE_FUNCTION();
 
+	// Note: We don't have the settings anymore here, mass/inertia scaling doesn't work with multiple integration substeps!
+	ContactSettings dummy_settings;
+
 	for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx)
 	{
 		ContactConstraint &constraint = mConstraints[*constraint_idx];
@@ -1380,7 +1434,7 @@ void ContactConstraintManager::SetupVelocityConstraints(const uint32 *inConstrai
 		RMat44 transform_body2 = body2.GetCenterOfMassTransform();
 
 		// Calculate friction and non-penetration constraint properties for all contact points
-		CalculateFrictionAndNonPenetrationConstraintProperties(constraint, inDeltaTime, transform_body1, transform_body2, body1, body2);
+		CalculateFrictionAndNonPenetrationConstraintProperties(constraint, dummy_settings, inDeltaTime, transform_body1, transform_body2, body1, body2);
 	}
 }
 

Jolt/Physics/Constraints/ContactConstraintManager.h

@@ -422,8 +422,9 @@ private:
 	public:
 		/// Calculate constraint properties below
 		void					CalculateNonPenetrationConstraintProperties(const Body &inBody1, const Body &inBody2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal);
+
 		template <EMotionType Type1, EMotionType Type2>
-		JPH_INLINE void			CalculateFrictionAndNonPenetrationConstraintProperties(float inDeltaTime, const Body &inBody1, const Body &inBody2, Mat44Arg inInvI1, Mat44Arg inInvI2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal, Vec3Arg inWorldSpaceTangent1, Vec3Arg inWorldSpaceTangent2, float inCombinedRestitution, float inCombinedFriction, float inMinVelocityForRestitution, float inSurfaceVelocity1, float inSurfaceVelocity2);
+		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, float inCombinedRestitution, float inCombinedFriction, float inMinVelocityForRestitution, float inSurfaceVelocity1, float inSurfaceVelocity2);
 
 		/// The constraint parts
 		AxisConstraintPart		mNonPenetrationConstraint;
@@ -477,14 +478,14 @@ private:
 
 	/// Internal helper function to calculate the friction and non-penetration constraint properties. Templated to the motion type to reduce the amount of branches and calculations.
 	template <EMotionType Type1, EMotionType Type2>
-	JPH_INLINE void				TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, float inDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2, Mat44Arg inInvI1, Mat44Arg inInvI2);
+	JPH_INLINE void				TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2);
 
 	/// Internal helper function to calculate the friction and non-penetration constraint properties.
-	inline void					CalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, float inDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2);
+	inline void					CalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2);
 
 	/// Internal helper function to add a contact constraint. Templated to the motion type to reduce the amount of branches and calculations.
 	template <EMotionType Type1, EMotionType Type2>
-	bool						TemplatedAddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPairHandle, Body &inBody1, Body &inBody2, const ContactManifold &inManifold, Mat44Arg inInvI1, Mat44Arg inInvI2);
+	bool						TemplatedAddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPairHandle, Body &inBody1, Body &inBody2, const ContactManifold &inManifold);
 
 	/// Internal helper function to warm start contact constraint. Templated to the motion type to reduce the amount of branches and calculations.
 	template <EMotionType Type1, EMotionType Type2>

Samples/Samples.cmake

@@ -121,6 +121,8 @@ set(SAMPLES_SRC_FILES
 	${SAMPLES_ROOT}/Tests/General/LoadSaveSceneTest.h
 	${SAMPLES_ROOT}/Tests/General/ManifoldReductionTest.cpp
 	${SAMPLES_ROOT}/Tests/General/ManifoldReductionTest.h
+	${SAMPLES_ROOT}/Tests/General/ModifyMassTest.cpp
+	${SAMPLES_ROOT}/Tests/General/ModifyMassTest.h
 	${SAMPLES_ROOT}/Tests/General/MultithreadedTest.cpp
 	${SAMPLES_ROOT}/Tests/General/MultithreadedTest.h
 	${SAMPLES_ROOT}/Tests/General/PyramidTest.cpp

Samples/SamplesApp.cpp

@@ -90,6 +90,7 @@ JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, BigVsSmallTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, ActiveEdgesTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, MultithreadedTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, ContactListenerTest)
+JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, ModifyMassTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, ActivateDuringUpdateTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, SensorTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, DynamicMeshTest)
@@ -127,6 +128,7 @@ static TestNameAndRTTI sGeneralTests[] =
 	{ "Active Edges",						JPH_RTTI(ActiveEdgesTest) },
 	{ "Multithreaded",						JPH_RTTI(MultithreadedTest) },
 	{ "Contact Listener",					JPH_RTTI(ContactListenerTest) },
+	{ "Modify Mass",						JPH_RTTI(ModifyMassTest) },
 	{ "Activate During Update",				JPH_RTTI(ActivateDuringUpdateTest) },
 	{ "Sensor",								JPH_RTTI(SensorTest) },
 	{ "Dynamic Mesh",						JPH_RTTI(DynamicMeshTest) },

Samples/Tests/General/ModifyMassTest.cpp

@@ -0,0 +1,110 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/General/ModifyMassTest.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Renderer/DebugRendererImp.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(ModifyMassTest) 
+{ 
+	JPH_ADD_BASE_CLASS(ModifyMassTest, Test) 
+}
+
+void ModifyMassTest::ResetBodies(int inCycle)
+{
+	mBodyInterface->SetPositionAndRotation(mBodies[0], RVec3(-5, 5, 0), Quat::sIdentity(), EActivation::Activate);
+	mBodyInterface->SetLinearAndAngularVelocity(mBodies[0], Vec3(10, 0, 0), Vec3::sZero());
+	mBodyInterface->SetUserData(mBodies[0], inCycle << 1);
+
+	mBodyInterface->SetPositionAndRotation(mBodies[1], RVec3(5, 5, 0), Quat::sIdentity(), EActivation::Activate);
+	mBodyInterface->SetLinearAndAngularVelocity(mBodies[1], Vec3(-10, 0, 0), Vec3::sZero());
+	mBodyInterface->SetUserData(mBodies[1], (inCycle << 1) + 1);
+}
+
+void ModifyMassTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	// Create two spheres on a collision course
+	BodyCreationSettings bcs(new SphereShape(1.0f), RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+	bcs.mRestitution = 1.0f;
+	mBodies[0] = mBodyInterface->CreateAndAddBody(bcs, EActivation::Activate);
+	mBodies[1] = mBodyInterface->CreateAndAddBody(bcs, EActivation::Activate);
+	ResetBodies(0);
+}
+
+void ModifyMassTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
+{
+	constexpr float cTimeBetweenTests = 2.0f;
+
+	int old_cycle = (int)(mTime / cTimeBetweenTests);
+	mTime += inParams.mDeltaTime;
+	int new_cycle = (int)(mTime / cTimeBetweenTests);
+	if (old_cycle != new_cycle)
+		ResetBodies(new_cycle);
+}
+
+void ModifyMassTest::PostPhysicsUpdate(float inDeltaTime)
+{
+	// Draw the mass scale
+	for (BodyID id : mBodies)
+	{
+		BodyLockRead body_lock(mPhysicsSystem->GetBodyLockInterface(), id);
+		if (body_lock.Succeeded())
+		{
+			const Body &body = body_lock.GetBody();
+			DebugRenderer::sInstance->DrawText3D(body.GetPosition(), StringFormat("Inv mass scale: %.1f\nVelocity X: %.1f", (double)sGetInvMassScale(body), (double)body.GetLinearVelocity().GetX()), Color::sWhite);
+		}
+	}
+}
+
+float ModifyMassTest::sGetInvMassScale(const Body &inBody)
+{
+	uint64 ud = inBody.GetUserData();
+	int index = ((ud & 1) != 0? (ud >> 1) : (ud >> 3)) & 0b11;
+	float mass_overrides[] = { 1.0f, 0.0f, 0.5f, 2.0f };
+	return mass_overrides[index];
+}
+
+void ModifyMassTest::OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings)
+{
+	// We're only concerned with dynamic bodies (floor gets normal collision response)
+	if (!inBody1.IsDynamic() || !inBody2.IsDynamic())
+		return;
+
+	// 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;
+}
+
+void ModifyMassTest::OnContactPersisted(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings)
+{
+	OnContactAdded(inBody1, inBody2, inManifold, ioSettings);
+}
+
+void ModifyMassTest::SaveState(StateRecorder& inStream) const
+{
+	Test::SaveState(inStream);
+
+	inStream.Write(mTime);
+}
+
+void ModifyMassTest::RestoreState(StateRecorder& inStream)
+{
+	Test::RestoreState(inStream);
+
+	inStream.Read(mTime);
+}
+

Samples/Tests/General/ModifyMassTest.h

@@ -0,0 +1,40 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+#include <Jolt/Physics/Collision/ContactListener.h>
+
+// Tests modifying mass from a contact listener
+class ModifyMassTest : public Test, public ContactListener
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, ModifyMassTest)
+
+	// See: Test
+	virtual void				Initialize() override;
+	virtual void				PrePhysicsUpdate(const PreUpdateParams &inParams) override;
+	virtual void				PostPhysicsUpdate(float inDeltaTime) override;
+	virtual void				SaveState(StateRecorder& inStream) const override;
+	virtual void				RestoreState(StateRecorder& inStream) override;
+
+	// If this test implements a contact listener, it should be returned here
+	virtual ContactListener *	GetContactListener() override		{ return this; }
+
+	// See: ContactListener
+	virtual void				OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override;
+	virtual void				OnContactPersisted(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override;
+
+private:
+	// Get the scale factor for a body based on its user data
+	static float				sGetInvMassScale(const Body &inBody);
+
+	// Reset the bodies to their initial states
+	void						ResetBodies(int inCycle);
+
+	float						mTime = 0.0f;
+
+	BodyID						mBodies[2];
+};

UnitTests/Physics/ContactListenerTests.cpp

@@ -412,4 +412,96 @@ TEST_SUITE("ContactListenerTests")
 		CHECK_APPROX_EQUAL(box.GetLinearVelocity(), floor.GetRotation() * listener.mLocalSpaceVelocity, 0.005f);
 		CHECK_APPROX_EQUAL(box.GetAngularVelocity(), Vec3::sZero(), 1.0e-4f);
 	}
+
+	static float sGetInvMassScale(const Body &inBody)
+	{
+		uint64 ud = inBody.GetUserData();
+		int index = ((ud & 1) != 0? (ud >> 1) : (ud >> 3)) & 0b11;
+		float mass_overrides[] = { 1.0f, 0.0f, 0.5f, 2.0f };
+		return mass_overrides[index];
+	}
+
+	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));
+		}
+	}
 }