Fixed bug in friction approximation (#572)

The friction was clamped independently on both tangential axis which meant that the total friction could be larger than the amount of friction that should have been allowed. It also meant that an object would slow down quicker on one axis than on another causing a curved trajectory.

Fixes #569

.github/workflows/determinism_check.yml

@@ -1,8 +1,8 @@
 name: Determinism Check
 
 env:
-    CONVEX_VS_MESH_HASH: '0x219df2ceee6baff2'
-    RAGDOLL_HASH: '0x9bace3bd537e6759'
+    CONVEX_VS_MESH_HASH: '0x694f34b44c755f7'
+    RAGDOLL_HASH: '0xea156c841e796fc9'
 
 on:
   push:

Jolt/Physics/Collision/EstimateCollisionResponse.cpp

@@ -87,24 +87,37 @@ void EstimateCollisionResponse(const Body &inBody1, const Body &inBody2, const C
 			mBias = 0.0f;
 		}
 
-		inline void		Solve(Vec3Arg inWorldSpaceNormal, float inInvM1, float inInvM2, float inMinLambda, float inMaxLambda, float &ioTotalLambda, CollisionEstimationResult &ioResult) const
+		inline float	SolveGetLambda(Vec3Arg inWorldSpaceNormal, const CollisionEstimationResult &inResult) const
 		{
 			// Calculate jacobian multiplied by linear/angular velocity
-			float jv = inWorldSpaceNormal.Dot(ioResult.mLinearVelocity1 - ioResult.mLinearVelocity2) + mR1PlusUxAxis.Dot(ioResult.mAngularVelocity1) - mR2xAxis.Dot(ioResult.mAngularVelocity2);
+			float jv = inWorldSpaceNormal.Dot(inResult.mLinearVelocity1 - inResult.mLinearVelocity2) + mR1PlusUxAxis.Dot(inResult.mAngularVelocity1) - mR2xAxis.Dot(inResult.mAngularVelocity2);
 
 			// Lagrange multiplier is:
 			//
 			// lambda = -K^-1 (J v + b)
-			float lambda = mEffectiveMass * (jv - mBias);
-			float new_lambda = Clamp(ioTotalLambda + lambda, inMinLambda, inMaxLambda); // Clamp impulse
-			lambda = new_lambda - ioTotalLambda; // Lambda potentially got clamped, calculate the new impulse to apply
-			ioTotalLambda = new_lambda; // Store accumulated impulse
+			return mEffectiveMass * (jv - mBias);
+		}
 
+		inline void		SolveApplyLambda(Vec3Arg inWorldSpaceNormal, float inInvM1, float inInvM2, float inLambda, CollisionEstimationResult &ioResult) const
+		{
 			// Apply impulse to body velocities
-			ioResult.mLinearVelocity1 -= (lambda * inInvM1) * inWorldSpaceNormal;
-			ioResult.mAngularVelocity1 -= lambda * mInvI1_R1PlusUxAxis;
-			ioResult.mLinearVelocity2 += (lambda * inInvM2) * inWorldSpaceNormal;
-			ioResult.mAngularVelocity2 += lambda * mInvI2_R2xAxis;
+			ioResult.mLinearVelocity1 -= (inLambda * inInvM1) * inWorldSpaceNormal;
+			ioResult.mAngularVelocity1 -= inLambda * mInvI1_R1PlusUxAxis;
+			ioResult.mLinearVelocity2 += (inLambda * inInvM2) * inWorldSpaceNormal;
+			ioResult.mAngularVelocity2 += inLambda * mInvI2_R2xAxis;
+		}
+
+		inline void		Solve(Vec3Arg inWorldSpaceNormal, float inInvM1, float inInvM2, float inMinLambda, float inMaxLambda, float &ioTotalLambda, CollisionEstimationResult &ioResult) const
+		{
+			// Calculate new total lambda
+			float total_lambda = ioTotalLambda + SolveGetLambda(inWorldSpaceNormal, ioResult);
+
+			// Clamp impulse
+			total_lambda = Clamp(total_lambda, inMinLambda, inMaxLambda);
+
+			SolveApplyLambda(inWorldSpaceNormal, inInvM1, inInvM2, total_lambda - ioTotalLambda, ioResult);
+
+			ioTotalLambda = total_lambda;
 		}
 
 		Vec3			mR1PlusUxAxis;
@@ -169,9 +182,26 @@ void EstimateCollisionResponse(const Body &inBody1, const Body &inBody2, const C
 				const Constraint &constraint = constraints[c];
 				CollisionEstimationResult::Impulse &impulse = outResult.mImpulses[c];
 
+				float lambda1 = impulse.mFrictionImpulse1 + constraint.mFriction1.SolveGetLambda(outResult.mTangent1, outResult);
+				float lambda2 = impulse.mFrictionImpulse2 + constraint.mFriction2.SolveGetLambda(outResult.mTangent2, outResult);
+
+				// Caclulate max impulse based on contact impulse
 				float max_impulse = inCombinedFriction * impulse.mContactImpulse;
-				constraint.mFriction1.Solve(outResult.mTangent1, inv_m1, inv_m2, -max_impulse, max_impulse, impulse.mFrictionImpulse1, outResult);
-				constraint.mFriction2.Solve(outResult.mTangent2, inv_m1, inv_m2, -max_impulse, max_impulse, impulse.mFrictionImpulse2, outResult);
+
+				// If the total lambda that we will apply is too large, scale it back
+				float total_lambda_sq = Square(lambda1) + Square(lambda2);
+				if (total_lambda_sq > Square(max_impulse))
+				{
+					float scale = max_impulse / sqrt(total_lambda_sq);
+					lambda1 *= scale;
+					lambda2 *= scale;
+				}
+
+				constraint.mFriction1.SolveApplyLambda(outResult.mTangent1, inv_m1, inv_m2, lambda1 - impulse.mFrictionImpulse1, outResult);
+				constraint.mFriction2.SolveApplyLambda(outResult.mTangent2, inv_m1, inv_m2, lambda2 - impulse.mFrictionImpulse2, outResult);
+
+				impulse.mFrictionImpulse1 = lambda1;
+				impulse.mFrictionImpulse2 = lambda2;
 			}
 
 		// Solve contact constraints last

Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h

@@ -292,9 +292,9 @@ public:
 		}
 	}
 
-	/// Templated form of SolveVelocityConstraint with the motion types baked in
+	/// Templated form of SolveVelocityConstraint with the motion types baked in, part 1: get the total lambda
 	template <EMotionType Type1, EMotionType Type2>
-	inline bool					TemplatedSolveVelocityConstraint(MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2, Vec3Arg inWorldSpaceAxis, float inMinLambda, float inMaxLambda)
+	JPH_INLINE float			TemplatedSolveVelocityConstraintGetTotalLambda(const MotionProperties *ioMotionProperties1, const MotionProperties *ioMotionProperties2, Vec3Arg inWorldSpaceAxis) const
 	{
 		// Calculate jacobian multiplied by linear velocity
 		float jv;
@@ -317,11 +317,31 @@ public:
 		//
 		// lambda = -K^-1 (J v + b)
 		float lambda = mEffectiveMass * (jv - mSpringPart.GetBias(mTotalLambda));
-		float new_lambda = Clamp(mTotalLambda + lambda, inMinLambda, inMaxLambda); // Clamp impulse
-		lambda = new_lambda - mTotalLambda; // Lambda potentially got clamped, calculate the new impulse to apply
-		mTotalLambda = new_lambda; // Store accumulated impulse
 
-		return ApplyVelocityStep<Type1, Type2>(ioMotionProperties1, ioMotionProperties2, inWorldSpaceAxis, lambda);
+		// Return the total accumulated lambda
+		return mTotalLambda + lambda;
+	}
+
+	/// 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)
+	{
+		float delta_lambda = inTotalLambda - mTotalLambda; // Calculate change in lambda
+		mTotalLambda = inTotalLambda; // Store accumulated impulse
+
+		return ApplyVelocityStep<Type1, Type2>(ioMotionProperties1, ioMotionProperties2, 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)
+	{
+		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);
 	}
 
 	/// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation.

Jolt/Physics/Constraints/ContactConstraintManager.cpp

@@ -1457,16 +1457,28 @@ JPH_INLINE bool ContactConstraintManager::sSolveVelocityConstraint(ContactConstr
 		{
 			JPH_ASSERT(wcp.mFrictionConstraint2.IsActive());
 
+			// Calculate impulse to stop motion in tangential direction
+			float lambda1 = wcp.mFrictionConstraint1.TemplatedSolveVelocityConstraintGetTotalLambda<Type1, Type2>(ioMotionProperties1, ioMotionProperties2, t1);
+			float lambda2 = wcp.mFrictionConstraint2.TemplatedSolveVelocityConstraintGetTotalLambda<Type1, Type2>(ioMotionProperties1, ioMotionProperties2, t2);
+			float total_lambda_sq = Square(lambda1) + Square(lambda2);
+
 			// Calculate max impulse that can be applied. Note that we're using the non-penetration impulse from the previous iteration here.
 			// We do this because non-penetration is more important so is solved last (the last things that are solved in an iterative solver
 			// contribute the most).
 			float max_lambda_f = ioConstraint.mCombinedFriction * wcp.mNonPenetrationConstraint.GetTotalLambda();
 
-			// Solve friction velocities
-			// Note that what we're doing is not fully correct since the max force we can apply is 2 * max_lambda_f instead of max_lambda_f since we're solving axis independently
-			if (wcp.mFrictionConstraint1.TemplatedSolveVelocityConstraint<Type1, Type2>(ioMotionProperties1, ioMotionProperties2, t1, -max_lambda_f, max_lambda_f))
+			// If the total lambda that we will apply is too large, scale it back
+			if (total_lambda_sq > Square(max_lambda_f))
+			{
+				float scale = max_lambda_f / sqrt(total_lambda_sq);
+				lambda1 *= scale;
+				lambda2 *= scale;
+			}
+
+			// Apply the friction impulse
+			if (wcp.mFrictionConstraint1.TemplatedSolveVelocityConstraintApplyLambda<Type1, Type2>(ioMotionProperties1, ioMotionProperties2, t1, lambda1))
 				any_impulse_applied = true;
-			if (wcp.mFrictionConstraint2.TemplatedSolveVelocityConstraint<Type1, Type2>(ioMotionProperties1, ioMotionProperties2, t2, -max_lambda_f, max_lambda_f))
+			if (wcp.mFrictionConstraint2.TemplatedSolveVelocityConstraintApplyLambda<Type1, Type2>(ioMotionProperties1, ioMotionProperties2, t2, lambda2))
 				any_impulse_applied = true;
 		}
 	}

UnitTests/Physics/PhysicsTests.cpp

@@ -1435,4 +1435,50 @@ TEST_SUITE("PhysicsTests")
 		}
 		CHECK((errors & EPhysicsUpdateError::ContactConstraintsFull) != EPhysicsUpdateError::None);
 	}
+
+	TEST_CASE("TestFriction")
+	{
+		const float friction_floor = 0.9f;
+		const float friction_box = 0.8f;
+		const float combined_friction = sqrt(friction_floor * friction_box);
+
+		for (float angle = 0; angle < 360.0f; angle += 30.0f)
+		{
+			// Create a context with space for 8 constraints
+			PhysicsTestContext c(1.0f / 60.0f, 1, 1, 0, 1024, 4096, 8);
+
+			// Create floor
+			Body &floor = c.CreateFloor();
+			floor.SetFriction(friction_floor);
+
+			// Create box with a velocity that will make it slide over the floor (making sure it intersects a little bit initially)
+			BodyCreationSettings box_settings(new BoxShape(Vec3::sReplicate(1.0f)), RVec3(0, 0.999_r, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+			box_settings.mFriction = friction_box;
+			box_settings.mLinearDamping = 0;
+			box_settings.mLinearVelocity = Vec3(Sin(DegreesToRadians(angle)), 0, Cos(DegreesToRadians(angle))) * 20.0f;
+			Body &box = *c.GetBodyInterface().CreateBody(box_settings);
+			c.GetBodyInterface().AddBody(box.GetID(), EActivation::Activate);
+
+			// We know that the friction force equals the normal force times the friction coefficient
+			float friction_acceleration = combined_friction * c.GetSystem()->GetGravity().Length();
+
+			// Simulate
+			Vec3 velocity = box_settings.mLinearVelocity;
+			RVec3 position = box_settings.mPosition;
+			for (int i = 0; i < 60; ++i)
+			{
+				c.SimulateSingleStep();
+
+				// Integrate our own simulation
+				velocity -= velocity.Normalized() * friction_acceleration * c.GetDeltaTime();
+				position += velocity * c.GetDeltaTime();
+			}
+
+			// Note that the result is not very accurate so we need quite a high tolerance
+			CHECK_APPROX_EQUAL(box.GetCenterOfMassPosition(), position, 1.0e-2f);
+			CHECK_APPROX_EQUAL(box.GetRotation(), box_settings.mRotation, 1.0e-2f);
+			CHECK_APPROX_EQUAL(box.GetLinearVelocity(), velocity, 2.0e-2f);
+			CHECK_APPROX_EQUAL(box.GetAngularVelocity(), Vec3::sZero(), 1.0e-2f);
+		}
+	}
 }