Ability to fully configure reference frame for slider constraint (#74)

Jolt/Physics/Constraints/SliderConstraint.cpp

@@ -18,18 +18,48 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(SliderConstraintSettings)
 {
 	JPH_ADD_BASE_CLASS(SliderConstraintSettings, TwoBodyConstraintSettings)
 
-	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mSliderAxis)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mPoint1)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mSliderAxis1)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mNormalAxis1)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mPoint2)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mSliderAxis2)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mNormalAxis2)
 	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mLimitsMin)
 	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mLimitsMax)
 	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mMaxFrictionForce)
 	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mMotorSettings)
 }
 
+void SliderConstraintSettings::SetPoint(const Body &inBody1, const Body &inBody2)
+{
+	// Determine anchor point: If any of the bodies can never be dynamic use the other body as anchor point, otherwise use the mid point between the two center of masses
+	Vec3 anchor;
+	if (!inBody1.CanBeKinematicOrDynamic())
+		anchor = inBody2.GetCenterOfMassPosition();
+	else if (!inBody2.CanBeKinematicOrDynamic())
+		anchor = inBody1.GetCenterOfMassPosition();
+	else
+		anchor = 0.5f * (inBody1.GetCenterOfMassPosition() + inBody2.GetCenterOfMassPosition());
+
+	mPoint1 = mPoint2 = anchor;
+}
+
+void SliderConstraintSettings::SetSliderAxis(Vec3Arg inSliderAxis)
+{
+	mSliderAxis1 = mSliderAxis2 = inSliderAxis;
+	mNormalAxis1 = mNormalAxis2 = inSliderAxis.GetNormalizedPerpendicular();
+}
+
 void SliderConstraintSettings::SaveBinaryState(StreamOut &inStream) const
 { 
 	ConstraintSettings::SaveBinaryState(inStream);
 
-	inStream.Write(mSliderAxis);
+	inStream.Write(mPoint1);
+	inStream.Write(mSliderAxis1);
+	inStream.Write(mNormalAxis1);
+	inStream.Write(mPoint2);
+	inStream.Write(mSliderAxis2);
+	inStream.Write(mNormalAxis2);
 	inStream.Write(mLimitsMin);
 	inStream.Write(mLimitsMax);
 	inStream.Write(mMaxFrictionForce);
@@ -40,7 +70,12 @@ void SliderConstraintSettings::RestoreBinaryState(StreamIn &inStream)
 {
 	ConstraintSettings::RestoreBinaryState(inStream);
 
-	inStream.Read(mSliderAxis);
+	inStream.Read(mPoint1);
+	inStream.Read(mSliderAxis1);
+	inStream.Read(mNormalAxis1);
+	inStream.Read(mPoint2);
+	inStream.Read(mSliderAxis2);
+	inStream.Read(mNormalAxis2);
 	inStream.Read(mLimitsMin);
 	inStream.Read(mLimitsMax);
 	inStream.Read(mMaxFrictionForce);
@@ -59,28 +94,33 @@ SliderConstraint::SliderConstraint(Body &inBody1, Body &inBody2, const SliderCon
 {
 	Mat44 inv_transform1 = inBody1.GetInverseCenterOfMassTransform();
 
-	// Determine anchor point: If any of the bodies can never be dynamic use the other body as anchor point, otherwise use the mid point between the two center of masses
-	Vec3 anchor;
-	if (!mBody1->CanBeKinematicOrDynamic())
-		anchor = mBody2->GetCenterOfMassPosition();
-	else if (!mBody2->CanBeKinematicOrDynamic())
-		anchor = mBody1->GetCenterOfMassPosition();
-	else
-		anchor = 0.5f * (mBody1->GetCenterOfMassPosition() + mBody2->GetCenterOfMassPosition());
-
 	// Store local positions
-	mLocalSpacePosition1 = inv_transform1 * anchor;
-	mLocalSpacePosition2 = inBody2.GetInverseCenterOfMassTransform() * anchor;
+	mLocalSpacePosition1 = inv_transform1 * inSettings.mPoint1;
+	mLocalSpacePosition2 = inBody2.GetInverseCenterOfMassTransform() * inSettings.mPoint2;
 
 	// Store local sliding axis
-	mLocalSpaceSliderAxis1 = inv_transform1.Multiply3x3(inSettings.mSliderAxis).Normalized();
+	mLocalSpaceSliderAxis1 = inv_transform1.Multiply3x3(inSettings.mSliderAxis1).Normalized();
 
 	// Store local space normals
-	mLocalSpaceNormal1 = mLocalSpaceSliderAxis1.GetNormalizedPerpendicular();
+	mLocalSpaceNormal1 = inv_transform1.Multiply3x3(inSettings.mNormalAxis1).Normalized();
 	mLocalSpaceNormal2 = mLocalSpaceSliderAxis1.Cross(mLocalSpaceNormal1);
 	
-	// Inverse of initial rotation from body 1 to body 2 in body 1 space
-	mInvInitialOrientation = RotationEulerConstraintPart::sGetInvInitialOrientation(inBody1, inBody2);
+	// Store inverse of initial rotation from body 1 to body 2 in body 1 space
+	if (inSettings.mSliderAxis1 == inSettings.mSliderAxis2 && inSettings.mNormalAxis1 == inSettings.mNormalAxis2)
+	{
+		// Bodies are in their neutral poses, no need to take slider and normal axis into account
+		mInvInitialOrientation = RotationEulerConstraintPart::sGetInvInitialOrientation(inBody1, inBody2);
+	}
+	else
+	{
+		// Bodies are not in their neutral pose, need to adjust initial rotation for it
+		// Form two world space constraint matrices C1, C2
+		// Body 1 needs to be rotated by D to get it into neutral pose: C2 = D C1 <=> D = C2 C1^1
+		// so instead of using body1 rotation as above use D R1 = C2 C1^-1 R1
+		Mat44 constraint1(Vec4(inSettings.mSliderAxis1, 0), Vec4(inSettings.mNormalAxis1, 0), Vec4(inSettings.mSliderAxis1.Cross(inSettings.mNormalAxis1), 0), Vec4(0, 0, 0, 1));
+		Mat44 constraint2(Vec4(inSettings.mSliderAxis2, 0), Vec4(inSettings.mNormalAxis2, 0), Vec4(inSettings.mSliderAxis2.Cross(inSettings.mNormalAxis2), 0), Vec4(0, 0, 0, 1));
+		mInvInitialOrientation = inBody2.GetRotation().Conjugated() * constraint2.GetQuaternion() * constraint1.GetQuaternion().Conjugated() * inBody1.GetRotation();
+	}
 
 	// Store limits
 	JPH_ASSERT(inSettings.mLimitsMin != inSettings.mLimitsMax, "Better use a fixed constraint");

Jolt/Physics/Constraints/SliderConstraint.h

@@ -23,10 +23,24 @@ public:
 	/// Create an an instance of this constraint
 	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
 
-	/// Axis along which movement is possible (world space direction).
-	Vec3						mSliderAxis = Vec3::sAxisX();
-
-	/// Bodies are assumed to be placed so that the slider position = 0, movement will be limited between [mLimitsMin, mLimitsMax] where mLimitsMin e [-inf, 0] and mLimitsMax e [0, inf]
+	/// Simple way of setting the anchor points so that the current relative position is chosen as the '0' position
+	void						SetPoint(const Body &inBody1, const Body &inBody2);
+
+	/// Simple way of setting the slider and normal axis in world space (assumes the bodies are already oriented correctly when the constraint is created)
+	void						SetSliderAxis(Vec3Arg inSliderAxis);
+
+	/// Body 1 constraint reference frame (in world space).
+	/// Slider axis is the axis along which movement is possible (world space direction), normal axis is a perpendicular vector to define the frame.
+	Vec3						mPoint1 = Vec3::sZero();
+	Vec3						mSliderAxis1 = Vec3::sAxisX();
+	Vec3						mNormalAxis1 = Vec3::sAxisY();
+	
+	/// Body 2 constraint reference frame (in world space)
+	Vec3						mPoint2 = Vec3::sZero();
+	Vec3						mSliderAxis2 = Vec3::sAxisX();
+	Vec3						mNormalAxis2 = Vec3::sAxisY();
+
+	/// When the bodies move so that mPoint1 coincides with mPoint2 the slider position is defined to be 0, movement will be limited between [mLimitsMin, mLimitsMax] where mLimitsMin e [-inf, 0] and mLimitsMax e [0, inf]
 	float						mLimitsMin = -FLT_MAX;
 	float						mLimitsMax = FLT_MAX;
 

Samples/Tests/Constraints/PoweredSliderConstraintTest.cpp

@@ -42,7 +42,8 @@ void PoweredSliderConstraintTest::Initialize()
 	mBodyInterface->AddBody(mBody2->GetID(), EActivation::Activate);
 
 	SliderConstraintSettings settings;
-	settings.mSliderAxis = Vec3::sAxisX();
+	settings.SetPoint(body1, *mBody2);
+	settings.SetSliderAxis(Vec3::sAxisX());
 	settings.mLimitsMin = -5.0f;
 	settings.mLimitsMax = 100.0f;
 	mConstraint = static_cast<SliderConstraint *>(settings.Create(body1, *mBody2));

Samples/Tests/Constraints/SliderConstraintTest.cpp

@@ -48,7 +48,8 @@ void SliderConstraintTest::Initialize()
 			mBodyInterface->AddBody(segment.GetID(), EActivation::Activate);
 
 			SliderConstraintSettings settings;
-			settings.mSliderAxis = Quat::sRotation(Vec3::sAxisZ(), -DegreesToRadians(10)) * Vec3::sAxisX();
+			settings.SetPoint(*prev, segment);
+			settings.SetSliderAxis(Quat::sRotation(Vec3::sAxisZ(), -DegreesToRadians(10)).RotateAxisX());
 			settings.mLimitsMin = -5.0f;
 			settings.mLimitsMax = 10.0f;
 			mPhysicsSystem->AddConstraint(settings.Create(*prev, segment));
@@ -76,7 +77,8 @@ void SliderConstraintTest::Initialize()
 			mBodyInterface->AddBody(segment.GetID(), EActivation::Activate);
 
 			SliderConstraintSettings settings;
-			settings.mSliderAxis = Quat::sRotation(Vec3::sAxisY(), displacement(random) * DegreesToRadians(20)) * Quat::sRotation(Vec3::sAxisZ(), -DegreesToRadians(10)) * Vec3::sAxisX();
+			settings.SetPoint(*prev, segment);
+			settings.SetSliderAxis(Quat::sRotation(Vec3::sAxisY(), displacement(random) * DegreesToRadians(20)) * Quat::sRotation(Vec3::sAxisZ(), -DegreesToRadians(10)).RotateAxisX());
 			settings.mLimitsMin = -5.0f;
 			settings.mLimitsMax = 10.0f;
 			mPhysicsSystem->AddConstraint(settings.Create(*prev, segment));

Samples/Utils/RagdollLoader.cpp

@@ -66,7 +66,9 @@ RagdollSettings *RagdollLoader::sLoad(const char *inFileName, EMotionType inMoti
 			case EConstraintOverride::TypeSlider:
 				{
 					SliderConstraintSettings *settings = new SliderConstraintSettings();
-					settings->mSliderAxis = original->mTwistAxis1;
+					settings->mPoint1 = settings->mPoint2 = original->mPosition1;
+					settings->mSliderAxis1 = settings->mSliderAxis2 = original->mTwistAxis1;
+					settings->mNormalAxis1 = settings->mNormalAxis2 = original->mTwistAxis1.GetNormalizedPerpendicular();
 					settings->mLimitsMin = -1.0f;
 					settings->mLimitsMax = 1.0f;
 					settings->mMaxFrictionForce = original->mMaxFrictionTorque;

UnitTests/Physics/SliderConstraintTests.cpp

@@ -32,7 +32,8 @@ TEST_SUITE("SliderConstraintTests")
 
 		// Create slider constraint
 		SliderConstraintSettings s;
-		s.mSliderAxis = Vec3::sAxisX();
+		s.SetPoint(body1, body2);
+		s.SetSliderAxis(Vec3::sAxisX());
 		s.mLimitsMin = cLimitMin;
 		s.mLimitsMax = 0.0f;
 		c.CreateConstraint<SliderConstraint>(body1, body2, s);
@@ -44,7 +45,7 @@ TEST_SUITE("SliderConstraintTests")
 		CHECK_APPROX_EQUAL(Vec3::sZero(), body2.GetLinearVelocity(), 1.0e-4f);
 
 		// Test resulting position
-		CHECK_APPROX_EQUAL(cInitialPos + cLimitMin * s.mSliderAxis, body2.GetPosition(), 1.0e-4f);
+		CHECK_APPROX_EQUAL(cInitialPos + cLimitMin * s.mSliderAxis1, body2.GetPosition(), 1.0e-4f);
 	}
 
 	// Test a box attached to a slider constraint, test that the body doesn't move beyond the max limit
@@ -63,7 +64,8 @@ TEST_SUITE("SliderConstraintTests")
 
 		// Create slider constraint
 		SliderConstraintSettings s;
-		s.mSliderAxis = Vec3::sAxisX();
+		s.SetPoint(body1, body2);
+		s.SetSliderAxis(Vec3::sAxisX());
 		s.mLimitsMin = 0.0f;
 		s.mLimitsMax = cLimitMax;
 		c.CreateConstraint<SliderConstraint>(body1, body2, s);
@@ -75,7 +77,7 @@ TEST_SUITE("SliderConstraintTests")
 		CHECK_APPROX_EQUAL(Vec3::sZero(), body2.GetLinearVelocity(), 1.0e-4f);
 
 		// Test resulting position
-		CHECK_APPROX_EQUAL(cInitialPos + cLimitMax * s.mSliderAxis, body2.GetPosition(), 1.0e-4f);
+		CHECK_APPROX_EQUAL(cInitialPos + cLimitMax * s.mSliderAxis1, body2.GetPosition(), 1.0e-4f);
 	}
 
 	// Test a box attached to a slider constraint, test that a motor can drive it to a specific velocity
@@ -91,7 +93,8 @@ TEST_SUITE("SliderConstraintTests")
 
 		// Create slider constraint
 		SliderConstraintSettings s;
-		s.mSliderAxis = Vec3::sAxisX();
+		s.SetPoint(body1, body2);
+		s.SetSliderAxis(Vec3::sAxisX());
 		constexpr float mass = Cubed(2.0f) * 1000.0f; // Density * Volume
 		s.mMotorSettings = MotorSettings(0.0f, 0.0f, mass * cMotorAcceleration, 0.0f);
 		SliderConstraint &constraint = c.CreateConstraint<SliderConstraint>(body1, body2, s);
@@ -102,18 +105,18 @@ TEST_SUITE("SliderConstraintTests")
 		c.Simulate(1.0f);
 
 		// Test resulting velocity
-		Vec3 expected_vel = cMotorAcceleration * s.mSliderAxis;
+		Vec3 expected_vel = cMotorAcceleration * s.mSliderAxis1;
 		CHECK_APPROX_EQUAL(expected_vel, body2.GetLinearVelocity(), 1.0e-4f);
 
 		// Simulate (after 0.5 seconds it should reach the target velocity)
 		c.Simulate(1.0f);
 
 		// Test resulting velocity
-		expected_vel = 1.5f * cMotorAcceleration * s.mSliderAxis;
+		expected_vel = 1.5f * cMotorAcceleration * s.mSliderAxis1;
 		CHECK_APPROX_EQUAL(expected_vel, body2.GetLinearVelocity(), 1.0e-4f);
 
 		// Test resulting position (1.5s of acceleration + 0.5s of constant speed)
-		Vec3 expected_pos = c.PredictPosition(cInitialPos, Vec3::sZero(), cMotorAcceleration * s.mSliderAxis, 1.5f) + 0.5f * expected_vel;
+		Vec3 expected_pos = c.PredictPosition(cInitialPos, Vec3::sZero(), cMotorAcceleration * s.mSliderAxis1, 1.5f) + 0.5f * expected_vel;
 		CHECK_APPROX_EQUAL(expected_pos, body2.GetPosition(), 1.0e-4f);
 	}
 
@@ -131,7 +134,8 @@ TEST_SUITE("SliderConstraintTests")
 
 		// Create slider constraint
 		SliderConstraintSettings s;
-		s.mSliderAxis = Vec3::sAxisX();
+		s.SetPoint(body1, body2);
+		s.SetSliderAxis(Vec3::sAxisX());
 		constexpr float mass = Cubed(2.0f) * 1000.0f; // Density * Volume
 		s.mMotorSettings = MotorSettings(0.0f, 0.0f, mass * cMotorAcceleration, 0.0f);
 		SliderConstraint &constraint = c.CreateConstraint<SliderConstraint>(body1, body2, s);
@@ -142,7 +146,7 @@ TEST_SUITE("SliderConstraintTests")
 		c.Simulate(1.0f);
 
 		// Test resulting velocity (both boxes move in opposite directions with the same force, so the resulting velocity difference is 2x as big as the previous test)
-		Vec3 expected_vel = cMotorAcceleration * s.mSliderAxis;
+		Vec3 expected_vel = cMotorAcceleration * s.mSliderAxis1;
 		CHECK_APPROX_EQUAL(-expected_vel, body1.GetLinearVelocity(), 1.0e-4f);
 		CHECK_APPROX_EQUAL(expected_vel, body2.GetLinearVelocity(), 1.0e-4f);
 
@@ -150,13 +154,13 @@ TEST_SUITE("SliderConstraintTests")
 		c.Simulate(1.0f);
 
 		// Test resulting velocity
-		expected_vel = 1.5f * cMotorAcceleration * s.mSliderAxis;
+		expected_vel = 1.5f * cMotorAcceleration * s.mSliderAxis1;
 		CHECK_APPROX_EQUAL(-expected_vel, body1.GetLinearVelocity(), 1.0e-4f);
 		CHECK_APPROX_EQUAL(expected_vel, body2.GetLinearVelocity(), 1.0e-4f);
 
 		// Test resulting position (1.5s of acceleration + 0.5s of constant speed)
-		Vec3 expected_pos1 = c.PredictPosition(Vec3::sZero(), Vec3::sZero(), -cMotorAcceleration * s.mSliderAxis, 1.5f) - 0.5f * expected_vel;
-		Vec3 expected_pos2 = c.PredictPosition(cInitialPos, Vec3::sZero(), cMotorAcceleration * s.mSliderAxis, 1.5f) + 0.5f * expected_vel;
+		Vec3 expected_pos1 = c.PredictPosition(Vec3::sZero(), Vec3::sZero(), -cMotorAcceleration * s.mSliderAxis1, 1.5f) - 0.5f * expected_vel;
+		Vec3 expected_pos2 = c.PredictPosition(cInitialPos, Vec3::sZero(), cMotorAcceleration * s.mSliderAxis1, 1.5f) + 0.5f * expected_vel;
 		CHECK_APPROX_EQUAL(expected_pos1, body1.GetPosition(), 1.0e-4f);
 		CHECK_APPROX_EQUAL(expected_pos2, body2.GetPosition(), 1.0e-4f);
 	}
@@ -174,10 +178,11 @@ TEST_SUITE("SliderConstraintTests")
 
 		// Create slider constraint
 		SliderConstraintSettings s;
-		s.mSliderAxis = Vec3::sAxisX();
+		s.SetPoint(body1, body2);
+		s.SetSliderAxis(Vec3::sAxisX());
 		SliderConstraint &constraint = c.CreateConstraint<SliderConstraint>(body1, body2, s);
 		constraint.SetMotorState(EMotorState::Position);
-		constraint.SetTargetPosition((cMotorPos - cInitialPos).Dot(s.mSliderAxis));
+		constraint.SetTargetPosition((cMotorPos - cInitialPos).Dot(s.mSliderAxis1));
 
 		// Simulate
 		c.Simulate(2.0f);
@@ -205,7 +210,8 @@ TEST_SUITE("SliderConstraintTests")
 
 		// Create slider constraint
 		SliderConstraintSettings s;
-		s.mSliderAxis = Vec3::sAxisX();
+		s.SetPoint(body1, body2);
+		s.SetSliderAxis(Vec3::sAxisX());
 		constexpr float mass = Cubed(2.0f) * 1000.0f; // Density * Volume
 		s.mMaxFrictionForce = mass * cFrictionAcceleration;
 		c.CreateConstraint<SliderConstraint>(body1, body2, s);
@@ -214,11 +220,11 @@ TEST_SUITE("SliderConstraintTests")
 		c.Simulate(cSimulationTime);
 
 		// Test resulting velocity
-		Vec3 expected_vel = cInitialVelocity - cFrictionAcceleration * cSimulationTime * s.mSliderAxis;
+		Vec3 expected_vel = cInitialVelocity - cFrictionAcceleration * cSimulationTime * s.mSliderAxis1;
 		CHECK_APPROX_EQUAL(expected_vel, body2.GetLinearVelocity(), 1.0e-4f);
 
 		// Test resulting position
-		Vec3 expected_pos = c.PredictPosition(cInitialPos, cInitialVelocity, -cFrictionAcceleration * s.mSliderAxis, cSimulationTime);
+		Vec3 expected_pos = c.PredictPosition(cInitialPos, cInitialVelocity, -cFrictionAcceleration * s.mSliderAxis1, cSimulationTime);
 		CHECK_APPROX_EQUAL(expected_pos, body2.GetPosition(), 1.0e-4f);
 	}
 
@@ -232,7 +238,8 @@ TEST_SUITE("SliderConstraintTests")
 
 		// Create slider constraint
 		SliderConstraintSettings s;
-		s.mSliderAxis = Vec3::sAxisX();
+		s.SetPoint(body1, body2);
+		s.SetSliderAxis(Vec3::sAxisX());
 		c.CreateConstraint<SliderConstraint>(body1, body2, s);
 
 		// Verify they're not active
@@ -265,7 +272,8 @@ TEST_SUITE("SliderConstraintTests")
 
 		// Create slider constraint
 		SliderConstraintSettings s;
-		s.mSliderAxis = Vec3::sAxisX();
+		s.SetPoint(body1, body2);
+		s.SetSliderAxis(Vec3::sAxisX());
 		c.CreateConstraint<SliderConstraint>(body1, body2, s);
 
 		// Verify they're not active
@@ -298,7 +306,8 @@ TEST_SUITE("SliderConstraintTests")
 
 		// Create slider constraint
 		SliderConstraintSettings s;
-		s.mSliderAxis = Vec3::sAxisX();
+		s.SetPoint(body1, body2);
+		s.SetSliderAxis(Vec3::sAxisX());
 		c.CreateConstraint<SliderConstraint>(body1, body2, s);
 
 		// Verify they're not active
@@ -331,7 +340,8 @@ TEST_SUITE("SliderConstraintTests")
 
 		// Create slider constraint
 		SliderConstraintSettings s;
-		s.mSliderAxis = Vec3::sAxisX();
+		s.SetPoint(body1, body2);
+		s.SetSliderAxis(Vec3::sAxisX());
 		c.CreateConstraint<SliderConstraint>(body1, body2, s);
 
 		// Verify they're not active
@@ -364,7 +374,8 @@ TEST_SUITE("SliderConstraintTests")
 
 		// Create slider constraint
 		SliderConstraintSettings s;
-		s.mSliderAxis = Vec3::sAxisX();
+		s.SetPoint(body1, body2);
+		s.SetSliderAxis(Vec3::sAxisX());
 		c.CreateConstraint<SliderConstraint>(body1, body2, s);
 
 		// Verify they're not active
@@ -386,4 +397,48 @@ TEST_SUITE("SliderConstraintTests")
 		CHECK(body1.IsActive());
 		CHECK(body2.IsActive());
 	}
+
+	// Test that when a reference frame is provided, the slider constraint is correctly constructed
+	TEST_CASE("TestSliderReferenceFrame")
+	{
+		// Create two boxes in semi random position/orientation
+		PhysicsTestContext c;
+		Body &body1 = c.CreateBox(Vec3(1, 2, 3), Quat::sRotation(Vec3(1, 1, 1).Normalized(), 0.1f * JPH_PI), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1), EActivation::Activate);
+		Body &body2 = c.CreateBox(Vec3(-3, -2, -1), Quat::sRotation(Vec3(1, 0, 1).Normalized(), 0.2f * JPH_PI), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1), EActivation::Activate);
+
+		// Disable collision between the boxes
+		GroupFilterTable *group_filter = new GroupFilterTable(2);
+		group_filter->DisableCollision(0, 1);
+		body1.SetCollisionGroup(CollisionGroup(group_filter, 0, 0));
+		body2.SetCollisionGroup(CollisionGroup(group_filter, 0, 1));
+
+		// Get their transforms
+		Mat44 t1 = body1.GetCenterOfMassTransform();
+		Mat44 t2 = body2.GetCenterOfMassTransform();
+
+		// Create slider constraint so that slider connects the bodies at their center of mass and rotated XY -> YZ
+		SliderConstraintSettings s;
+		s.mPoint1 = t1.GetTranslation();
+		s.mSliderAxis1 = t1.GetColumn3(0);
+		s.mNormalAxis1 = t1.GetColumn3(1);
+		s.mPoint2 = t2.GetTranslation();
+		s.mSliderAxis2 = t2.GetColumn3(1);
+		s.mNormalAxis2 = t2.GetColumn3(2);
+		SliderConstraint &constraint = c.CreateConstraint<SliderConstraint>(body1, body2, s);
+
+		// Activate the motor to drive to 0
+		constraint.SetMotorState(EMotorState::Position);
+		constraint.SetTargetPosition(0);
+
+		// Simulate for a second
+		c.Simulate(1.0f);
+
+		// Now the bodies should have aligned so their COM is at the same position and they're rotated XY -> YZ
+		t1 = body1.GetCenterOfMassTransform();
+		t2 = body2.GetCenterOfMassTransform();
+		CHECK_APPROX_EQUAL(t1.GetColumn3(0), t2.GetColumn3(1), 1.0e-4f);
+		CHECK_APPROX_EQUAL(t1.GetColumn3(1), t2.GetColumn3(2), 1.0e-4f);
+		CHECK_APPROX_EQUAL(t1.GetColumn3(2), t2.GetColumn3(0), 1.0e-4f);
+		CHECK_APPROX_EQUAL(t1.GetTranslation(), t2.GetTranslation(), 1.0e-2f);
+	}
 }