A 6DOF constraint that constrains all rotation axis in combination with a body that has some of its rotation axis locked would not constrain the rotation in the unlocked axis (#1771)

See godotengine/godot#109018

Docs/ReleaseNotes.md

@@ -2,6 +2,12 @@
 
 For breaking API changes see [this document](https://github.com/jrouwe/JoltPhysics/blob/master/Docs/APIChanges.md).
 
+## Unreleased Changes
+
+### Bug Fixes
+
+* A 6DOF constraint that constrains all rotation axis in combination with a body that has some of its rotation axis locked would not constrain the rotation in the unlocked axis
+
 ## v5.4.0
 
 ### New functionality

Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h

@@ -143,8 +143,21 @@ public:
 		mInvI2 = inBody2.IsDynamic()? inBody2.GetMotionProperties()->GetInverseInertiaForRotation(inRotation2) : Mat44::sZero();
 
 		// Calculate effective mass: K^-1 = (J M^-1 J^T)^-1
-		if (!mEffectiveMass.SetInversed3x3(mInvI1 + mInvI2))
-			Deactivate();
+		Mat44 inertia_sum = mInvI1 + mInvI2;
+		if (!mEffectiveMass.SetInversed3x3(inertia_sum))
+		{
+			// If a column is zero, the axis is locked and we set the column to identity.
+			// This does not matter because any impulse will always be multiplied with mInvI1 or mInvI2 which will result in zero for the locked coordinate.
+			Vec4 zero = Vec4::sZero();
+			if (inertia_sum.GetColumn4(0) == zero)
+				inertia_sum.SetColumn4(0, Vec4(1, 0, 0, 0));
+			if (inertia_sum.GetColumn4(1) == zero)
+				inertia_sum.SetColumn4(1, Vec4(0, 1, 0, 0));
+			if (inertia_sum.GetColumn4(2) == zero)
+				inertia_sum.SetColumn4(2, Vec4(0, 0, 1, 0));
+			if (!mEffectiveMass.SetInversed3x3(inertia_sum))
+				Deactivate();
+		}
 	}
 
 	/// Deactivate this constraint

UnitTests/Physics/SixDOFConstraintTests.cpp

@@ -5,6 +5,7 @@
 #include "UnitTestFramework.h"
 #include "PhysicsTestContext.h"
 #include <Jolt/Physics/Constraints/SixDOFConstraint.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
 #include "Layers.h"
 
 TEST_SUITE("SixDOFConstraintTests")
@@ -84,4 +85,43 @@ TEST_SUITE("SixDOFConstraintTests")
 			}
 		}
 	}
+
+	// Test combination of locked rotation axis with a 6DOF constraint
+	TEST_CASE("TestSixDOFLockedRotation")
+	{
+		PhysicsTestContext context;
+		BodyInterface &bi = context.GetBodyInterface();
+		PhysicsSystem *system = context.GetSystem();
+
+		RefConst<Shape> box_shape = new BoxShape(Vec3::sReplicate(1.0f));
+
+		// Static 'anchor' body
+		BodyCreationSettings settings1(box_shape, RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+		Body &body1 = *bi.CreateBody(settings1);
+		bi.AddBody(body1.GetID(), EActivation::Activate);
+
+		// Dynamic body that cannot rotate around X and Y
+		const RVec3 position2(3, 0, 0);
+		const Quat rotation2 = Quat::sIdentity();
+		BodyCreationSettings settings2(box_shape, position2, rotation2, EMotionType::Dynamic, Layers::MOVING);
+		settings2.mAllowedDOFs = EAllowedDOFs::RotationZ | EAllowedDOFs::TranslationX | EAllowedDOFs::TranslationY | EAllowedDOFs::TranslationZ;
+		Body &body2 = *bi.CreateBody(settings2);
+		bi.AddBody(body2.GetID(), EActivation::Activate);
+
+		// Lock all 6 axis with a 6DOF constraint
+		SixDOFConstraintSettings six_dof;
+		six_dof.MakeFixedAxis(SixDOFConstraintSettings::EAxis::TranslationX);
+		six_dof.MakeFixedAxis(SixDOFConstraintSettings::EAxis::TranslationY);
+		six_dof.MakeFixedAxis(SixDOFConstraintSettings::EAxis::TranslationZ);
+		six_dof.MakeFixedAxis(SixDOFConstraintSettings::EAxis::RotationX);
+		six_dof.MakeFixedAxis(SixDOFConstraintSettings::EAxis::RotationY);
+		six_dof.MakeFixedAxis(SixDOFConstraintSettings::EAxis::RotationZ);
+		system->AddConstraint(six_dof.Create(body1, body2));
+
+		context.Simulate(1.0f);
+
+		// Check that body didn't rotate
+		CHECK_APPROX_EQUAL(body2.GetPosition(), position2, 5.0e-3f);
+		CHECK_APPROX_EQUAL(body2.GetRotation(), rotation2, 5.0e-3f);
+	}
 }