Ability to specify which DOFs a body has (#602)

This allows limiting the physics simulation to a 2D plane or e.g. disabling rotation for a body but allowing translation.

Jolt/Jolt.cmake

@@ -160,6 +160,7 @@ set(JOLT_PHYSICS_SRC_FILES
 	${JOLT_PHYSICS_ROOT}/ObjectStream/SerializableObject.h
 	${JOLT_PHYSICS_ROOT}/ObjectStream/TypeDeclarations.cpp
 	${JOLT_PHYSICS_ROOT}/ObjectStream/TypeDeclarations.h
+	${JOLT_PHYSICS_ROOT}/Physics/Body/AllowedDOFs.h
 	${JOLT_PHYSICS_ROOT}/Physics/Body/Body.cpp
 	${JOLT_PHYSICS_ROOT}/Physics/Body/Body.h
 	${JOLT_PHYSICS_ROOT}/Physics/Body/Body.inl

Jolt/Physics/Body/AllowedDOFs.h

@@ -0,0 +1,34 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Enum used in BodyCreationSettings and MotionProperties to indicate which degrees of freedom a body has
+enum class EAllowedDOFs : uint8
+{
+	All					= 0b111111,									///< All degrees of freedom are allowed
+	TranslationX		= 0b000001,									///< Body cannot move in world space X axis
+	TranslationY		= 0b000010,									///< Body cannot move in world space Y axis
+	TranslationZ		= 0b000100,									///< Body cannot move in world space Z axis
+	RotationX			= 0b001000,									///< Body cannot rotate around local space X axis
+	RotationY			= 0b010000,									///< Body cannot rotate around local space Y axis
+	RotationZ			= 0b100000,									///< Body cannot rotate around local space Z axis
+	Plane2D				= TranslationX | TranslationY | RotationZ,	///< Body can only move in X and Y axis and rotate around Z axis
+};
+
+/// Bitwise OR operator for EAllowedDOFs
+inline EAllowedDOFs operator | (EAllowedDOFs inLHS, EAllowedDOFs inRHS)
+{
+	return EAllowedDOFs(uint8(inLHS) | uint8(inRHS));
+}
+
+/// Bitwise AND operator for EAllowedDOFs
+inline EAllowedDOFs operator & (EAllowedDOFs inLHS, EAllowedDOFs inRHS)
+{
+	return EAllowedDOFs(uint8(inLHS) & uint8(inRHS));
+}
+
+JPH_NAMESPACE_END

Jolt/Physics/Body/Body.cpp

@@ -117,7 +117,7 @@ void Body::UpdateCenterOfMassInternal(Vec3Arg inPreviousCenterOfMass, bool inUpd
 
 	// Recalculate mass and inertia if requested
 	if (inUpdateMassProperties && mMotionProperties != nullptr)
-		mMotionProperties->SetMassProperties(mShape->GetMassProperties());
+		mMotionProperties->SetMassProperties(mMotionProperties->GetAllowedDOFs(), mShape->GetMassProperties());
 }
 
 void Body::SetShapeInternal(const Shape *inShape, bool inUpdateMassProperties)
@@ -322,6 +322,7 @@ BodyCreationSettings Body::GetBodyCreationSettings() const
 	result.mUserData = mUserData;
 	result.mCollisionGroup = GetCollisionGroup();
 	result.mMotionType = GetMotionType();
+	result.mAllowedDOFs = mMotionProperties != nullptr? mMotionProperties->GetAllowedDOFs() : EAllowedDOFs::All;
 	result.mAllowDynamicOrKinematic = mMotionProperties != nullptr;
 	result.mIsSensor = IsSensor();
 	result.mSensorDetectsStatic = SensorDetectsStatic();

Jolt/Physics/Body/Body.h

@@ -245,8 +245,8 @@ public:
 	static inline bool		sFindCollidingPairsCanCollide(const Body &inBody1, const Body &inBody2);
 
 	/// Update position using an Euler step (used during position integrate & constraint solving)
-	inline void				AddPositionStep(Vec3Arg inLinearVelocityTimesDeltaTime)			{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::ReadWrite)); mPosition += inLinearVelocityTimesDeltaTime; JPH_ASSERT(!mPosition.IsNaN()); }
-	inline void				SubPositionStep(Vec3Arg inLinearVelocityTimesDeltaTime) 		{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::ReadWrite)); mPosition -= inLinearVelocityTimesDeltaTime; JPH_ASSERT(!mPosition.IsNaN()); }
+	inline void				AddPositionStep(Vec3Arg inLinearVelocityTimesDeltaTime)			{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::ReadWrite)); mPosition += mMotionProperties->LockTranslation(inLinearVelocityTimesDeltaTime); JPH_ASSERT(!mPosition.IsNaN()); }
+	inline void				SubPositionStep(Vec3Arg inLinearVelocityTimesDeltaTime) 		{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::ReadWrite)); mPosition -= mMotionProperties->LockTranslation(inLinearVelocityTimesDeltaTime); JPH_ASSERT(!mPosition.IsNaN()); }
 
 	/// Update rotation using an Euler step (using during position integrate & constraint solving)
 	inline void				AddRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime);

Jolt/Physics/Body/BodyCreationSettings.cpp

@@ -22,6 +22,7 @@ JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(BodyCreationSettings)
 	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mCollisionGroup)
 	JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mObjectLayer)
 	JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mMotionType)
+	JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mAllowedDOFs)
 	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mAllowDynamicOrKinematic)
 	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mIsSensor)
 	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mSensorDetectsStatic)
@@ -49,6 +50,7 @@ void BodyCreationSettings::SaveBinaryState(StreamOut &inStream) const
 	mCollisionGroup.SaveBinaryState(inStream);
 	inStream.Write(mObjectLayer);
 	inStream.Write(mMotionType);
+	inStream.Write(mAllowedDOFs);
 	inStream.Write(mAllowDynamicOrKinematic);
 	inStream.Write(mIsSensor);
 	inStream.Write(mSensorDetectsStatic);
@@ -76,6 +78,7 @@ void BodyCreationSettings::RestoreBinaryState(StreamIn &inStream)
 	mCollisionGroup.RestoreBinaryState(inStream);
 	inStream.Read(mObjectLayer);
 	inStream.Read(mMotionType);
+	inStream.Read(mAllowedDOFs);
 	inStream.Read(mAllowDynamicOrKinematic);
 	inStream.Read(mIsSensor);
 	inStream.Read(mSensorDetectsStatic);

Jolt/Physics/Body/BodyCreationSettings.h

@@ -9,6 +9,7 @@
 #include <Jolt/Physics/Collision/CollisionGroup.h>
 #include <Jolt/Physics/Body/MotionType.h>
 #include <Jolt/Physics/Body/MotionQuality.h>
+#include <Jolt/Physics/Body/AllowedDOFs.h>
 #include <Jolt/ObjectStream/SerializableObject.h>
 
 JPH_NAMESPACE_BEGIN
@@ -89,6 +90,7 @@ public:
 
 	///@name Simulation properties
 	EMotionType				mMotionType = EMotionType::Dynamic;								///< Motion type, determines if the object is static, dynamic or kinematic
+	EAllowedDOFs			mAllowedDOFs = EAllowedDOFs::All;								///< Which degrees of freedom this body has (can be used to limit simulation to 2D)
 	bool					mAllowDynamicOrKinematic = false;								///< When this body is created as static, this setting tells the system to create a MotionProperties object so that the object can be switched to kinematic or dynamic
 	bool					mIsSensor = false;												///< If this body is a sensor. A sensor will receive collision callbacks, but will not cause any collision responses and can be used as a trigger volume. See description at Body::SetIsSensor.
 	bool					mSensorDetectsStatic = false;									///< If this sensor detects static objects entering it. Note that the sensor must be kinematic and active for it to detect static objects.

Jolt/Physics/Body/BodyManager.cpp

@@ -185,7 +185,7 @@ Body *BodyManager::AllocateBody(const BodyCreationSettings &inBodyCreationSettin
 		mp->mIndexInActiveBodies = Body::cInactiveIndex;
 		mp->mIslandIndex = Body::cInactiveIndex;
 		JPH_IF_ENABLE_ASSERTS(mp->mCachedMotionType = body->mMotionType;)
-		mp->SetMassProperties(inBodyCreationSettings.GetMassProperties());
+		mp->SetMassProperties(inBodyCreationSettings.mAllowedDOFs, inBodyCreationSettings.GetMassProperties());
 	}
 
 	// Position body
@@ -878,21 +878,24 @@ void BodyManager::Draw(const DrawSettings &inDrawSettings, const PhysicsSettings
 			if (inDrawSettings.mDrawMassAndInertia && body->IsDynamic())
 			{
 				const MotionProperties *mp = body->GetMotionProperties();
+				if (mp->GetInverseMass() > 0.0f
+					&& !Vec3::sEquals(mp->GetInverseInertiaDiagonal(), Vec3::sZero()).TestAnyXYZTrue())
+				{
+					// Invert mass again
+					float mass = 1.0f / mp->GetInverseMass();
 
-				// Invert mass again
-				float mass = 1.0f / mp->GetInverseMass();
-
-				// Invert diagonal again
-				Vec3 diagonal = mp->GetInverseInertiaDiagonal().Reciprocal();
+					// Invert diagonal again
+					Vec3 diagonal = mp->GetInverseInertiaDiagonal().Reciprocal();
 
-				// Determine how big of a box has the equivalent inertia
-				Vec3 box_size = MassProperties::sGetEquivalentSolidBoxSize(mass, diagonal);
+					// Determine how big of a box has the equivalent inertia
+					Vec3 box_size = MassProperties::sGetEquivalentSolidBoxSize(mass, diagonal);
 
-				// Draw box with equivalent inertia
-				inRenderer->DrawWireBox(body->GetCenterOfMassTransform() * Mat44::sRotation(mp->GetInertiaRotation()), AABox(-0.5f * box_size, 0.5f * box_size), Color::sOrange);
+					// Draw box with equivalent inertia
+					inRenderer->DrawWireBox(body->GetCenterOfMassTransform() * Mat44::sRotation(mp->GetInertiaRotation()), AABox(-0.5f * box_size, 0.5f * box_size), Color::sOrange);
 
-				// Draw mass
-				inRenderer->DrawText3D(body->GetCenterOfMassPosition(), StringFormat("%.2f", (double)mass), Color::sOrange, 0.2f);
+					// Draw mass
+					inRenderer->DrawText3D(body->GetCenterOfMassPosition(), StringFormat("%.2f", (double)mass), Color::sOrange, 0.2f);
+				}
 			}
 
 			if (inDrawSettings.mDrawSleepStats && body->IsDynamic() && body->IsActive())

Jolt/Physics/Body/MotionProperties.cpp

@@ -9,6 +9,117 @@
 
 JPH_NAMESPACE_BEGIN
 
+void MotionProperties::SetMassProperties(EAllowedDOFs inAllowedDOFs, const MassProperties &inMassProperties)
+{
+	// Store allowed DOFs
+	mAllowedDOFs = inAllowedDOFs;
+
+	// Decompose DOFs
+	uint allowed_translation_axis = uint(inAllowedDOFs) & 0b111;
+	uint allowed_rotation_axis = (uint(inAllowedDOFs) >> 3) & 0b111;
+
+	// Set inverse mass
+	if (allowed_translation_axis == 0)
+	{
+		// No translation possible
+		mInvMass = 0.0f;
+	}
+	else
+	{
+		JPH_ASSERT(inMassProperties.mMass > 0.0f);
+		mInvMass = 1.0f / inMassProperties.mMass;
+	}
+
+	if (allowed_rotation_axis == 0b111)
+	{
+		// Set inverse inertia
+		Mat44 rotation;
+		Vec3 diagonal;
+		if (inMassProperties.DecomposePrincipalMomentsOfInertia(rotation, diagonal) 
+			&& !diagonal.IsNearZero())
+		{	
+			mInvInertiaDiagonal = diagonal.Reciprocal();
+			mInertiaRotation = rotation.GetQuaternion();
+		}
+		else
+		{
+			// Failed! Fall back to inertia tensor of sphere with radius 1.
+			mInvInertiaDiagonal = Vec3::sReplicate(2.5f * mInvMass);
+			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!");
+}
+
 void MotionProperties::SaveState(StateRecorder &inStream) const
 {
 	// Only write properties that can change at runtime

Jolt/Physics/Body/MotionProperties.h

@@ -5,6 +5,7 @@
 #pragma once
 
 #include <Jolt/Geometry/Sphere.h>
+#include <Jolt/Physics/Body/AllowedDOFs.h>
 #include <Jolt/Physics/Body/MotionQuality.h>
 #include <Jolt/Physics/Body/BodyAccess.h>
 #include <Jolt/Physics/Body/MotionType.h>
@@ -24,6 +25,9 @@ public:
 	/// Motion quality, or how well it detects collisions when it has a high velocity
 	EMotionQuality			GetMotionQuality() const										{ return mMotionQuality; }
 
+	/// Get the allowed degrees of freedom that this body has (this can be changed by calling SetMassProperties)
+	inline EAllowedDOFs		GetAllowedDOFs() const											{ return mAllowedDOFs; }
+
 	/// Get world space linear velocity of the center of mass
 	inline Vec3				GetLinearVelocity() const										{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::Read)); return mLinearVelocity; }
 
@@ -74,7 +78,7 @@ public:
 	void					SetGravityFactor(float inGravityFactor)							{ mGravityFactor = inGravityFactor; }
 
 	/// Set the mass and inertia tensor
-	inline void				SetMassProperties(const MassProperties &inMassProperties);
+	void					SetMassProperties(EAllowedDOFs inAllowedDOFs, const MassProperties &inMassProperties);
 
 	/// Get inverse mass (1 / mass). Should only be called on a dynamic object (static or kinematic bodies have infinite mass so should be treated as 1 / mass = 0)
 	inline float			GetInverseMass() const											{ JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); return mInvMass; }
@@ -83,6 +87,7 @@ public:
 	/// Set the inverse mass (1 / mass).
 	/// 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 mass, inertia should probably change as well. See MassProperties::ScaleToMass.
+	/// If all your translation degrees of freedom are restricted, make sure this is zero (see EAllowedDOFs).
 	void					SetInverseMass(float inInverseMass)								{ mInvMass = inInverseMass; }
 
 	/// Diagonal of inverse inertia matrix: D. Should only be called on a dynamic object (static or kinematic bodies have infinite mass so should be treated as D = 0)
@@ -94,6 +99,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).
 	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.
@@ -123,14 +129,22 @@ 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); }
 
+	/// 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)
+	{
+		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()));
+	}
+
 	////////////////////////////////////////////////////////////
 	// FUNCTIONS BELOW THIS LINE ARE FOR INTERNAL USE ONLY
 	////////////////////////////////////////////////////////////
 
 	///@name Update linear and angular velocity (used during constraint solving)
 	///@{
-	inline void				AddLinearVelocityStep(Vec3Arg inLinearVelocityChange)			{ JPH_DET_LOG("AddLinearVelocityStep: " << inLinearVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite)); mLinearVelocity += inLinearVelocityChange; JPH_ASSERT(!mLinearVelocity.IsNaN()); }
-	inline void				SubLinearVelocityStep(Vec3Arg inLinearVelocityChange)			{ JPH_DET_LOG("SubLinearVelocityStep: " << inLinearVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite)); mLinearVelocity -= inLinearVelocityChange; JPH_ASSERT(!mLinearVelocity.IsNaN()); }
+	inline void				AddLinearVelocityStep(Vec3Arg inLinearVelocityChange)			{ JPH_DET_LOG("AddLinearVelocityStep: " << inLinearVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite)); mLinearVelocity = LockTranslation(mLinearVelocity + inLinearVelocityChange); JPH_ASSERT(!mLinearVelocity.IsNaN()); }
+	inline void				SubLinearVelocityStep(Vec3Arg inLinearVelocityChange)			{ JPH_DET_LOG("SubLinearVelocityStep: " << inLinearVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite)); mLinearVelocity = LockTranslation(mLinearVelocity - inLinearVelocityChange); JPH_ASSERT(!mLinearVelocity.IsNaN()); }
 	inline void				AddAngularVelocityStep(Vec3Arg inAngularVelocityChange)			{ JPH_DET_LOG("AddAngularVelocityStep: " << inAngularVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite)); mAngularVelocity += inAngularVelocityChange; JPH_ASSERT(!mAngularVelocity.IsNaN()); }
 	inline void				SubAngularVelocityStep(Vec3Arg inAngularVelocityChange) 		{ JPH_DET_LOG("SubAngularVelocityStep: " << inAngularVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite)); mAngularVelocity -= inAngularVelocityChange; JPH_ASSERT(!mAngularVelocity.IsNaN()); }
 	///@}
@@ -185,6 +199,7 @@ private:
 	// 1 byte aligned
 	EMotionQuality			mMotionQuality;													///< Motion quality, or how well it detects collisions when it has a high velocity
 	bool					mAllowSleeping;													///< If this body can go to sleep
+	EAllowedDOFs			mAllowedDOFs;													///< Allowed degrees of freedom for this body
 
 	// 3rd cache line (least frequently used)
 	// 4 byte aligned (or 8 byte if running in double precision)

Jolt/Physics/Body/MotionProperties.inl

@@ -6,29 +6,6 @@
 
 JPH_NAMESPACE_BEGIN
 
-void MotionProperties::SetMassProperties(const MassProperties &inMassProperties)
-{
-	// Set inverse mass
-	JPH_ASSERT(inMassProperties.mMass > 0.0f);
-	mInvMass = 1.0f / inMassProperties.mMass;
-
-	// Set inverse inertia
-	Mat44 rotation;
-	Vec3 diagonal;
-	if (inMassProperties.DecomposePrincipalMomentsOfInertia(rotation, diagonal) 
-		&& !diagonal.IsNearZero())
-	{	
-		mInvInertiaDiagonal = diagonal.Reciprocal();
-		mInertiaRotation = rotation.GetQuaternion();
-	}
-	else
-	{
-		// Failed! Fall back to inertia tensor of sphere with radius 1.
-		mInvInertiaDiagonal = Vec3::sReplicate(2.5f * mInvMass);
-		mInertiaRotation = Quat::sIdentity();
-	}
-}
-
 void MotionProperties::MoveKinematic(Vec3Arg inDeltaPosition, QuatArg inDeltaRotation, float inDeltaTime)
 {
 	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite)); 
@@ -101,7 +78,7 @@ void MotionProperties::ApplyForceTorqueAndDragInternal(QuatArg inBodyRotation, V
 	JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic);
 
 	// Update linear velocity
-	mLinearVelocity += inDeltaTime * (mGravityFactor * inGravity + mInvMass * GetAccumulatedForce());
+	mLinearVelocity = LockTranslation(mLinearVelocity + inDeltaTime * (mGravityFactor * inGravity + mInvMass * GetAccumulatedForce()));
 
 	// Update angular velocity
 	mAngularVelocity += inDeltaTime * MultiplyWorldSpaceInverseInertiaByVector(inBodyRotation, GetAccumulatedTorque());

Jolt/Physics/Character/Character.cpp

@@ -34,18 +34,15 @@ Character::Character(const CharacterSettings *inSettings, RVec3Arg inPosition, Q
 {
 	// Construct rigid body
 	BodyCreationSettings settings(mShape, inPosition, inRotation, EMotionType::Dynamic, mLayer);
+	settings.mAllowedDOFs = EAllowedDOFs::TranslationX | EAllowedDOFs::TranslationY | EAllowedDOFs::TranslationZ;
+	settings.mOverrideMassProperties = EOverrideMassProperties::MassAndInertiaProvided;
+	settings.mMassPropertiesOverride.mMass = inSettings->mMass;
 	settings.mFriction = inSettings->mFriction;
 	settings.mGravityFactor = inSettings->mGravityFactor;
 	settings.mUserData = inUserData;
-	Body *body = mSystem->GetBodyInterface().CreateBody(settings);
+	const Body *body = mSystem->GetBodyInterface().CreateBody(settings);
 	if (body != nullptr)
-	{
-		// Update the mass properties of the shape so that we set the correct mass and don't allow any rotation
-		body->GetMotionProperties()->SetInverseMass(1.0f / inSettings->mMass);
-		body->GetMotionProperties()->SetInverseInertia(Vec3::sZero(), Quat::sIdentity());
-
 		mBodyID = body->GetID();
-	}
 }
 
 Character::~Character()

Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h

@@ -124,7 +124,9 @@ public:
 	{
 		float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inBody2, inWorldSpaceAxis);
 
-		if (inSpringSettings.mMode == ESpringMode::FrequencyAndDamping)
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else if (inSpringSettings.mMode == ESpringMode::FrequencyAndDamping)
 			mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings.mFrequency, inSpringSettings.mDamping, mEffectiveMass);
 		else
 			mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings.mStiffness, inSpringSettings.mDamping, mEffectiveMass);

Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h

@@ -249,7 +249,9 @@ public:
 	{
 		float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis);
 
-		if (inSpringSettings.mMode == ESpringMode::FrequencyAndDamping)
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else if (inSpringSettings.mMode == ESpringMode::FrequencyAndDamping)
 			mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings.mFrequency, inSpringSettings.mDamping, mEffectiveMass);
 		else
 			mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings.mStiffness, inSpringSettings.mDamping, mEffectiveMass);

Samples/Samples.cmake

@@ -75,6 +75,8 @@ set(SAMPLES_SRC_FILES
 	${SAMPLES_ROOT}/Tests/ConvexCollision/RandomRayTest.h
 	${SAMPLES_ROOT}/Tests/General/ActiveEdgesTest.cpp
 	${SAMPLES_ROOT}/Tests/General/ActiveEdgesTest.h
+	${SAMPLES_ROOT}/Tests/General/AllowedDOFsTest.cpp
+	${SAMPLES_ROOT}/Tests/General/AllowedDOFsTest.h
 	${SAMPLES_ROOT}/Tests/General/BigVsSmallTest.cpp
 	${SAMPLES_ROOT}/Tests/General/BigVsSmallTest.h
 	${SAMPLES_ROOT}/Tests/General/ShapeFilterTest.cpp

Samples/SamplesApp.cpp

@@ -96,6 +96,7 @@ 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)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, TwoDFunnelTest)
+JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, AllowedDOFsTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, ShapeFilterTest)
 
 static TestNameAndRTTI sGeneralTests[] =
@@ -133,6 +134,7 @@ static TestNameAndRTTI sGeneralTests[] =
 	{ "Activate During Update",				JPH_RTTI(ActivateDuringUpdateTest) },
 	{ "Sensor",								JPH_RTTI(SensorTest) },
 	{ "Dynamic Mesh",						JPH_RTTI(DynamicMeshTest) },
+	{ "Allowed Degrees of Freedom",			JPH_RTTI(AllowedDOFsTest) },
 	{ "Shape Filter",						JPH_RTTI(ShapeFilterTest) },
 };
 

Samples/Tests/General/AllowedDOFsTest.cpp

@@ -0,0 +1,74 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/General/AllowedDOFsTest.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Constraints/DistanceConstraint.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Renderer/DebugRendererImp.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(AllowedDOFsTest) 
+{ 
+	JPH_ADD_BASE_CLASS(AllowedDOFsTest, Test) 
+}
+
+void AllowedDOFsTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	Vec3 box_size(0.5f, 1.0f, 2.0f);
+	RefConst<Shape> box_shape = new BoxShape(box_size);
+
+	for (int allowed_dofs = 1; allowed_dofs <= 0b111111; ++allowed_dofs)
+	{
+		float x = -35.0f + 10.0f * (allowed_dofs & 0b111);
+		float z = -35.0f + 10.0f * ((allowed_dofs >> 3) & 0b111);
+
+		// Create body 
+		BodyCreationSettings bcs(box_shape, RVec3(x, 10, z), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+		bcs.mAllowedDOFs = (EAllowedDOFs)allowed_dofs;
+		BodyID id = mBodyInterface->CreateAndAddBody(bcs, EActivation::Activate);
+		mBodies.push_back(id);
+
+		// Create a constraint
+		DistanceConstraintSettings dcs;
+		dcs.mPoint1 = bcs.mPosition + Vec3(5, 5, 5);
+		dcs.mPoint2 = bcs.mPosition + box_size;
+		dcs.mMinDistance = 0.0f;
+		dcs.mMaxDistance = sqrt(3.0f) * 5.0f + 1.0f;
+		mPhysicsSystem->AddConstraint(mBodyInterface->CreateConstraint(&dcs, BodyID(), id));
+	}
+}
+
+void AllowedDOFsTest::PostPhysicsUpdate(float inDeltaTime)
+{
+	// Draw degrees of freedom
+	for (BodyID id : mBodies)
+	{
+		BodyLockRead body_lock(mPhysicsSystem->GetBodyLockInterface(), id);
+		if (body_lock.Succeeded())
+		{
+			const Body &body = body_lock.GetBody();
+			String allowed_dofs_str = "";
+			EAllowedDOFs allowed_dofs = body.GetMotionProperties()->GetAllowedDOFs();
+			if ((allowed_dofs & EAllowedDOFs::TranslationX) == EAllowedDOFs::TranslationX)
+				allowed_dofs_str += "X ";
+			if ((allowed_dofs & EAllowedDOFs::TranslationY) == EAllowedDOFs::TranslationY)
+				allowed_dofs_str += "Y ";
+			if ((allowed_dofs & EAllowedDOFs::TranslationZ) == EAllowedDOFs::TranslationZ)
+				allowed_dofs_str += "Z ";
+			if ((allowed_dofs & EAllowedDOFs::RotationX) == EAllowedDOFs::RotationX)
+				allowed_dofs_str += "RX ";
+			if ((allowed_dofs & EAllowedDOFs::RotationY) == EAllowedDOFs::RotationY)
+				allowed_dofs_str += "RY ";
+			if ((allowed_dofs & EAllowedDOFs::RotationZ) == EAllowedDOFs::RotationZ)
+				allowed_dofs_str += "RZ ";
+			DebugRenderer::sInstance->DrawText3D(body.GetPosition(), allowed_dofs_str, Color::sWhite);
+		}
+	}
+}

Samples/Tests/General/AllowedDOFsTest.h

@@ -0,0 +1,21 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+// This test tests all permutations of allowed degrees of freedom (see EAllowedDOFs)
+class AllowedDOFsTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, AllowedDOFsTest)
+
+	// See: Test
+	virtual void		Initialize() override;
+	virtual void		PostPhysicsUpdate(float inDeltaTime) override;
+
+private:
+	BodyIDVector		mBodies;
+};

Samples/Tests/General/TwoDFunnelTest.cpp

@@ -8,7 +8,6 @@
 #include <Jolt/Physics/Collision/Shape/BoxShape.h>
 #include <Jolt/Physics/Collision/Shape/SphereShape.h>
 #include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
-#include <Jolt/Physics/Constraints/SixDOFConstraint.h>
 #include <Jolt/Physics/Body/BodyCreationSettings.h>
 #include <Layers.h>
 
@@ -22,10 +21,11 @@ void TwoDFunnelTest::Initialize()
 	// Floor
 	CreateFloor();
 
-	// 2D funnel
 	RefConst<Shape> wall = new BoxShape(Vec3(0.1f, 10, 1));
-	mBodyInterface->CreateAndAddBody(BodyCreationSettings(wall, RVec3(-12, 8, 0), Quat::sRotation(Vec3::sAxisZ(), 0.2f * JPH_PI), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
-	mBodyInterface->CreateAndAddBody(BodyCreationSettings(wall, RVec3(12, 8, 0), Quat::sRotation(Vec3::sAxisZ(), -0.2f * JPH_PI), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+
+	// 2D funnel
+	mBodyInterface->CreateAndAddBody(BodyCreationSettings(wall, RVec3(-12, 8, -5), Quat::sRotation(Vec3::sAxisZ(), 0.2f * JPH_PI), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+	mBodyInterface->CreateAndAddBody(BodyCreationSettings(wall, RVec3(12, 8, -5), Quat::sRotation(Vec3::sAxisZ(), -0.2f * JPH_PI), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
 
 	// Shapes falling in 2D funnel
 	Ref<Shape> shapes[] = {
@@ -33,35 +33,13 @@ void TwoDFunnelTest::Initialize()
 		new BoxShape(Vec3::sReplicate(0.5f)),
 		new CapsuleShape(0.2f, 0.3f)
 	};
-
-	// Constraint to limit motion in XY plane
-	SixDOFConstraintSettings constraint_settings;
-	constraint_settings.MakeFreeAxis(SixDOFConstraintSettings::EAxis::RotationX); // Don't limit, we use inertia tensor to limit rotation
-	constraint_settings.MakeFreeAxis(SixDOFConstraintSettings::EAxis::RotationY); // Don't limit, we use inertia tensor to limit rotation
-	constraint_settings.MakeFreeAxis(SixDOFConstraintSettings::EAxis::RotationZ);
-	constraint_settings.MakeFreeAxis(SixDOFConstraintSettings::EAxis::TranslationX);
-	constraint_settings.MakeFreeAxis(SixDOFConstraintSettings::EAxis::TranslationY);
-	constraint_settings.MakeFixedAxis(SixDOFConstraintSettings::EAxis::TranslationZ); // Don't allow movement in Z direction
-
 	BodyCreationSettings bcs(shapes[0], RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+	bcs.mAllowedDOFs = EAllowedDOFs::Plane2D;
 	for (int x = 0; x < 20; ++x)
 		for (int y = 0; y < 10; ++y)
 		{
-			// Create body
 			bcs.SetShape(shapes[(x * y) % size(shapes)]);
-			bcs.mPosition = RVec3(-10.0_r + x, 10.0_r + y, 0);
-			Body &body = *mBodyInterface->CreateBody(bcs);			
-			mBodyInterface->AddBody(body.GetID(), EActivation::Activate);
-
-			// Constraint the body to the XY plane
-			constraint_settings.mPosition1 = constraint_settings.mPosition2 = body.GetCenterOfMassPosition(); // Attach it at the initial position of the body for best precision
-			TwoBodyConstraint *constraint = constraint_settings.Create(Body::sFixedToWorld, body);
-			mPhysicsSystem->AddConstraint(constraint);
-
-			// Update the mass properties for this body to make it rotate only around Z
-			MassProperties mass_properties = bcs.GetShape()->GetMassProperties();
-			MotionProperties *mp = body.GetMotionProperties();
-			mp->SetInverseMass(1.0f / mass_properties.mMass);
-			mp->SetInverseInertia(Vec3(0, 0, 1.0f / mass_properties.mInertia.GetAxisZ().Length()), Quat::sIdentity());
+			bcs.mPosition = RVec3(-10.0_r + x, 10.0_r + y, -5.0_r);
+			mBodyInterface->CreateAndAddBody(bcs, EActivation::Activate);
 		}
 }

UnitTests/Physics/PhysicsTests.cpp

@@ -1421,4 +1421,60 @@ TEST_SUITE("PhysicsTests")
 			CHECK_APPROX_EQUAL(box.GetAngularVelocity(), Vec3::sZero(), 1.0e-2f);
 		}
 	}
+
+	TEST_CASE("TestAllowedDOFs")
+	{
+		for (uint allowed_dofs = 1; allowed_dofs <= 0b111111; ++allowed_dofs)
+		{
+			// Create a context
+			PhysicsTestContext c;
+			c.ZeroGravity();
+
+			// Create box
+			RVec3 initial_position(1, 2, 3);
+			Quat initial_rotation = Quat::sRotation(Vec3::sReplicate(sqrt(1.0f / 3.0f)), DegreesToRadians(20.0f));
+			ShapeRefC box_shape = new BoxShape(Vec3(0.3f, 0.5f, 0.7f));
+			BodyCreationSettings box_settings(box_shape, initial_position, initial_rotation, EMotionType::Dynamic, Layers::MOVING);
+			box_settings.mLinearDamping = 0;
+			box_settings.mAngularDamping = 0;
+			box_settings.mAllowedDOFs = (EAllowedDOFs)allowed_dofs;
+			Body &box = *c.GetBodyInterface().CreateBody(box_settings);
+			c.GetBodyInterface().AddBody(box.GetID(), EActivation::Activate);
+
+			// 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);
+
+			// Simulate
+			c.SimulateSingleStep();
+
+			// Cancel components that should not be allowed by the allowed DOFs
+			for (uint axis = 0; axis < 3; ++axis)
+			{
+				if ((allowed_dofs & (1 << axis)) == 0)
+					force.SetComponent(axis, 0.0f);
+
+				if ((allowed_dofs & (0b1000 << axis)) == 0)
+					local_torque.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
+			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
+			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;
+			CHECK_APPROX_EQUAL(box.GetRotation(), expected_rotation);
+		}
+	}
 }