Removed FixedConstraint and SliderConstraint SetPoint functions and replaced with mAutoDetectPoint (#216)

* Removed FixedConstraint and SliderConstraint SetPoint functions and replaced them with mAutoDetectPoint which will calculate the anchor points when the constraint is created. This has the advantage that the bodies don't need to have been created when you create the Settings objects and that the Settings object can be reused for multiple bodies in different orientations. 
* Added missing <algorithm> include for push/pop_heap

Jolt/Geometry/EPAConvexHullBuilder.h

@@ -16,6 +16,10 @@
 	#include <Jolt/Core/StringTools.h>
 #endif
 
+JPH_SUPPRESS_WARNINGS_STD_BEGIN
+#include <algorithm>
+JPH_SUPPRESS_WARNINGS_STD_END
+
 JPH_NAMESPACE_BEGIN
 
 /// A convex hull builder specifically made for the EPA penetration depth calculation. It trades accuracy for speed and will simply abort of the hull forms defects due to numerical precision problems.

Jolt/Physics/Constraints/FixedConstraint.cpp

@@ -17,6 +17,7 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(FixedConstraintSettings)
 	JPH_ADD_BASE_CLASS(FixedConstraintSettings, TwoBodyConstraintSettings)
 
 	JPH_ADD_ENUM_ATTRIBUTE(FixedConstraintSettings, mSpace)
+	JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mAutoDetectPoint)
 	JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mPoint1)
 	JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mAxisX1)
 	JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mAxisY1)
@@ -30,6 +31,7 @@ void FixedConstraintSettings::SaveBinaryState(StreamOut &inStream) const
 	ConstraintSettings::SaveBinaryState(inStream);
 
 	inStream.Write(mSpace);
+	inStream.Write(mAutoDetectPoint);
 	inStream.Write(mPoint1);
 	inStream.Write(mAxisX1);
 	inStream.Write(mAxisY1);
@@ -43,6 +45,7 @@ void FixedConstraintSettings::RestoreBinaryState(StreamIn &inStream)
 	ConstraintSettings::RestoreBinaryState(inStream);
 
 	inStream.Read(mSpace);
+	inStream.Read(mAutoDetectPoint);
 	inStream.Read(mPoint1);
 	inStream.Read(mAxisX1);
 	inStream.Read(mAxisY1);
@@ -56,27 +59,6 @@ TwoBodyConstraint *FixedConstraintSettings::Create(Body &inBody1, Body &inBody2)
 	return new FixedConstraint(inBody1, inBody2, *this);
 }
 
-void FixedConstraintSettings::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
-	Vec3 anchor;
-	if (!inBody1.CanBeKinematicOrDynamic())
-		anchor = inBody2.GetCenterOfMassPosition();
-	else if (!inBody2.CanBeKinematicOrDynamic())
-		anchor = inBody1.GetCenterOfMassPosition();
-	else
-	{
-		// 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;
-}
-
 FixedConstraint::FixedConstraint(Body &inBody1, Body &inBody2, const FixedConstraintSettings &inSettings) :
 	TwoBodyConstraint(inBody1, inBody2, inSettings),
 	mLocalSpacePosition1(inSettings.mPoint1),
@@ -87,6 +69,25 @@ FixedConstraint::FixedConstraint(Body &inBody1, Body &inBody2, const FixedConstr
 
 	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
 	{
+		if (inSettings.mAutoDetectPoint)
+		{
+			// 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
+			{
+				// 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);
+			}
+
+			mLocalSpacePosition1 = mLocalSpacePosition2 = anchor;
+		}
+
 		// Store local positions
 		mLocalSpacePosition1 = inBody1.GetInverseCenterOfMassTransform() * mLocalSpacePosition1;
 		mLocalSpacePosition2 = inBody2.GetInverseCenterOfMassTransform() * mLocalSpacePosition2;

Jolt/Physics/Constraints/FixedConstraint.h

@@ -21,12 +21,12 @@ public:
 	/// Create an an instance of this constraint
 	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
 
-	/// Simple way of calculating the parameters below to fixate two bodies in their current relative position/orientation
-	void						SetPoint(const Body &inBody1, const Body &inBody2);
-
 	/// This determines in which space the constraint is setup, all properties below should be in the specified space
 	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
 
+	/// When mSpace is WorldSpace mPoint1 and mPoint2 can be automatically calculated based on the positions of the bodies when the constraint is created (they will be fixated in their current relative position/orientation). Set this to false if you want to supply the attachment points yourself.
+	bool						mAutoDetectPoint = false;
+
 	/// Body 1 constraint reference frame (space determined by mSpace)
 	Vec3						mPoint1 = Vec3::sZero();
 	Vec3						mAxisX1 = Vec3::sAxisX();

Jolt/Physics/Constraints/SliderConstraint.cpp

@@ -19,6 +19,7 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(SliderConstraintSettings)
 	JPH_ADD_BASE_CLASS(SliderConstraintSettings, TwoBodyConstraintSettings)
 
 	JPH_ADD_ENUM_ATTRIBUTE(SliderConstraintSettings, mSpace)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mAutoDetectPoint)
 	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mPoint1)
 	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mSliderAxis1)
 	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mNormalAxis1)
@@ -33,27 +34,6 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(SliderConstraintSettings)
 	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mMotorSettings)
 }
 
-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
-	Vec3 anchor;
-	if (!inBody1.CanBeKinematicOrDynamic())
-		anchor = inBody2.GetCenterOfMassPosition();
-	else if (!inBody2.CanBeKinematicOrDynamic())
-		anchor = inBody1.GetCenterOfMassPosition();
-	else
-	{
-		// 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;
-}
-
 void SliderConstraintSettings::SetSliderAxis(Vec3Arg inSliderAxis)
 {
 	JPH_ASSERT(mSpace == EConstraintSpace::WorldSpace);
@@ -67,6 +47,7 @@ void SliderConstraintSettings::SaveBinaryState(StreamOut &inStream) const
 	ConstraintSettings::SaveBinaryState(inStream);
 
 	inStream.Write(mSpace);
+	inStream.Write(mAutoDetectPoint);
 	inStream.Write(mPoint1);
 	inStream.Write(mSliderAxis1);
 	inStream.Write(mNormalAxis1);
@@ -86,6 +67,7 @@ void SliderConstraintSettings::RestoreBinaryState(StreamIn &inStream)
 	ConstraintSettings::RestoreBinaryState(inStream);
 
 	inStream.Read(mSpace);
+	inStream.Read(mAutoDetectPoint);
 	inStream.Read(mPoint1);
 	inStream.Read(mSliderAxis1);
 	inStream.Read(mNormalAxis1);
@@ -119,6 +101,25 @@ SliderConstraint::SliderConstraint(Body &inBody1, Body &inBody2, const SliderCon
 
 	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
 	{
+		if (inSettings.mAutoDetectPoint)
+		{
+			// 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
+			{
+				// 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);
+			}
+
+			mLocalSpacePosition1 = mLocalSpacePosition2 = anchor;
+		}
+
 		// If all properties were specified in world space, take them to local space now
 		Mat44 inv_transform1 = inBody1.GetInverseCenterOfMassTransform();
 		mLocalSpacePosition1 = inv_transform1 * mLocalSpacePosition1;

Jolt/Physics/Constraints/SliderConstraint.h

@@ -24,15 +24,15 @@ public:
 	/// Note that the rotation constraint will be solved from body 1. This means that if body 1 and body 2 have different masses / inertias (kinematic body = infinite mass / inertia), 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
-	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);
 
 	/// This determines in which space the constraint is setup, all properties below should be in the specified space
 	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
 
+	/// When mSpace is WorldSpace mPoint1 and mPoint2 can be automatically calculated based on the positions of the bodies when the constraint is created (the current relative position/orientation is chosen as the '0' position). Set this to false if you want to supply the attachment points yourself.
+	bool						mAutoDetectPoint = false;
+
 	/// Body 1 constraint reference frame (space determined by mSpace).
 	/// Slider axis is the axis along which movement is possible (direction), normal axis is a perpendicular vector to define the frame.
 	Vec3						mPoint1 = Vec3::sZero();

Samples/Tests/Constraints/ConstraintSingularityTest.cpp

@@ -62,7 +62,7 @@ void ConstraintSingularityTest::Initialize()
 			default:
 				{
 					FixedConstraintSettings settings;
-					settings.SetPoint(body1, body2);
+					settings.mAutoDetectPoint = true;
 					constraint = settings.Create(body1, body2);
 					break;
 				}

Samples/Tests/Constraints/FixedConstraintTest.cpp

@@ -63,7 +63,7 @@ void FixedConstraintTest::Initialize()
 			mBodyInterface->AddBody(segment.GetID(), EActivation::Activate);
 
 			FixedConstraintSettings settings;
-			settings.SetPoint(*prev, segment);
+			settings.mAutoDetectPoint = true;
 			Ref<Constraint> c = settings.Create(*prev, segment);
 			mPhysicsSystem->AddConstraint(c);
 					
@@ -81,11 +81,11 @@ void FixedConstraintTest::Initialize()
 		mBodyInterface->AddBody(light2->GetID(), EActivation::Activate);
 
 		FixedConstraintSettings light1_heavy;
-		light1_heavy.SetPoint(*light1, *heavy);
+		light1_heavy.mAutoDetectPoint = true;
 		mPhysicsSystem->AddConstraint(light1_heavy.Create(*light1, *heavy));
 
 		FixedConstraintSettings heavy_light2;
-		heavy_light2.SetPoint(*heavy, *light2);
+		heavy_light2.mAutoDetectPoint = true;
 		mPhysicsSystem->AddConstraint(heavy_light2.Create(*heavy, *light2));
 	}
 
@@ -126,7 +126,7 @@ void FixedConstraintTest::Initialize()
 				for (int j = 0; j < 2; ++j)
 				{
 					FixedConstraintSettings constraint;
-					constraint.SetPoint(*pillars[(i + j) % 4], *cross);
+					constraint.mAutoDetectPoint = true;
 					mPhysicsSystem->AddConstraint(constraint.Create(*pillars[(i + j) % 4], *cross));
 				}
 
@@ -134,7 +134,7 @@ void FixedConstraintTest::Initialize()
 				if (prev_pillars[i] != nullptr)
 				{
 					FixedConstraintSettings constraint;
-					constraint.SetPoint(*prev_pillars[i], *pillars[i]);
+					constraint.mAutoDetectPoint = true;
 					mPhysicsSystem->AddConstraint(constraint.Create(*prev_pillars[i], *pillars[i]));
 				}
 
@@ -153,7 +153,7 @@ void FixedConstraintTest::Initialize()
 		for (int i = 0; i < 4; ++i)
 		{
 			FixedConstraintSettings constraint;
-			constraint.SetPoint(*prev_pillars[i], *top);
+			constraint.mAutoDetectPoint = true;
 			mPhysicsSystem->AddConstraint(constraint.Create(*prev_pillars[i], *top));
 		}
 	}

Samples/Tests/Constraints/PoweredSliderConstraintTest.cpp

@@ -42,7 +42,7 @@ void PoweredSliderConstraintTest::Initialize()
 	mBodyInterface->AddBody(mBody2->GetID(), EActivation::Activate);
 
 	SliderConstraintSettings settings;
-	settings.SetPoint(body1, *mBody2);
+	settings.mAutoDetectPoint = true;
 	settings.SetSliderAxis(Vec3::sAxisX());
 	settings.mLimitsMin = -5.0f;
 	settings.mLimitsMax = 100.0f;

Samples/Tests/Constraints/SliderConstraintTest.cpp

@@ -66,7 +66,7 @@ void SliderConstraintTest::Initialize()
 			mBodyInterface->AddBody(segment.GetID(), EActivation::Activate);
 
 			SliderConstraintSettings settings;
-			settings.SetPoint(*prev, segment);
+			settings.mAutoDetectPoint = true;
 			settings.SetSliderAxis(slider_axis);
 			settings.mLimitsMin = -5.0f;
 			settings.mLimitsMax = 10.0f;
@@ -93,7 +93,7 @@ void SliderConstraintTest::Initialize()
 
 		// 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.mAutoDetectPoint = true;
 		slider1.SetSliderAxis(Vec3::sAxisZ());
 		slider1.mLimitsMin = 0.0f;
 		slider1.mLimitsMax = 1.0f;
@@ -101,7 +101,7 @@ void SliderConstraintTest::Initialize()
 
 		// This constraint has the heavy body as body 1 so will work fine
 		SliderConstraintSettings slider2;
-		slider2.SetPoint(*heavy, *light2);
+		slider2.mAutoDetectPoint = true;
 		slider2.SetSliderAxis(Vec3::sAxisZ());
 		slider2.mLimitsMin = 0.0f;
 		slider2.mLimitsMax = 1.0f;
@@ -119,7 +119,7 @@ void SliderConstraintTest::Initialize()
 		++group_id;
 
 		SliderConstraintSettings slider;
-		slider.SetPoint(*vert1, *vert2);
+		slider.mAutoDetectPoint = true;
 		slider.SetSliderAxis(Vec3::sAxisY());
 		slider.mLimitsMin = 0.0f;
 		slider.mLimitsMax = 2.0f;
@@ -137,7 +137,7 @@ void SliderConstraintTest::Initialize()
 		++group_id;
 
 		SliderConstraintSettings slider;
-		slider.SetPoint(*vert1, *vert2);
+		slider.mAutoDetectPoint = true;
 		slider.SetSliderAxis(Vec3::sAxisY());
 		slider.mLimitsMin = 0.0f;
 		slider.mLimitsMax = 2.0f;

UnitTests/Physics/SliderConstraintTests.cpp

@@ -32,7 +32,7 @@ TEST_SUITE("SliderConstraintTests")
 
 		// Create slider constraint
 		SliderConstraintSettings s;
-		s.SetPoint(body1, body2);
+		s.mAutoDetectPoint = true;
 		s.SetSliderAxis(Vec3::sAxisX());
 		s.mLimitsMin = cLimitMin;
 		s.mLimitsMax = 0.0f;
@@ -64,7 +64,7 @@ TEST_SUITE("SliderConstraintTests")
 
 		// Create slider constraint
 		SliderConstraintSettings s;
-		s.SetPoint(body1, body2);
+		s.mAutoDetectPoint = true;
 		s.SetSliderAxis(Vec3::sAxisX());
 		s.mLimitsMin = 0.0f;
 		s.mLimitsMax = cLimitMax;
@@ -93,7 +93,7 @@ TEST_SUITE("SliderConstraintTests")
 
 		// Create slider constraint
 		SliderConstraintSettings s;
-		s.SetPoint(body1, body2);
+		s.mAutoDetectPoint = true;
 		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);
@@ -134,7 +134,7 @@ TEST_SUITE("SliderConstraintTests")
 
 		// Create slider constraint
 		SliderConstraintSettings s;
-		s.SetPoint(body1, body2);
+		s.mAutoDetectPoint = true;
 		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);
@@ -178,7 +178,7 @@ TEST_SUITE("SliderConstraintTests")
 
 		// Create slider constraint
 		SliderConstraintSettings s;
-		s.SetPoint(body1, body2);
+		s.mAutoDetectPoint = true;
 		s.SetSliderAxis(Vec3::sAxisX());
 		SliderConstraint &constraint = c.CreateConstraint<SliderConstraint>(body1, body2, s);
 		constraint.SetMotorState(EMotorState::Position);
@@ -210,7 +210,7 @@ TEST_SUITE("SliderConstraintTests")
 
 		// Create slider constraint
 		SliderConstraintSettings s;
-		s.SetPoint(body1, body2);
+		s.mAutoDetectPoint = true;
 		s.SetSliderAxis(Vec3::sAxisX());
 		constexpr float mass = Cubed(2.0f) * 1000.0f; // Density * Volume
 		s.mMaxFrictionForce = mass * cFrictionAcceleration;
@@ -238,7 +238,7 @@ TEST_SUITE("SliderConstraintTests")
 
 		// Create slider constraint
 		SliderConstraintSettings s;
-		s.SetPoint(body1, body2);
+		s.mAutoDetectPoint = true;
 		s.SetSliderAxis(Vec3::sAxisX());
 		c.CreateConstraint<SliderConstraint>(body1, body2, s);
 
@@ -272,7 +272,7 @@ TEST_SUITE("SliderConstraintTests")
 
 		// Create slider constraint
 		SliderConstraintSettings s;
-		s.SetPoint(body1, body2);
+		s.mAutoDetectPoint = true;
 		s.SetSliderAxis(Vec3::sAxisX());
 		c.CreateConstraint<SliderConstraint>(body1, body2, s);
 
@@ -306,7 +306,7 @@ TEST_SUITE("SliderConstraintTests")
 
 		// Create slider constraint
 		SliderConstraintSettings s;
-		s.SetPoint(body1, body2);
+		s.mAutoDetectPoint = true;
 		s.SetSliderAxis(Vec3::sAxisX());
 		c.CreateConstraint<SliderConstraint>(body1, body2, s);
 
@@ -340,7 +340,7 @@ TEST_SUITE("SliderConstraintTests")
 
 		// Create slider constraint
 		SliderConstraintSettings s;
-		s.SetPoint(body1, body2);
+		s.mAutoDetectPoint = true;
 		s.SetSliderAxis(Vec3::sAxisX());
 		c.CreateConstraint<SliderConstraint>(body1, body2, s);
 
@@ -374,7 +374,7 @@ TEST_SUITE("SliderConstraintTests")
 
 		// Create slider constraint
 		SliderConstraintSettings s;
-		s.SetPoint(body1, body2);
+		s.mAutoDetectPoint = true;
 		s.SetSliderAxis(Vec3::sAxisX());
 		c.CreateConstraint<SliderConstraint>(body1, body2, s);