Reduce contact point size by storing inverse mass only once instead of per constraint part (#757)

Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h

@@ -43,7 +43,7 @@ class AxisConstraintPart
 {
 	/// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated
 	template <EMotionType Type1, EMotionType Type2>
-	JPH_INLINE bool				ApplyVelocityStep(MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2, Vec3Arg inWorldSpaceAxis, float inLambda) const
+	JPH_INLINE bool				ApplyVelocityStep(MotionProperties *ioMotionProperties1, float inInvMass1, MotionProperties *ioMotionProperties2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inLambda) const
 	{
 		// Apply impulse if delta is not zero
 		if (inLambda != 0.0f)
@@ -57,12 +57,12 @@ class AxisConstraintPart
 			// v' = v + M^-1 P
 			if constexpr (Type1 == EMotionType::Dynamic)
 			{
-				ioMotionProperties1->SubLinearVelocityStep((inLambda * mInverseMass1) * inWorldSpaceAxis);
+				ioMotionProperties1->SubLinearVelocityStep((inLambda * inInvMass1) * inWorldSpaceAxis);
 				ioMotionProperties1->SubAngularVelocityStep(inLambda * Vec3::sLoadFloat3Unsafe(mInvI1_R1PlusUxAxis));
 			}
 			if constexpr (Type2 == EMotionType::Dynamic)
 			{
-				ioMotionProperties2->AddLinearVelocityStep((inLambda * mInverseMass2) * inWorldSpaceAxis);
+				ioMotionProperties2->AddLinearVelocityStep((inLambda * inInvMass2) * inWorldSpaceAxis);
 				ioMotionProperties2->AddAngularVelocityStep(inLambda * Vec3::sLoadFloat3Unsafe(mInvI2_R2xAxis));
 			}
 			return true;
@@ -81,7 +81,6 @@ 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);
 		}
@@ -89,14 +88,12 @@ class AxisConstraintPart
 		{
 		#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);
 		}
@@ -104,7 +101,6 @@ class AxisConstraintPart
 		{
 		#ifdef _DEBUG
 			Vec3::sNaN().StoreFloat3(&mR2xAxis);
-			mInverseMass2 = numeric_limits<float>::quiet_NaN();
 		#endif
 		}
 
@@ -115,7 +111,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 = mInverseMass1 + invi1_r1_plus_u_x_axis.Dot(r1_plus_u_x_axis);
+			inv_effective_mass = inInvMass1 + invi1_r1_plus_u_x_axis.Dot(r1_plus_u_x_axis);
 		}
 		else
 		{
@@ -128,7 +124,7 @@ class AxisConstraintPart
 		{
 			Vec3 invi2_r2_x_axis = inInvI2.Multiply3x3(r2_x_axis);
 			invi2_r2_x_axis.StoreFloat3(&mInvI2_R2xAxis);
-			inv_effective_mass += mInverseMass2 + invi2_r2_x_axis.Dot(r2_x_axis);
+			inv_effective_mass += inInvMass2 + invi2_r2_x_axis.Dot(r2_x_axis);
 		}
 		else
 		{
@@ -148,18 +144,18 @@ class AxisConstraintPart
 		case EMotionType::Dynamic:
 			{
 				const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked();
-				float invm1 = mp1->GetInverseMass();
-				Mat44 invi1 = inBody1.GetInverseInertia();
+				float inv_m1 = mp1->GetInverseMass();
+				Mat44 inv_i1 = inBody1.GetInverseInertia();
 				switch (inBody2.GetMotionType())
 				{
 				case EMotionType::Dynamic:
-					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Dynamic>(invm1, invi1, inR1PlusU, inBody2.GetMotionPropertiesUnchecked()->GetInverseMass(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
+					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Dynamic>(inv_m1, inv_i1, inR1PlusU, inBody2.GetMotionPropertiesUnchecked()->GetInverseMass(), 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);
+					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Kinematic>(inv_m1, inv_i1, 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);
+					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Static>(inv_m1, inv_i1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis);
 
 				default:
 					break;
@@ -184,26 +180,24 @@ class AxisConstraintPart
 	}
 
 	/// 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)
+	JPH_INLINE float			CalculateInverseEffectiveMassWithMassOverride(const Body &inBody1, float inInvMass1, float inInvInertiaScale1, Vec3Arg inR1PlusU, const Body &inBody2, float inInvMass2, 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();
+				Mat44 inv_i1 = 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);
+					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Dynamic>(inInvMass1, inv_i1, inR1PlusU, inInvMass2, 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);
+					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Kinematic>(inInvMass1, inv_i1, 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);
+					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Static>(inInvMass1, inv_i1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis);
 
 				default:
 					break;
@@ -213,11 +207,11 @@ class AxisConstraintPart
 
 		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);
+			return TemplatedCalculateInverseEffectiveMass<EMotionType::Kinematic, EMotionType::Dynamic>(0 /* Will not be used */, Mat44() /* Will not be used */, inR1PlusU, inInvMass2, 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);
+			return TemplatedCalculateInverseEffectiveMass<EMotionType::Static, EMotionType::Dynamic>(0 /* Will not be used */, Mat44() /* Will not be used */, inR1PlusU, inInvMass2, inInvInertiaScale2 * inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
 
 		default:
 			break;
@@ -262,17 +256,17 @@ 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 inInvMass1 The inverse mass of body 1 (only used when body 1 is dynamic)
+	/// @param inInvMass2 The inverse mass of body 2 (only used when body 2 is dynamic)
 	/// @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)
+	inline void					CalculateConstraintPropertiesWithMassOverride(const Body &inBody1, float inInvMass1, float inInvInertiaScale1, Vec3Arg inR1PlusU, const Body &inBody2, float inInvMass2, float inInvInertiaScale2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f)
 	{
-		float inv_effective_mass = CalculateInverseEffectiveMassWithMassScale(inBody1, inInvMassScale1, inInvInertiaScale1, inR1PlusU, inBody2, inInvMassScale2, inInvInertiaScale2, inR2, inWorldSpaceAxis);
+		float inv_effective_mass = CalculateInverseEffectiveMassWithMassOverride(inBody1, inInvMass1, inInvInertiaScale1, inR1PlusU, inBody2, inInvMass2, inInvInertiaScale2, inR2, inWorldSpaceAxis);
 
 		if (inv_effective_mass == 0.0f)
 			Deactivate();
@@ -353,11 +347,11 @@ public:
 
 	/// Templated form of WarmStart with the motion types baked in
 	template <EMotionType Type1, EMotionType Type2>
-	inline void					TemplatedWarmStart(MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2, Vec3Arg inWorldSpaceAxis, float inWarmStartImpulseRatio)
+	inline void					TemplatedWarmStart(MotionProperties *ioMotionProperties1, float inInvMass1, MotionProperties *ioMotionProperties2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inWarmStartImpulseRatio)
 	{
 		mTotalLambda *= inWarmStartImpulseRatio;
 
-		ApplyVelocityStep<Type1, Type2>(ioMotionProperties1, ioMotionProperties2, inWorldSpaceAxis, mTotalLambda);
+		ApplyVelocityStep<Type1, Type2>(ioMotionProperties1, inInvMass1, ioMotionProperties2, inInvMass2, inWorldSpaceAxis, mTotalLambda);
 	}
 
 	/// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses
@@ -378,14 +372,14 @@ public:
 		if (motion_type1 == EMotionType::Dynamic)
 		{
 			if (motion_type2 == EMotionType::Dynamic)
-				TemplatedWarmStart<EMotionType::Dynamic, EMotionType::Dynamic>(motion_properties1, motion_properties2, inWorldSpaceAxis, inWarmStartImpulseRatio);
+				TemplatedWarmStart<EMotionType::Dynamic, EMotionType::Dynamic>(motion_properties1, motion_properties1->GetInverseMass(), motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inWarmStartImpulseRatio);
 			else
-				TemplatedWarmStart<EMotionType::Dynamic, EMotionType::Static>(motion_properties1, motion_properties2, inWorldSpaceAxis, inWarmStartImpulseRatio);
+				TemplatedWarmStart<EMotionType::Dynamic, EMotionType::Static>(motion_properties1, motion_properties1->GetInverseMass(), motion_properties2, 0.0f /* Unused */, inWorldSpaceAxis, inWarmStartImpulseRatio);
 		}
 		else
 		{
 			JPH_ASSERT(motion_type2 == EMotionType::Dynamic);
-			TemplatedWarmStart<EMotionType::Static, EMotionType::Dynamic>(motion_properties1, motion_properties2, inWorldSpaceAxis, inWarmStartImpulseRatio);
+			TemplatedWarmStart<EMotionType::Static, EMotionType::Dynamic>(motion_properties1, 0.0f /* Unused */, motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inWarmStartImpulseRatio);
 		}
 	}
 
@@ -421,24 +415,24 @@ public:
 
 	/// Templated form of SolveVelocityConstraint with the motion types baked in, part 2: apply new lambda
 	template <EMotionType Type1, EMotionType Type2>
-	JPH_INLINE bool				TemplatedSolveVelocityConstraintApplyLambda(MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2, Vec3Arg inWorldSpaceAxis, float inTotalLambda)
+	JPH_INLINE bool				TemplatedSolveVelocityConstraintApplyLambda(MotionProperties *ioMotionProperties1, float inInvMass1, MotionProperties *ioMotionProperties2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inTotalLambda)
 	{
 		float delta_lambda = inTotalLambda - mTotalLambda; // Calculate change in lambda
 		mTotalLambda = inTotalLambda; // Store accumulated impulse
 
-		return ApplyVelocityStep<Type1, Type2>(ioMotionProperties1, ioMotionProperties2, inWorldSpaceAxis, delta_lambda);
+		return ApplyVelocityStep<Type1, Type2>(ioMotionProperties1, inInvMass1, ioMotionProperties2, inInvMass2, inWorldSpaceAxis, delta_lambda);
 	}
 
 	/// Templated form of SolveVelocityConstraint with the motion types baked in
 	template <EMotionType Type1, EMotionType Type2>
-	inline bool					TemplatedSolveVelocityConstraint(MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2, Vec3Arg inWorldSpaceAxis, float inMinLambda, float inMaxLambda)
+	inline bool					TemplatedSolveVelocityConstraint(MotionProperties *ioMotionProperties1, float inInvMass1, MotionProperties *ioMotionProperties2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inMinLambda, float inMaxLambda)
 	{
 		float total_lambda = TemplatedSolveVelocityConstraintGetTotalLambda<Type1, Type2>(ioMotionProperties1, ioMotionProperties2, inWorldSpaceAxis);
 
 		// Clamp impulse to specified range
 		total_lambda = Clamp(total_lambda, inMinLambda, inMaxLambda);
 
-		return TemplatedSolveVelocityConstraintApplyLambda<Type1, Type2>(ioMotionProperties1, ioMotionProperties2, inWorldSpaceAxis, total_lambda);
+		return TemplatedSolveVelocityConstraintApplyLambda<Type1, Type2>(ioMotionProperties1, inInvMass1, ioMotionProperties2, inInvMass2, inWorldSpaceAxis, total_lambda);
 	}
 
 	/// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation.
@@ -462,13 +456,66 @@ public:
 			switch (motion_type2)
 			{
 			case EMotionType::Dynamic:
-				return TemplatedSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Dynamic>(motion_properties1, motion_properties2, inWorldSpaceAxis, inMinLambda, inMaxLambda);
+				return TemplatedSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Dynamic>(motion_properties1, motion_properties1->GetInverseMass(), motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inMinLambda, inMaxLambda);
+
+			case EMotionType::Kinematic:
+				return TemplatedSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Kinematic>(motion_properties1, motion_properties1->GetInverseMass(), motion_properties2, 0.0f /* Unused */, inWorldSpaceAxis, inMinLambda, inMaxLambda);
+
+			case EMotionType::Static:
+				return TemplatedSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Static>(motion_properties1, motion_properties1->GetInverseMass(), motion_properties2, 0.0f /* Unused */, inWorldSpaceAxis, inMinLambda, inMaxLambda);
+
+			default:
+				JPH_ASSERT(false);
+				break;
+			}
+			break;
+
+		case EMotionType::Kinematic:
+			JPH_ASSERT(motion_type2 == EMotionType::Dynamic);
+			return TemplatedSolveVelocityConstraint<EMotionType::Kinematic, EMotionType::Dynamic>(motion_properties1, 0.0f /* Unused */, motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inMinLambda, inMaxLambda);
+
+		case EMotionType::Static:
+			JPH_ASSERT(motion_type2 == EMotionType::Dynamic);
+			return TemplatedSolveVelocityConstraint<EMotionType::Static, EMotionType::Dynamic>(motion_properties1, 0.0f /* Unused */, motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inMinLambda, inMaxLambda);
+
+		default:
+			JPH_ASSERT(false);
+			break;
+		}
+
+		return false;
+	}
+
+	/// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation.
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inInvMass1 The inverse mass of body 1 (only used when body 1 is dynamic)
+	/// @param inInvMass2 The inverse mass of body 2 (only used when body 2 is dynamic)
+	/// @param inWorldSpaceAxis Axis along which the constraint acts (normalized)
+	/// @param inMinLambda Minimum value of constraint impulse to apply (N s)
+	/// @param inMaxLambda Maximum value of constraint impulse to apply (N s)
+	inline bool					SolveVelocityConstraintWithMassOverride(Body &ioBody1, float inInvMass1, Body &ioBody2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inMinLambda, float inMaxLambda)
+	{
+		EMotionType motion_type1 = ioBody1.GetMotionType();
+		MotionProperties *motion_properties1 = ioBody1.GetMotionPropertiesUnchecked();
+
+		EMotionType motion_type2 = ioBody2.GetMotionType();
+		MotionProperties *motion_properties2 = ioBody2.GetMotionPropertiesUnchecked();
+
+		// Dispatch to the correct templated form
+		switch (motion_type1)
+		{
+		case EMotionType::Dynamic:
+			switch (motion_type2)
+			{
+			case EMotionType::Dynamic:
+				return TemplatedSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Dynamic>(motion_properties1, inInvMass1, motion_properties2, inInvMass2, inWorldSpaceAxis, inMinLambda, inMaxLambda);
 
 			case EMotionType::Kinematic:
-				return TemplatedSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Kinematic>(motion_properties1, motion_properties2, inWorldSpaceAxis, inMinLambda, inMaxLambda);
+				return TemplatedSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Kinematic>(motion_properties1, inInvMass1, motion_properties2, 0.0f /* Unused */, inWorldSpaceAxis, inMinLambda, inMaxLambda);
 
 			case EMotionType::Static:
-				return TemplatedSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Static>(motion_properties1, motion_properties2, inWorldSpaceAxis, inMinLambda, inMaxLambda);
+				return TemplatedSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Static>(motion_properties1, inInvMass1, motion_properties2, 0.0f /* Unused */, inWorldSpaceAxis, inMinLambda, inMaxLambda);
 
 			default:
 				JPH_ASSERT(false);
@@ -478,11 +525,11 @@ public:
 
 		case EMotionType::Kinematic:
 			JPH_ASSERT(motion_type2 == EMotionType::Dynamic);
-			return TemplatedSolveVelocityConstraint<EMotionType::Kinematic, EMotionType::Dynamic>(motion_properties1, motion_properties2, inWorldSpaceAxis, inMinLambda, inMaxLambda);
+			return TemplatedSolveVelocityConstraint<EMotionType::Kinematic, EMotionType::Dynamic>(motion_properties1, 0.0f /* Unused */, motion_properties2, inInvMass2, inWorldSpaceAxis, inMinLambda, inMaxLambda);
 
 		case EMotionType::Static:
 			JPH_ASSERT(motion_type2 == EMotionType::Dynamic);
-			return TemplatedSolveVelocityConstraint<EMotionType::Static, EMotionType::Dynamic>(motion_properties1, motion_properties2, inWorldSpaceAxis, inMinLambda, inMaxLambda);
+			return TemplatedSolveVelocityConstraint<EMotionType::Static, EMotionType::Dynamic>(motion_properties1, 0.0f /* Unused */, motion_properties2, inInvMass2, inWorldSpaceAxis, inMinLambda, inMaxLambda);
 
 		default:
 			JPH_ASSERT(false);
@@ -527,12 +574,63 @@ public:
 			// integrate + a position integrate and then discard the velocity change.
 			if (ioBody1.IsDynamic())
 			{
-				ioBody1.SubPositionStep((lambda * mInverseMass1) * inWorldSpaceAxis);
+				ioBody1.SubPositionStep((lambda * ioBody1.GetMotionProperties()->GetInverseMass()) * inWorldSpaceAxis);
+				ioBody1.SubRotationStep(lambda * Vec3::sLoadFloat3Unsafe(mInvI1_R1PlusUxAxis));
+			}
+			if (ioBody2.IsDynamic())
+			{
+				ioBody2.AddPositionStep((lambda * ioBody2.GetMotionProperties()->GetInverseMass()) * inWorldSpaceAxis);
+				ioBody2.AddRotationStep(lambda * Vec3::sLoadFloat3Unsafe(mInvI2_R2xAxis));
+			}
+			return true;
+		}
+
+		return false;
+	}
+
+	/// Iteratively update the position constraint. Makes sure C(...) = 0.
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inInvMass1 The inverse mass of body 1 (only used when body 1 is dynamic)
+	/// @param inInvMass2 The inverse mass of body 2 (only used when body 2 is dynamic)
+	/// @param inWorldSpaceAxis Axis along which the constraint acts (normalized)
+	/// @param inC Value of the constraint equation (C)
+	/// @param inBaumgarte Baumgarte constant (fraction of the error to correct)
+	inline bool					SolvePositionConstraintWithMassOverride(Body &ioBody1, float inInvMass1, Body &ioBody2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inC, float inBaumgarte) const
+	{
+		// Only apply position constraint when the constraint is hard, otherwise the velocity bias will fix the constraint
+		if (inC != 0.0f && !mSpringPart.IsActive())
+		{
+			// Calculate lagrange multiplier (lambda) for Baumgarte stabilization:
+			//
+			// lambda = -K^-1 * beta / dt * C
+			//
+			// We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
+			float lambda = -mEffectiveMass * inBaumgarte * inC;
+
+			// Directly integrate velocity change for one time step
+			//
+			// Euler velocity integration:
+			// dv = M^-1 P
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler position integration:
+			// x' = x + dv * dt
+			//
+			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
+			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
+			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
+			// integrate + a position integrate and then discard the velocity change.
+			if (ioBody1.IsDynamic())
+			{
+				ioBody1.SubPositionStep((lambda * inInvMass1) * inWorldSpaceAxis);
 				ioBody1.SubRotationStep(lambda * Vec3::sLoadFloat3Unsafe(mInvI1_R1PlusUxAxis));
 			}
 			if (ioBody2.IsDynamic())
 			{
-				ioBody2.AddPositionStep((lambda * mInverseMass2) * inWorldSpaceAxis);
+				ioBody2.AddPositionStep((lambda * inInvMass2) * inWorldSpaceAxis);
 				ioBody2.AddRotationStep(lambda * Vec3::sLoadFloat3Unsafe(mInvI2_R2xAxis));
 			}
 			return true;
@@ -570,8 +668,6 @@ 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/ConstraintPart/PointConstraintPart.h

@@ -90,12 +90,12 @@ public:
 		if (inBody1.IsDynamic())
 		{
 			const MotionProperties *mp1 = inBody1.GetMotionProperties();
-			Mat44 invi1 = mp1->GetInverseInertiaForRotation(inRotation1);
+			Mat44 inv_i1 = mp1->GetInverseInertiaForRotation(inRotation1);
 			summed_inv_mass = mp1->GetInverseMass();
 
 			Mat44 r1x = Mat44::sCrossProduct(mR1);
-			mInvI1_R1X = invi1.Multiply3x3(r1x);
-			inv_effective_mass = r1x.Multiply3x3(invi1).Multiply3x3RightTransposed(r1x);
+			mInvI1_R1X = inv_i1.Multiply3x3(r1x);
+			inv_effective_mass = r1x.Multiply3x3(inv_i1).Multiply3x3RightTransposed(r1x);
 		}
 		else
 		{
@@ -108,12 +108,12 @@ public:
 		if (inBody2.IsDynamic())
 		{
 			const MotionProperties *mp2 = inBody2.GetMotionProperties();
-			Mat44 invi2 = mp2->GetInverseInertiaForRotation(inRotation2);
+			Mat44 inv_i2 = mp2->GetInverseInertiaForRotation(inRotation2);
 			summed_inv_mass += mp2->GetInverseMass();
 
 			Mat44 r2x = Mat44::sCrossProduct(mR2);
-			mInvI2_R2X = invi2.Multiply3x3(r2x);
-			inv_effective_mass += r2x.Multiply3x3(invi2).Multiply3x3RightTransposed(r2x);
+			mInvI2_R2X = inv_i2.Multiply3x3(r2x);
+			inv_effective_mass += r2x.Multiply3x3(inv_i2).Multiply3x3RightTransposed(r2x);
 		}
 		else
 		{

Jolt/Physics/Constraints/ConstraintPart/RotationQuatConstraintPart.h

@@ -132,15 +132,15 @@ public:
 		Mat44 jp = (Mat44::sQuatLeftMultiply(0.5f * inBody1.GetRotation().Conjugated()) * Mat44::sQuatRightMultiply(inBody2.GetRotation() * inInvInitialOrientation)).GetRotationSafe();
 
 		// Calculate properties used during constraint solving
-		Mat44 invi1 = inBody1.IsDynamic()? inBody1.GetMotionProperties()->GetInverseInertiaForRotation(inRotation1) : Mat44::sZero();
-		Mat44 invi2 = inBody2.IsDynamic()? inBody2.GetMotionProperties()->GetInverseInertiaForRotation(inRotation2) : Mat44::sZero();
-		mInvI1_JPT = invi1.Multiply3x3RightTransposed(jp);
-		mInvI2_JPT = invi2.Multiply3x3RightTransposed(jp);
+		Mat44 inv_i1 = inBody1.IsDynamic()? inBody1.GetMotionProperties()->GetInverseInertiaForRotation(inRotation1) : Mat44::sZero();
+		Mat44 inv_i2 = inBody2.IsDynamic()? inBody2.GetMotionProperties()->GetInverseInertiaForRotation(inRotation2) : Mat44::sZero();
+		mInvI1_JPT = inv_i1.Multiply3x3RightTransposed(jp);
+		mInvI2_JPT = inv_i2.Multiply3x3RightTransposed(jp);
 
 		// Calculate effective mass: K^-1 = (J M^-1 J^T)^-1
 		// = (JP * I1^-1 * JP^T + JP * I2^-1 * JP^T)^-1
 		// = (JP * (I1^-1 + I2^-1) * JP^T)^-1
-		if (!mEffectiveMass.SetInversed3x3(jp.Multiply3x3(invi1 + invi2).Multiply3x3RightTransposed(jp)))
+		if (!mEffectiveMass.SetInversed3x3(jp.Multiply3x3(inv_i1 + inv_i2).Multiply3x3RightTransposed(jp)))
 			Deactivate();
 		else
 			mEffectiveMass_JP = mEffectiveMass.Multiply3x3(jp);

Jolt/Physics/Constraints/ContactConstraintManager.cpp

@@ -33,14 +33,14 @@ bool ContactConstraintManager::sDrawContactManifolds = false;
 // ContactConstraintManager::WorldContactPoint
 ////////////////////////////////////////////////////////////////////////////////////////////////////////
 
-void ContactConstraintManager::WorldContactPoint::CalculateNonPenetrationConstraintProperties(const Body &inBody1, float inInvMassScale1, float inInvInertiaScale1, const Body &inBody2, float inInvMassScale2, float inInvInertiaScale2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal)
+void ContactConstraintManager::WorldContactPoint::CalculateNonPenetrationConstraintProperties(const Body &inBody1, float inInvMass1, float inInvInertiaScale1, const Body &inBody2, float inInvMass2, 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.CalculateConstraintPropertiesWithMassScale(inBody1, inInvMassScale1, inInvInertiaScale1, r1, inBody2, inInvMassScale2, inInvInertiaScale2, r2, inWorldSpaceNormal);
+	mNonPenetrationConstraint.CalculateConstraintPropertiesWithMassOverride(inBody1, inInvMass1, inInvInertiaScale1, r1, inBody2, inInvMass2, inInvInertiaScale2, r2, inWorldSpaceNormal);
 }
 
 template <EMotionType Type1, EMotionType Type2>
@@ -649,31 +649,25 @@ template <EMotionType Type1, EMotionType Type2>
 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();
 	}
 
@@ -689,7 +683,7 @@ JPH_INLINE void ContactConstraintManager::TemplatedCalculateFrictionAndNonPenetr
 	{
 		RVec3 p1 = inTransformBody1 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition1);
 		RVec3 p2 = inTransformBody2 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition2);
-		wcp.TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<Type1, Type2>(inDeltaTime, inBody1, inBody2, inv_m1, inv_m2, inv_i1, inv_i2, p1, p2, ws_normal, t1, t2, inSettings, min_velocity_for_restitution);
+		wcp.TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<Type1, Type2>(inDeltaTime, inBody1, inBody2, ioConstraint.mInvMass1, ioConstraint.mInvMass2, inv_i1, inv_i2, p1, p2, ws_normal, t1, t2, inSettings, min_velocity_for_restitution);
 	}
 }
 
@@ -894,9 +888,9 @@ void ContactConstraintManager::GetContactsFromCache(ContactAllocator &ioContactA
 			constraint.mSortKey = input_hash;
 			world_space_normal.StoreFloat3(&constraint.mWorldSpaceNormal);
 			constraint.mCombinedFriction = settings.mCombinedFriction;
-			constraint.mInvMassScale1 = settings.mInvMassScale1;
+			constraint.mInvMass1 = body1->GetMotionPropertiesUnchecked() != nullptr? settings.mInvMassScale1 * body1->GetMotionPropertiesUnchecked()->GetInverseMassUnchecked() : 0.0f;
 			constraint.mInvInertiaScale1 = settings.mInvInertiaScale1;
-			constraint.mInvMassScale2 = settings.mInvMassScale2;
+			constraint.mInvMass2 = body2->GetMotionPropertiesUnchecked() != nullptr? settings.mInvMassScale2 * body2->GetMotionPropertiesUnchecked()->GetInverseMassUnchecked() : 0.0f;
 			constraint.mInvInertiaScale2 = settings.mInvInertiaScale2;
 			constraint.mContactPoints.resize(output_cm->mNumContactPoints);
 			for (uint32 i = 0; i < output_cm->mNumContactPoints; ++i)
@@ -1089,9 +1083,9 @@ bool ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &i
 		constraint.mSortKey = key_hash;
 		inManifold.mWorldSpaceNormal.StoreFloat3(&constraint.mWorldSpaceNormal);
 		constraint.mCombinedFriction = settings.mCombinedFriction;
-		constraint.mInvMassScale1 = settings.mInvMassScale1;
+		constraint.mInvMass1 = inBody1.GetMotionPropertiesUnchecked() != nullptr? settings.mInvMassScale1 * inBody1.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked() : 0.0f;
 		constraint.mInvInertiaScale1 = settings.mInvInertiaScale1;
-		constraint.mInvMassScale2 = settings.mInvMassScale2;
+		constraint.mInvMass2 = inBody2.GetMotionPropertiesUnchecked() != nullptr? settings.mInvMassScale2 * inBody2.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked() : 0.0f;
 		constraint.mInvInertiaScale2 = settings.mInvInertiaScale2;
 
 		JPH_DET_LOG("TemplatedAddContactConstraint: id1: " << constraint.mBody1->GetID() << " id2: " << constraint.mBody2->GetID() << " key: " << constraint.mSortKey);
@@ -1456,10 +1450,10 @@ JPH_INLINE void ContactConstraintManager::sWarmStartConstraint(ContactConstraint
 		if (wcp.mFrictionConstraint1.IsActive())
 		{
 			JPH_ASSERT(wcp.mFrictionConstraint2.IsActive());
-			wcp.mFrictionConstraint1.TemplatedWarmStart<Type1, Type2>(ioMotionProperties1, ioMotionProperties2, t1, inWarmStartImpulseRatio);
-			wcp.mFrictionConstraint2.TemplatedWarmStart<Type1, Type2>(ioMotionProperties1, ioMotionProperties2, t2, inWarmStartImpulseRatio);
+			wcp.mFrictionConstraint1.TemplatedWarmStart<Type1, Type2>(ioMotionProperties1, ioConstraint.mInvMass1, ioMotionProperties2, ioConstraint.mInvMass2, t1, inWarmStartImpulseRatio);
+			wcp.mFrictionConstraint2.TemplatedWarmStart<Type1, Type2>(ioMotionProperties1, ioConstraint.mInvMass1, ioMotionProperties2, ioConstraint.mInvMass2, t2, inWarmStartImpulseRatio);
 		}
-		wcp.mNonPenetrationConstraint.TemplatedWarmStart<Type1, Type2>(ioMotionProperties1, ioMotionProperties2, ws_normal, inWarmStartImpulseRatio);
+		wcp.mNonPenetrationConstraint.TemplatedWarmStart<Type1, Type2>(ioMotionProperties1, ioConstraint.mInvMass1, ioMotionProperties2, ioConstraint.mInvMass2, ws_normal, inWarmStartImpulseRatio);
 	}
 }
 
@@ -1533,9 +1527,9 @@ JPH_INLINE bool ContactConstraintManager::sSolveVelocityConstraint(ContactConstr
 			}
 
 			// Apply the friction impulse
-			if (wcp.mFrictionConstraint1.TemplatedSolveVelocityConstraintApplyLambda<Type1, Type2>(ioMotionProperties1, ioMotionProperties2, t1, lambda1))
+			if (wcp.mFrictionConstraint1.TemplatedSolveVelocityConstraintApplyLambda<Type1, Type2>(ioMotionProperties1, ioConstraint.mInvMass1, ioMotionProperties2, ioConstraint.mInvMass2, t1, lambda1))
 				any_impulse_applied = true;
-			if (wcp.mFrictionConstraint2.TemplatedSolveVelocityConstraintApplyLambda<Type1, Type2>(ioMotionProperties1, ioMotionProperties2, t2, lambda2))
+			if (wcp.mFrictionConstraint2.TemplatedSolveVelocityConstraintApplyLambda<Type1, Type2>(ioMotionProperties1, ioConstraint.mInvMass1, ioMotionProperties2, ioConstraint.mInvMass2, t2, lambda2))
 				any_impulse_applied = true;
 		}
 	}
@@ -1546,7 +1540,7 @@ JPH_INLINE bool ContactConstraintManager::sSolveVelocityConstraint(ContactConstr
 	for (WorldContactPoint &wcp : ioConstraint.mContactPoints)
 	{
 		// Solve non penetration velocities
-		if (wcp.mNonPenetrationConstraint.TemplatedSolveVelocityConstraint<Type1, Type2>(ioMotionProperties1, ioMotionProperties2, ws_normal, 0.0f, FLT_MAX))
+		if (wcp.mNonPenetrationConstraint.TemplatedSolveVelocityConstraint<Type1, Type2>(ioMotionProperties1, ioConstraint.mInvMass1, ioMotionProperties2, ioConstraint.mInvMass2, ws_normal, 0.0f, FLT_MAX))
 			any_impulse_applied = true;
 	}
 
@@ -1666,10 +1660,10 @@ bool ContactConstraintManager::SolvePositionConstraints(const uint32 *inConstrai
 			if (separation < 0.0f)
 			{
 				// Update constraint properties (bodies may have moved)
-				wcp.CalculateNonPenetrationConstraintProperties(body1, constraint.mInvMassScale1, constraint.mInvInertiaScale1, body2, constraint.mInvMassScale2, constraint.mInvInertiaScale2, p1, p2, ws_normal);
+				wcp.CalculateNonPenetrationConstraintProperties(body1, constraint.mInvMass1, constraint.mInvInertiaScale1, body2, constraint.mInvMass2, constraint.mInvInertiaScale2, p1, p2, ws_normal);
 
 				// Solve position errors
-				if (wcp.mNonPenetrationConstraint.SolvePositionConstraint(body1, body2, ws_normal, separation, mPhysicsSettings.mBaumgarte))
+				if (wcp.mNonPenetrationConstraint.SolvePositionConstraintWithMassOverride(body1, constraint.mInvMass1, body2, constraint.mInvMass2, ws_normal, separation, mPhysicsSettings.mBaumgarte))
 					any_impulse_applied = true;
 			}
 		}

Jolt/Physics/Constraints/ContactConstraintManager.h

@@ -420,7 +420,7 @@ private:
 	{
 	public:
 		/// Calculate constraint properties below
-		void					CalculateNonPenetrationConstraintProperties(const Body &inBody1, float inInvMassScale1, float inInvInertiaScale1, const Body &inBody2, float inInvMassScale2, float inInvInertiaScale2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal);
+		void					CalculateNonPenetrationConstraintProperties(const Body &inBody1, float inInvMass1, float inInvInertiaScale1, const Body &inBody2, float inInvMass2, 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,9 +464,9 @@ private:
 		uint64					mSortKey;
 		Float3					mWorldSpaceNormal;
 		float					mCombinedFriction;
-		float					mInvMassScale1;
+		float					mInvMass1;
 		float					mInvInertiaScale1;
-		float					mInvMassScale2;
+		float					mInvMass2;
 		float					mInvInertiaScale2;
 		WorldContactPoints		mContactPoints;
 	};

Jolt/Physics/PhysicsSystem.cpp

@@ -1990,10 +1990,14 @@ void PhysicsSystem::JobResolveCCDContacts(PhysicsUpdateContext *ioContext, Physi
 					else
 						normal_velocity_bias = 0.0f;
 
+					// Get inverse masses
+					float inv_m1 = contact_settings.mInvMassScale1 * body_mp->GetInverseMass();
+					float inv_m2 = body2.GetMotionPropertiesUnchecked() != nullptr? contact_settings.mInvMassScale2 * body2.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked() : 0.0f;
+
 					// Solve contact constraint
 					AxisConstraintPart contact_constraint;
-					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);
+					contact_constraint.CalculateConstraintPropertiesWithMassOverride(body1, inv_m1, contact_settings.mInvInertiaScale1, r1_plus_u, body2, inv_m2, contact_settings.mInvInertiaScale2, r2, ccd_body->mContactNormal, normal_velocity_bias);
+					contact_constraint.SolveVelocityConstraintWithMassOverride(body1, inv_m1, body2, inv_m2, ccd_body->mContactNormal, -FLT_MAX, FLT_MAX);
 
 					// Apply friction
 					if (contact_settings.mCombinedFriction > 0.0f)
@@ -2010,8 +2014,8 @@ void PhysicsSystem::JobResolveCCDContacts(PhysicsUpdateContext *ioContext, Physi
 							float max_lambda_f = contact_settings.mCombinedFriction * contact_constraint.GetTotalLambda();
 
 							AxisConstraintPart friction;
-							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);
+							friction.CalculateConstraintPropertiesWithMassOverride(body1, inv_m1, contact_settings.mInvInertiaScale1, r1_plus_u, body2, inv_m2, contact_settings.mInvInertiaScale2, r2, friction_direction);
+							friction.SolveVelocityConstraintWithMassOverride(body1, inv_m1, body2, inv_m2, friction_direction, -max_lambda_f, max_lambda_f);
 						}
 					}