Weight anchor point according to inverse mass for fixed/slider constraint (#154)

Also added examples of connecting heavy to light bodies and a comment on which order they should be attached.

Jolt/Physics/Constraints/FixedConstraint.cpp

@@ -25,14 +25,19 @@ TwoBodyConstraint *FixedConstraintSettings::Create(Body &inBody1, Body &inBody2)
 FixedConstraint::FixedConstraint(Body &inBody1, Body &inBody2, const FixedConstraintSettings &inSettings) :
 	TwoBodyConstraint(inBody1, inBody2, inSettings)
 {	
-	// 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
+	// Determine anchor point: If any of the bodies can never be dynamic use the other body as anchor point
 	Vec3 anchor;
 	if (!mBody1->CanBeKinematicOrDynamic())
 		anchor = mBody2->GetCenterOfMassPosition();
 	else if (!mBody2->CanBeKinematicOrDynamic())
 		anchor = mBody1->GetCenterOfMassPosition();
 	else
-		anchor = 0.5f * (mBody1->GetCenterOfMassPosition() + mBody2->GetCenterOfMassPosition());
+	{
+		// Otherwise use weighted anchor point towards the lightest body
+		float inv_m1 = mBody1->GetMotionPropertiesUnchecked()->GetInverseMassUnchecked();
+		float inv_m2 = mBody2->GetMotionPropertiesUnchecked()->GetInverseMassUnchecked();
+		anchor = (inv_m1 * mBody1->GetCenterOfMassPosition() + inv_m2 * mBody2->GetCenterOfMassPosition()) / (inv_m1 + inv_m2);
+	}
 
 	// Store local positions
 	mLocalSpacePosition1 = inBody1.GetInverseCenterOfMassTransform() * anchor;

Jolt/Physics/Constraints/SliderConstraint.cpp

@@ -35,14 +35,19 @@ void SliderConstraintSettings::SetPoint(const Body &inBody1, const Body &inBody2
 {
 	JPH_ASSERT(mSpace == EConstraintSpace::WorldSpace);
 
-	// 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
+	// Determine anchor point: If any of the bodies can never be dynamic use the other body as anchor point
 	Vec3 anchor;
 	if (!inBody1.CanBeKinematicOrDynamic())
 		anchor = inBody2.GetCenterOfMassPosition();
 	else if (!inBody2.CanBeKinematicOrDynamic())
 		anchor = inBody1.GetCenterOfMassPosition();
 	else
-		anchor = 0.5f * (inBody1.GetCenterOfMassPosition() + inBody2.GetCenterOfMassPosition());
+	{
+		// Otherwise use weighted anchor point towards the lightest body
+		float inv_m1 = inBody1.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked();
+		float inv_m2 = inBody2.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked();
+		anchor = (inv_m1 * inBody1.GetCenterOfMassPosition() + inv_m2 * inBody2.GetCenterOfMassPosition()) / (inv_m1 + inv_m2);
+	}
 
 	mPoint1 = mPoint2 = anchor;
 }

Jolt/Physics/Constraints/SliderConstraint.h

@@ -20,7 +20,8 @@ public:
 	// See: ConstraintSettings::SaveBinaryState
 	virtual void				SaveBinaryState(StreamOut &inStream) const override;
 
-	/// Create an an instance of this constraint
+	/// Create an an instance of this constraint.
+	/// Note that the rotation constraint will be solved from body 1. This means that if body 1 and body 2 have different masses (kinematic body = infinite mass), body 1 should be the heaviest body.
 	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
 
 	/// Simple way of setting the anchor points in world space so that the current relative position is chosen as the '0' position

Samples/Tests/Constraints/FixedConstraintTest.cpp

@@ -68,4 +68,15 @@ void FixedConstraintTest::Initialize()
 			prev = &segment;
 		}
 	}
+
+	// Two light bodies attached to a heavy body (gives issues if the wrong anchor point is chosen)
+	Body *light1 = mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sReplicate(0.1f)), Vec3(-5.0f, 7.0f, -5.2f), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+	mBodyInterface->AddBody(light1->GetID(), EActivation::Activate);
+	Body *heavy = mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sReplicate(5.0f)), Vec3(-5.0f, 7.0f, 0.0f), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+	mBodyInterface->AddBody(heavy->GetID(), EActivation::Activate);
+	Body *light2 = mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sReplicate(0.1f)), Vec3(-5.0f, 7.0f, 5.2f), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+	mBodyInterface->AddBody(light2->GetID(), EActivation::Activate);
+
+	mPhysicsSystem->AddConstraint(FixedConstraintSettings().Create(*light1, *heavy));
+	mPhysicsSystem->AddConstraint(FixedConstraintSettings().Create(*heavy, *light2));
 }

Samples/Tests/Constraints/SliderConstraintTest.cpp

@@ -86,4 +86,28 @@ void SliderConstraintTest::Initialize()
 			prev = &segment;
 		}
 	}
+
+	// Two light bodies attached to a heavy body (gives issues if the wrong anchor point is chosen)
+	Body *light1 = mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sReplicate(0.1f)), Vec3(-5.0f, 7.0f, -5.2f), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+	mBodyInterface->AddBody(light1->GetID(), EActivation::Activate);
+	Body *heavy = mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sReplicate(5.0f)), Vec3(-5.0f, 7.0f, 0.0f), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+	mBodyInterface->AddBody(heavy->GetID(), EActivation::Activate);
+	Body *light2 = mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sReplicate(0.1f)), Vec3(-5.0f, 7.0f, 5.2f), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+	mBodyInterface->AddBody(light2->GetID(), EActivation::Activate);
+
+	// Note: This violates the recommendation that body 1 is heavier than body 2, therefore this constraint will not work well (the rotation constraint will not be solved accurately)
+	SliderConstraintSettings slider1;
+	slider1.SetPoint(*light1, *heavy);
+	slider1.SetSliderAxis(Vec3::sAxisZ());
+	slider1.mLimitsMin = 0.0f;
+	slider1.mLimitsMax = 1.0f;
+	mPhysicsSystem->AddConstraint(slider1.Create(*light1, *heavy));
+
+	// This constraint has the heavy body as body 1 so will work fine
+	SliderConstraintSettings slider2;
+	slider2.SetPoint(*heavy, *light2);
+	slider2.SetSliderAxis(Vec3::sAxisZ());
+	slider2.mLimitsMin = 0.0f;
+	slider2.mLimitsMax = 1.0f;
+	mPhysicsSystem->AddConstraint(slider2.Create(*heavy, *light2));
 }