Allowed DOFs now work in world space for rotation (#931)

Previously the rotation DOFs were restricted in local space. This differs from what other physics engines offer so was leading to problems when integrating Jolt and also led to issues in combination with constraints.

Fixes #907

Jolt/Physics/Body/AllowedDOFs.h

@@ -14,9 +14,9 @@ enum class EAllowedDOFs : uint8
 	TranslationX		= 0b000001,									///< Body can move in world space X axis
 	TranslationY		= 0b000010,									///< Body can move in world space Y axis
 	TranslationZ		= 0b000100,									///< Body can move in world space Z axis
-	RotationX			= 0b001000,									///< Body can rotate around local space X axis
-	RotationY			= 0b010000,									///< Body can rotate around local space Y axis
-	RotationZ			= 0b100000,									///< Body can rotate around local space Z axis
+	RotationX			= 0b001000,									///< Body can rotate around world space X axis
+	RotationY			= 0b010000,									///< Body can rotate around world space Y axis
+	RotationZ			= 0b100000,									///< Body can rotate around world space Z axis
 	Plane2D				= TranslationX | TranslationY | RotationZ,	///< Body can only move in X and Y axis and rotate around Z axis
 };
 

Jolt/Physics/Body/MotionProperties.cpp

@@ -30,7 +30,13 @@ void MotionProperties::SetMassProperties(EAllowedDOFs inAllowedDOFs, const MassP
 		mInvMass = 1.0f / inMassProperties.mMass;
 	}
 
-	if (allowed_rotation_axis == 0b111)
+	if (allowed_rotation_axis == 0)
+	{
+		// No rotation possible
+		mInvInertiaDiagonal = Vec3::sZero();
+		mInertiaRotation = Quat::sIdentity();
+	}
+	else
 	{
 		// Set inverse inertia
 		Mat44 rotation;
@@ -48,74 +54,6 @@ void MotionProperties::SetMassProperties(EAllowedDOFs inAllowedDOFs, const MassP
 			mInertiaRotation = Quat::sIdentity();
 		}
 	}
-	else if (allowed_rotation_axis == 0)
-	{
-		// No rotation possible
-		mInvInertiaDiagonal = Vec3::sZero();
-		mInertiaRotation = Quat::sIdentity();
-	}
-	else
-	{
-		uint num_allowed_rotation_axis = CountBits(allowed_rotation_axis);
-		if (num_allowed_rotation_axis == 1)
-		{
-			// We can only rotate around one axis so the inverse inertia is trivial to calculate
-			mInertiaRotation = Quat::sIdentity();
-			mInvInertiaDiagonal = Vec3::sZero();
-			for (int axis = 0; axis < 3; ++axis)
-				if ((allowed_rotation_axis & (1 << axis)) != 0)
-					mInvInertiaDiagonal.SetComponent(axis, 1.0f / inMassProperties.mInertia(axis, axis));
-		}
-		else
-		{
-			JPH_ASSERT(num_allowed_rotation_axis == 2);
-			uint locked_axis = CountTrailingZeros(~allowed_rotation_axis);
-
-			// Copy the mass properties so we can modify it
-			MassProperties copy = inMassProperties;
-			Mat44 &inertia = copy.mInertia;
-
-			// Set the locked row and column to 0
-			for (uint axis = 0; axis < 3; ++axis)
-			{
-				inertia(axis, locked_axis) = 0.0f;
-				inertia(locked_axis, axis) = 0.0f;
-			}
-
-			// Set the diagonal entry to 1
-			inertia(locked_axis, locked_axis) = 1.0f;
-
-			// Decompose the inertia matrix, note that using a 2x2 matrix would have been more efficient
-			// but we happen to have a 3x3 matrix version lying around so we use that.
-			Mat44 rotation;
-			Vec3 diagonal;
-			if (copy.DecomposePrincipalMomentsOfInertia(rotation, diagonal))
-			{
-				mInvInertiaDiagonal = diagonal.Reciprocal();
-				mInertiaRotation = rotation.GetQuaternion();
-
-				// Now set the diagonal entry corresponding to the locked axis to 0
-				for (uint axis = 0; axis < 3; ++axis)
-					if (abs(inertia.GetColumn3(locked_axis).Dot(rotation.GetColumn3(axis))) > 0.999f)
-					{
-						mInvInertiaDiagonal.SetComponent(axis, 0.0f);
-						break;
-					}
-
-				// Check that we placed a zero
-				JPH_ASSERT(Vec3::sEquals(mInvInertiaDiagonal, Vec3::sZero()).TestAnyXYZTrue());
-			}
-			else
-			{
-				// Failed! Fall back to inaccurate version.
-				mInertiaRotation = Quat::sIdentity();
-				mInvInertiaDiagonal = Vec3::sZero();
-				for (uint axis = 0; axis < 3; ++axis)
-					if (axis != locked_axis)
-						mInvInertiaDiagonal.SetComponent(axis, 1.0f / inertia.GetColumn3(axis).Length());
-			}
-		}
-	}
 
 	JPH_ASSERT(mInvMass != 0.0f || mInvInertiaDiagonal != Vec3::sZero(), "Can't lock all axes, use a static body for this. This will crash with a division by zero later!");
 }

Jolt/Physics/Body/MotionProperties.h

@@ -52,10 +52,10 @@ public:
 	inline Vec3				GetAngularVelocity() const										{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::Read)); return mAngularVelocity; }
 
 	/// Set world space angular velocity of the center of mass
-	void					SetAngularVelocity(Vec3Arg inAngularVelocity)					{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite)); JPH_ASSERT(inAngularVelocity.Length() <= mMaxAngularVelocity); mAngularVelocity = inAngularVelocity; }
+	void					SetAngularVelocity(Vec3Arg inAngularVelocity)					{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite)); JPH_ASSERT(inAngularVelocity.Length() <= mMaxAngularVelocity); mAngularVelocity = LockAngular(inAngularVelocity); }
 
 	/// Set world space angular velocity of the center of mass, will make sure the value is clamped against the maximum angular velocity
-	void					SetAngularVelocityClamped(Vec3Arg inAngularVelocity)			{ mAngularVelocity = inAngularVelocity; ClampAngularVelocity(); }
+	void					SetAngularVelocityClamped(Vec3Arg inAngularVelocity)			{ mAngularVelocity = LockAngular(inAngularVelocity); ClampAngularVelocity(); }
 
 	/// Set velocity of body such that it will be rotate/translate by inDeltaPosition/Rotation in inDeltaTime seconds.
 	inline void				MoveKinematic(Vec3Arg inDeltaPosition, QuatArg inDeltaRotation, float inDeltaTime);
@@ -110,7 +110,7 @@ public:
 	/// Set the inverse inertia tensor in local space by setting the diagonal and the rotation: \f$I_{body}^{-1} = R \: D \: R^{-1}\f$.
 	/// Note that mass and inertia are linearly related (e.g. inertia of a sphere with mass m and radius r is \f$2/5 \: m \: r^2\f$).
 	/// If you change inertia, mass should probably change as well. See MassProperties::ScaleToMass.
-	/// If you don't allow all rotational degrees of freedom, make sure that the corresponding diagonal elements are zero (see EAllowedDOFs).
+	/// If all your rotation degrees of freedom are restricted, make sure this is zero (see EAllowedDOFs).
 	void					SetInverseInertia(Vec3Arg inDiagonal, QuatArg inRot)			{ mInvInertiaDiagonal = inDiagonal; mInertiaRotation = inRot; }
 
 	/// Get inverse inertia matrix (\f$I_{body}^{-1}\f$). Will be a matrix of zeros for a static or kinematic object.
@@ -140,21 +140,30 @@ public:
 	// Reset the total accumulated torque, not that this will be done automatically after every time step.
 	JPH_INLINE void			ResetTorque()													{ mTorque = Float3(0, 0, 0); }
 
+	/// Returns a vector where the linear components that are not allowed by mAllowedDOFs are set to 0 and the rest to 0xffffffff
+	JPH_INLINE UVec4		GetLinearDOFsMask() const
+	{
+		UVec4 mask(uint32(EAllowedDOFs::TranslationX), uint32(EAllowedDOFs::TranslationY), uint32(EAllowedDOFs::TranslationZ), 0);
+		return UVec4::sEquals(UVec4::sAnd(UVec4::sReplicate(uint32(mAllowedDOFs)), mask), mask);
+	}
+
 	/// Takes a translation vector inV and returns a vector where the components that are not allowed by mAllowedDOFs are set to 0
 	JPH_INLINE Vec3			LockTranslation(Vec3Arg inV) const
 	{
-		uint32 allowed_dofs = uint32(mAllowedDOFs);
-		UVec4 allowed_dofs_mask = UVec4(allowed_dofs << 31, allowed_dofs << 30, allowed_dofs << 29, 0).ArithmeticShiftRight<31>();
-		return Vec3::sAnd(inV, Vec3(allowed_dofs_mask.ReinterpretAsFloat()));
+		return Vec3::sAnd(inV, Vec3(GetLinearDOFsMask().ReinterpretAsFloat()));
+	}
+
+	/// Returns a vector where the angular components that are not allowed by mAllowedDOFs are set to 0 and the rest to 0xffffffff
+	JPH_INLINE UVec4		GetAngularDOFsMask() const
+	{
+		UVec4 mask(uint32(EAllowedDOFs::RotationX), uint32(EAllowedDOFs::RotationY), uint32(EAllowedDOFs::RotationZ), 0);
+		return UVec4::sEquals(UVec4::sAnd(UVec4::sReplicate(uint32(mAllowedDOFs)), mask), mask);
 	}
 
 	/// Takes an angular velocity / torque vector inV and returns a vector where the components that are not allowed by mAllowedDOFs are set to 0
-	/// Interfaces like Body::AddTorque and Body::SetAngularVelocity don't automatically clear the components that are not allowed by mAllowedDOFs so this function can be used to do that.
 	JPH_INLINE Vec3			LockAngular(Vec3Arg inV) const
 	{
-		uint32 allowed_dofs = uint32(mAllowedDOFs);
-		UVec4 allowed_dofs_mask = UVec4(allowed_dofs << 28, allowed_dofs << 27, allowed_dofs << 26, 0).ArithmeticShiftRight<31>();
-		return Vec3::sAnd(inV, Vec3(allowed_dofs_mask.ReinterpretAsFloat()));
+		return Vec3::sAnd(inV, Vec3(GetAngularDOFsMask().ReinterpretAsFloat()));
 	}
 
 	/// Used only when this body is dynamic and colliding. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island.

Jolt/Physics/Body/MotionProperties.inl

@@ -14,13 +14,13 @@ void MotionProperties::MoveKinematic(Vec3Arg inDeltaPosition, QuatArg inDeltaRot
 	JPH_ASSERT(mCachedMotionType != EMotionType::Static);
 
 	// Calculate required linear velocity
-	mLinearVelocity = inDeltaPosition / inDeltaTime;
+	mLinearVelocity = LockTranslation(inDeltaPosition / inDeltaTime);
 
 	// Calculate required angular velocity
 	Vec3 axis;
 	float angle;
 	inDeltaRotation.GetAxisAngle(axis, angle);
-	mAngularVelocity = axis * (angle / inDeltaTime);
+	mAngularVelocity = LockAngular(axis * (angle / inDeltaTime));
 }
 
 void MotionProperties::ClampLinearVelocity()
@@ -62,15 +62,31 @@ Mat44 MotionProperties::GetInverseInertiaForRotation(Mat44Arg inRotation) const
 
 	Mat44 rotation = inRotation.Multiply3x3(Mat44::sRotation(mInertiaRotation));
 	Mat44 rotation_mul_scale_transposed(mInvInertiaDiagonal.SplatX() * rotation.GetColumn4(0), mInvInertiaDiagonal.SplatY() * rotation.GetColumn4(1), mInvInertiaDiagonal.SplatZ() * rotation.GetColumn4(2), Vec4(0, 0, 0, 1));
-	return rotation.Multiply3x3RightTransposed(rotation_mul_scale_transposed);
+	Mat44 inverse_inertia = rotation.Multiply3x3RightTransposed(rotation_mul_scale_transposed);
+
+	// We need to mask out both the rows and columns of DOFs that are not allowed
+	Vec4 angular_dofs_mask = GetAngularDOFsMask().ReinterpretAsFloat();
+	inverse_inertia.SetColumn4(0, Vec4::sAnd(inverse_inertia.GetColumn4(0), Vec4::sAnd(angular_dofs_mask, angular_dofs_mask.SplatX())));
+	inverse_inertia.SetColumn4(1, Vec4::sAnd(inverse_inertia.GetColumn4(1), Vec4::sAnd(angular_dofs_mask, angular_dofs_mask.SplatY())));
+	inverse_inertia.SetColumn4(2, Vec4::sAnd(inverse_inertia.GetColumn4(2), Vec4::sAnd(angular_dofs_mask, angular_dofs_mask.SplatZ())));
+
+	return inverse_inertia;
 }
 
 Vec3 MotionProperties::MultiplyWorldSpaceInverseInertiaByVector(QuatArg inBodyRotation, Vec3Arg inV) const
 {
 	JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic);
 
+	// Mask out columns of DOFs that are not allowed
+	Vec3 angular_dofs_mask = Vec3(GetAngularDOFsMask().ReinterpretAsFloat());
+	Vec3 v = Vec3::sAnd(inV, angular_dofs_mask);
+
+	// Multiply vector by inverse inertia
 	Mat44 rotation = Mat44::sRotation(inBodyRotation * mInertiaRotation);
-	return rotation.Multiply3x3(mInvInertiaDiagonal * rotation.Multiply3x3Transposed(inV));
+	Vec3 result = rotation.Multiply3x3(mInvInertiaDiagonal * rotation.Multiply3x3Transposed(v));
+
+	// Mask out rows of DOFs that are not allowed
+	return Vec3::sAnd(result, angular_dofs_mask);
 }
 
 void MotionProperties::ApplyGyroscopicForceInternal(QuatArg inBodyRotation, float inDeltaTime)

UnitTests/Physics/PhysicsTests.cpp

@@ -1488,33 +1488,36 @@ TEST_SUITE("PhysicsTests")
 			// Apply a force and torque in 3D
 			Vec3 force(100000, 110000, 120000);
 			box.AddForce(force);
-			Vec3 local_torque(13000, 14000, 15000);
-			box.AddTorque(initial_rotation * local_torque);
+			Vec3 torque(13000, 14000, 15000);
+			box.AddTorque(torque);
 
 			// Simulate
 			c.SimulateSingleStep();
 
 			// Cancel components that should not be allowed by the allowed DOFs
+			Vec3 linear_lock = Vec3::sReplicate(1.0f), angular_lock = Vec3::sReplicate(1.0f);
 			for (uint axis = 0; axis < 3; ++axis)
 			{
 				if ((allowed_dofs & (1 << axis)) == 0)
-					force.SetComponent(axis, 0.0f);
+					linear_lock.SetComponent(axis, 0.0f);
 
 				if ((allowed_dofs & (0b1000 << axis)) == 0)
-					local_torque.SetComponent(axis, 0.0f);
+					angular_lock.SetComponent(axis, 0.0f);
 			}
 
 			// Check resulting linear velocity
 			MassProperties mp = box_shape->GetMassProperties();
-			Vec3 expected_linear_velocity = force / mp.mMass * c.GetDeltaTime();
-			CHECK((force == Vec3::sZero() || expected_linear_velocity.Length() > 1.0f)); // Just to check that we applied a high enough force
+			Vec3 expected_linear_velocity = linear_lock * (force / mp.mMass * c.GetDeltaTime());
+			CHECK((linear_lock == Vec3::sZero() || expected_linear_velocity.Length() > 1.0f)); // Just to check that we applied a high enough force
 			CHECK_APPROX_EQUAL(box.GetLinearVelocity(), expected_linear_velocity);
 			RVec3 expected_position = initial_position + expected_linear_velocity * c.GetDeltaTime();
 			CHECK_APPROX_EQUAL(box.GetPosition(), expected_position);
 
 			// Check resulting angular velocity
-			Vec3 expected_angular_velocity = initial_rotation * (mp.mInertia.Inversed3x3() * local_torque) * c.GetDeltaTime();
-			CHECK((local_torque == Vec3::sZero() || expected_angular_velocity.Length() > 1.0f)); // Just to check that we applied a high enough torque
+			Mat44 inv_inertia = Mat44::sRotation(initial_rotation) * mp.mInertia.Inversed3x3() * Mat44::sRotation(initial_rotation.Conjugated());
+			inv_inertia = Mat44::sScale(angular_lock) * inv_inertia * Mat44::sScale(angular_lock); // Clear row and column for locked axes
+			Vec3 expected_angular_velocity = inv_inertia * torque * c.GetDeltaTime();
+			CHECK((angular_lock == Vec3::sZero() || expected_angular_velocity.Length() > 1.0f)); // Just to check that we applied a high enough torque
 			CHECK_APPROX_EQUAL(box.GetAngularVelocity(), expected_angular_velocity);
 			float expected_angular_velocity_len = expected_angular_velocity.Length();
 			Quat expected_rotation = expected_angular_velocity_len > 0.0f? Quat::sRotation(expected_angular_velocity / expected_angular_velocity_len, expected_angular_velocity_len * c.GetDeltaTime()) * initial_rotation : initial_rotation;