Support for angular surface velocity (#601)

The contact listener can now also supply an angular surface velocity which allows implementing godot's constant_angular_velocity property, see godot-jolt/godot-jolt#331

Jolt/Physics/Collision/ContactListener.h

@@ -48,6 +48,7 @@ public:
 	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
+	Vec3					mRelativeAngularSurfaceVelocity = Vec3::sZero(); ///< Relative angular surface velocity between the bodies (world space angular surface velocity of body 2 - world space angular surface velocity of body 1)
 };
 
 /// Return value for the OnContactValidate callback. Determines if the contact is being processed or not.

Jolt/Physics/Constraints/ContactConstraintManager.cpp

@@ -44,12 +44,12 @@ void ContactConstraintManager::WorldContactPoint::CalculateNonPenetrationConstra
 }
 
 template <EMotionType Type1, EMotionType Type2>
-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_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, const ContactSettings &inSettings, float inMinVelocityForRestitution)
 {
 	JPH_DET_LOG("TemplatedCalculateFrictionAndNonPenetrationConstraintProperties: p1: " << inWorldSpacePosition1 << " p2: " << inWorldSpacePosition2
 		<< " normal: " << inWorldSpaceNormal << " tangent1: " << inWorldSpaceTangent1 << " tangent2: " << inWorldSpaceTangent2
-		<< " restitution: " << inCombinedRestitution << " friction: " << inCombinedFriction << " minv: " << inMinVelocityForRestitution
-		<< " surf1: " << inSurfaceVelocity1 << " surf2: " << inSurfaceVelocity2);
+		<< " restitution: " << inSettings.mCombinedRestitution << " friction: " << inSettings.mCombinedFriction << " minv: " << inMinVelocityForRestitution
+		<< " surface_vel: " << inSettings.mRelativeSurfaceVelocity << " surface_ang: " << inSettings.mRelativeAngularSurfaceVelocity);
 
 	// Calculate collision points relative to body
 	RVec3 p = 0.5_r * (inWorldSpacePosition1 + inWorldSpacePosition2);
@@ -82,7 +82,7 @@ JPH_INLINE void ContactConstraintManager::WorldContactPoint::TemplatedCalculateF
 
 	// Determine if the velocity is big enough for restitution
 	float normal_velocity_bias;
-	if (inCombinedRestitution > 0.0f && normal_velocity < -inMinVelocityForRestitution)
+	if (inSettings.mCombinedRestitution > 0.0f && normal_velocity < -inMinVelocityForRestitution)
 	{
 		// We have a velocity that is big enough for restitution. This is where speculative contacts don't work
 		// great as we have to decide now if we're going to apply the restitution or not. If the relative
@@ -92,7 +92,7 @@ JPH_INLINE void ContactConstraintManager::WorldContactPoint::TemplatedCalculateF
 		// position rather than from a position where it is touching the other object. This causes the object
 		// to appear to move faster for 1 frame (the opposite of time stealing).
 		if (normal_velocity < -speculative_contact_velocity_bias)
-			normal_velocity_bias = inCombinedRestitution * normal_velocity;
+			normal_velocity_bias = inSettings.mCombinedRestitution * normal_velocity;
 		else
 			// In this case we have predicted that we don't hit the other object, but if we do (due to other constraints changing velocities)
 			// the speculative contact will prevent penetration but will not apply restitution leading to another artifact.
@@ -107,11 +107,16 @@ JPH_INLINE void ContactConstraintManager::WorldContactPoint::TemplatedCalculateF
 	mNonPenetrationConstraint.TemplatedCalculateConstraintProperties<Type1, Type2>(inInvM1, inInvI1, r1, inInvM2, inInvI2, r2, inWorldSpaceNormal, normal_velocity_bias);
 
 	// Calculate friction part
-	if (inCombinedFriction > 0.0f)
+	if (inSettings.mCombinedFriction > 0.0f)
 	{
+		// Get surface velocity relative to tangents
+		Vec3 ws_surface_velocity = inSettings.mRelativeSurfaceVelocity + inSettings.mRelativeAngularSurfaceVelocity.Cross(r1);
+		float surface_velocity1 = inWorldSpaceTangent1.Dot(ws_surface_velocity);
+		float surface_velocity2 = inWorldSpaceTangent2.Dot(ws_surface_velocity);
+
 		// Implement friction as 2 AxisContraintParts
-		mFrictionConstraint1.TemplatedCalculateConstraintProperties<Type1, Type2>(inInvM1, inInvI1, r1, inInvM2, inInvI2, r2, inWorldSpaceTangent1, inSurfaceVelocity1);
-		mFrictionConstraint2.TemplatedCalculateConstraintProperties<Type1, Type2>(inInvM1, inInvI1, r1, inInvM2, inInvI2, r2, inWorldSpaceTangent2, inSurfaceVelocity2);
+		mFrictionConstraint1.TemplatedCalculateConstraintProperties<Type1, Type2>(inInvM1, inInvI1, r1, inInvM2, inInvI2, r2, inWorldSpaceTangent1, surface_velocity1);
+		mFrictionConstraint2.TemplatedCalculateConstraintProperties<Type1, Type2>(inInvM1, inInvI1, r1, inInvM2, inInvI2, r2, inWorldSpaceTangent2, surface_velocity2);
 	}
 	else
 	{
@@ -656,10 +661,6 @@ JPH_INLINE void ContactConstraintManager::TemplatedCalculateFrictionAndNonPenetr
 	Vec3 t1, t2;
 	ioConstraint.GetTangents(t1, t2);
 
-	// Get surface velocity relative to tangents
-	float surface_velocity1 = t1.Dot(inSettings.mRelativeSurfaceVelocity);
-	float surface_velocity2 = t2.Dot(inSettings.mRelativeSurfaceVelocity);
-
 	Vec3 ws_normal = ioConstraint.GetWorldSpaceNormal();
 
 	// Setup velocity constraint properties
@@ -668,7 +669,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.mCombinedRestitution, inSettings.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, inSettings, min_velocity_for_restitution);
 	}
 }
 
@@ -1106,10 +1107,6 @@ bool ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &i
 		Vec3 t1, t2;
 		constraint.GetTangents(t1, t2);
 
-		// Get surface velocity relative to tangents
-		float surface_velocity1 = t1.Dot(settings.mRelativeSurfaceVelocity);
-		float surface_velocity2 = t2.Dot(settings.mRelativeSurfaceVelocity);
-
 		constraint.mContactPoints.resize(num_contact_points);
 		for (int i = 0; i < num_contact_points; ++i)
 		{
@@ -1149,7 +1146,7 @@ bool ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &i
 			wcp.mContactPoint = &cp;
 
 			// Setup velocity constraint
-			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);
+			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, mPhysicsSettings.mMinVelocityForRestitution);
 		}
 
 	#ifdef JPH_DEBUG_RENDERER

Jolt/Physics/Constraints/ContactConstraintManager.h

@@ -421,7 +421,7 @@ private:
 		void					CalculateNonPenetrationConstraintProperties(const Body &inBody1, const Body &inBody2, 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, 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, const ContactSettings &inSettings, float inMinVelocityForRestitution);
 
 		/// The constraint parts
 		AxisConstraintPart		mNonPenetrationConstraint;

Samples/Tests/General/ConveyorBeltTest.cpp

@@ -29,7 +29,7 @@ void ConveyorBeltTest::Initialize()
 		belt_settings.mFriction = 0.25f * (i + 1);
 		belt_settings.mRotation = Quat::sRotation(Vec3::sAxisY(), 0.5f * JPH_PI * i) * Quat::sRotation(Vec3::sAxisX(), DegreesToRadians(1.0f));
 		belt_settings.mPosition = RVec3(belt_settings.mRotation * Vec3(cBeltLength, 6.0f, cBeltWidth));
-		mBelts.push_back(mBodyInterface->CreateAndAddBody(belt_settings, EActivation::DontActivate));
+		mLinearBelts.push_back(mBodyInterface->CreateAndAddBody(belt_settings, EActivation::DontActivate));
 	}
 
 	// Bodies with decreasing friction
@@ -42,32 +42,62 @@ void ConveyorBeltTest::Initialize()
 	}
 
 	// Create 2 cylinders
-	BodyCreationSettings cylinder_settings(new CylinderShape(6.0f, 1.0f), RVec3(0, 1.0f, -20.0f), Quat::sRotation(Vec3::sAxisZ(), 0.5f * JPH_PI), EMotionType::Dynamic, Layers::MOVING);
+	BodyCreationSettings cylinder_settings(new CylinderShape(6.0f, 1.0f), RVec3(-25.0f, 1.0f, -20.0f), Quat::sRotation(Vec3::sAxisZ(), 0.5f * JPH_PI), EMotionType::Dynamic, Layers::MOVING);
 	mBodyInterface->CreateAndAddBody(cylinder_settings, EActivation::Activate);
 	cylinder_settings.mPosition.SetZ(20.0f);
 	mBodyInterface->CreateAndAddBody(cylinder_settings, EActivation::Activate);
 
 	// Let a dynamic belt rest on it
-	BodyCreationSettings dynamic_belt(new BoxShape(Vec3(5.0f, 0.1f, 25.0f), 0.0f), RVec3(0, 3.0f, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
-	mBelts.push_back(mBodyInterface->CreateAndAddBody(dynamic_belt, EActivation::Activate));
+	BodyCreationSettings dynamic_belt(new BoxShape(Vec3(5.0f, 0.1f, 25.0f), 0.0f), RVec3(-25.0f, 3.0f, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+	mLinearBelts.push_back(mBodyInterface->CreateAndAddBody(dynamic_belt, EActivation::Activate));
 
 	// Create cargo on the dynamic belt
-	cargo_settings.mPosition = RVec3(0, 6.0f, 15.0f);
+	cargo_settings.mPosition = RVec3(-25.0f, 6.0f, 15.0f);
 	cargo_settings.mFriction = 1.0f;
 	mBodyInterface->CreateAndAddBody(cargo_settings, EActivation::Activate);
+
+	// Create an angular belt
+	BodyCreationSettings angular_belt(new BoxShape(Vec3(20.0f, 0.1f, 20.0f), 0.0f), RVec3(10.0f, 3.0f, 0), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+	mAngularBelt = mBodyInterface->CreateAndAddBody(angular_belt, EActivation::Activate);
+
+	// Bodies with decreasing friction dropping on the angular belt
+	for (int i = 0; i <= 6; ++i)
+	{
+		cargo_settings.mPosition = RVec3(10.0f, 10.0f, -15.0f + 5.0f * i);
+		cargo_settings.mFriction = 1.0f - 0.1f * i;
+		mBodyInterface->CreateAndAddBody(cargo_settings, EActivation::Activate);
+	}
 }
 
 void ConveyorBeltTest::OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings)
 {
-	// Determine the world space surface velocity of both bodies
-	const Vec3 cLocalSpaceVelocity(0, 0, -10.0f);
-	bool body1_belt = std::find(mBelts.begin(), mBelts.end(), inBody1.GetID()) != mBelts.end();
-	Vec3 body1_surface_velocity = body1_belt? inBody1.GetRotation() * cLocalSpaceVelocity : Vec3::sZero();
-	bool body2_belt = std::find(mBelts.begin(), mBelts.end(), inBody2.GetID()) != mBelts.end();
-	Vec3 body2_surface_velocity = body2_belt? inBody2.GetRotation() * cLocalSpaceVelocity : Vec3::sZero();
+	// Linear belts
+	bool body1_linear_belt = std::find(mLinearBelts.begin(), mLinearBelts.end(), inBody1.GetID()) != mLinearBelts.end();
+	bool body2_linear_belt = std::find(mLinearBelts.begin(), mLinearBelts.end(), inBody2.GetID()) != mLinearBelts.end();
+	if (body1_linear_belt || body2_linear_belt)
+	{
+		// Determine the world space surface velocity of both bodies
+		const Vec3 cLocalSpaceVelocity(0, 0, -10.0f);
+		Vec3 body1_linear_surface_velocity = body1_linear_belt? inBody1.GetRotation() * cLocalSpaceVelocity : Vec3::sZero();
+		Vec3 body2_linear_surface_velocity = body2_linear_belt? inBody2.GetRotation() * cLocalSpaceVelocity : Vec3::sZero();
 
-	// Calculate the relative surface velocity
-	ioSettings.mRelativeSurfaceVelocity = body2_surface_velocity - body1_surface_velocity;
+		// Calculate the relative surface velocity
+		ioSettings.mRelativeSurfaceVelocity = body2_linear_surface_velocity - body1_linear_surface_velocity;
+	}
+
+	// Angular belt
+	bool body1_angular = inBody1.GetID() == mAngularBelt;
+	bool body2_angular = inBody2.GetID() == mAngularBelt;
+	if (body1_angular || body2_angular)
+	{		
+		// Determine the world space angular surface velocity of both bodies
+		const Vec3 cLocalSpaceAngularVelocity(0, DegreesToRadians(10.0f), 0);
+		Vec3 body1_angular_surface_velocity = body1_angular? inBody1.GetRotation() * cLocalSpaceAngularVelocity : Vec3::sZero();
+		Vec3 body2_angular_surface_velocity = body2_angular? inBody2.GetRotation() * cLocalSpaceAngularVelocity : Vec3::sZero();
+		
+		// Calculate the relative angular surface velocity
+		ioSettings.mRelativeAngularSurfaceVelocity = body2_angular_surface_velocity - body1_angular_surface_velocity;
+	}
 }
 
 void ConveyorBeltTest::OnContactPersisted(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings)

Samples/Tests/General/ConveyorBeltTest.h

@@ -23,5 +23,6 @@ public:
 	virtual void			OnContactPersisted(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override;
 
 private:
-	BodyIDVector			mBelts;
+	BodyIDVector			mLinearBelts;
+	BodyID					mAngularBelt;	
 };

UnitTests/Physics/ContactListenerTests.cpp

@@ -373,44 +373,53 @@ TEST_SUITE("ContactListenerTests")
 		Body &floor = c.CreateBox(RVec3(0, -1, 0), Quat::sRotation(Vec3::sAxisY(), DegreesToRadians(10.0f)), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, Vec3(100.0f, 1.0f, 100.0f));
 		floor.SetFriction(1.0f);
 
-		Body &box = c.CreateBox(RVec3(0, 0.999f, 0), Quat::sRotation(Vec3::sAxisY(), DegreesToRadians(30.0f)), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(1.0f));
-		box.SetFriction(1.0f);
-
-		// Contact listener sets a constant surface velocity
-		class ContactListenerImpl : public ContactListener
+		for (int iteration = 0; iteration < 2; ++iteration)
 		{
-		public:
-							ContactListenerImpl(Body &inFloor, Body &inBox) : mFloor(inFloor), mBox(inBox) { }
+			Body &box = c.CreateBox(RVec3(0, 0.999f, 0), Quat::sRotation(Vec3::sAxisY(), DegreesToRadians(30.0f)), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(1.0f));
+			box.SetFriction(1.0f);
 
-			virtual void	OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+			// Contact listener sets a constant surface velocity
+			class ContactListenerImpl : public ContactListener
 			{
-				// Ensure that the body order is as expected
-				JPH_ASSERT(inBody1.GetID() == mFloor.GetID() || inBody2.GetID() == mBox.GetID());
+			public:
+								ContactListenerImpl(Body &inFloor, Body &inBox) : mFloor(inFloor), mBox(inBox) { }
 
-				// Calculate the relative surface velocity
-				ioSettings.mRelativeSurfaceVelocity = -(inBody1.GetRotation() * mLocalSpaceVelocity);
-			}
+				virtual void	OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+				{
+					// Ensure that the body order is as expected
+					JPH_ASSERT(inBody1.GetID() == mFloor.GetID() || inBody2.GetID() == mBox.GetID());
 
-			virtual void	OnContactPersisted(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
-			{
-				OnContactAdded(inBody1, inBody2, inManifold, ioSettings);
-			}
+					// Calculate the relative surface velocity
+					ioSettings.mRelativeSurfaceVelocity = -(inBody1.GetRotation() * mLocalSpaceLinearVelocity);
+					ioSettings.mRelativeAngularSurfaceVelocity = -(inBody1.GetRotation() * mLocalSpaceAngularVelocity);
+				}
 
-			Body &			mFloor;
-			Body &			mBox;
-			Vec3			mLocalSpaceVelocity = Vec3(0, 0, -2.0f);
-		};
+				virtual void	OnContactPersisted(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+				{
+					OnContactAdded(inBody1, inBody2, inManifold, ioSettings);
+				}
 
-		// Set listener
-		ContactListenerImpl listener(floor, box);
-		c.GetSystem()->SetContactListener(&listener);
+				Body &			mFloor;
+				Body &			mBox;
+				Vec3			mLocalSpaceLinearVelocity;
+				Vec3			mLocalSpaceAngularVelocity;
+			};
 
-		// Simulate
-		c.Simulate(5.0f);
+			// Set listener
+			ContactListenerImpl listener(floor, box);
+			c.GetSystem()->SetContactListener(&listener);
+
+			// Set linear velocity or angular velocity depending on the iteration
+			listener.mLocalSpaceLinearVelocity = iteration == 0? Vec3(0, 0, -2.0f) : Vec3::sZero();
+			listener.mLocalSpaceAngularVelocity = iteration == 0? Vec3::sZero() : Vec3(0, DegreesToRadians(30.0f), 0);
 
-		// Check that the box is moving
-		CHECK_APPROX_EQUAL(box.GetLinearVelocity(), floor.GetRotation() * listener.mLocalSpaceVelocity, 0.005f);
-		CHECK_APPROX_EQUAL(box.GetAngularVelocity(), Vec3::sZero(), 1.0e-4f);
+			// Simulate
+			c.Simulate(5.0f);
+
+			// Check that the box is moving with the correct linear/angular velocity
+			CHECK_APPROX_EQUAL(box.GetLinearVelocity(), floor.GetRotation() * listener.mLocalSpaceLinearVelocity, 0.005f);
+			CHECK_APPROX_EQUAL(box.GetAngularVelocity(), floor.GetRotation() * listener.mLocalSpaceAngularVelocity, 1.0e-4f);
+		}
 	}
 
 	static float sGetInvMassScale(const Body &inBody)