Added pyramid shape rotation limits for SwingTwist and SixDOF constraints (#807)

For the SixDOFConstraint this allows non-symmetrical rotation limits around the swing Y and Z axis.
For the SwingTwistConstraint you can now select between Cone and Pyramid limits.

Fixes #779

Jolt/Physics/Constraints/ConstraintPart/SwingTwistConstraintPart.h

@@ -10,6 +10,13 @@
 
 JPH_NAMESPACE_BEGIN
 
+/// How the swing limit behaves
+enum class ESwingType : uint8
+{
+	Cone,						///< Swing is limited by a cone shape, note that this cone starts to deform for larger swing angles
+	Pyramid,					///< Swing is limited by a pyramid shape, note that this pyramid starts to deform for larger swing angles
+};
+
 /// Quaternion based constraint that decomposes the rotation in constraint space in swing and twist: q = q_swing * q_twist
 /// where q_swing.x = 0 and where q_twist.y = q_twist.z = 0
 ///
@@ -25,20 +32,44 @@ JPH_NAMESPACE_BEGIN
 class SwingTwistConstraintPart
 {
 public:
+	/// Override the swing type
+	void						SetSwingType(ESwingType inSwingType)
+	{
+		mSwingType = inSwingType;
+	}
+
+	/// Get the swing type for this part
+	ESwingType					GetSwingType() const
+	{
+		return mSwingType;
+	}
+
 	/// Set limits for this constraint (see description above for parameters)
-	void						SetLimits(float inTwistMinAngle, float inTwistMaxAngle, float inSwingYHalfAngle, float inSwingZHalfAngle)
+	void						SetLimits(float inTwistMinAngle, float inTwistMaxAngle, float inSwingYMinAngle, float inSwingYMaxAngle, float inSwingZMinAngle, float inSwingZMaxAngle)
 	{
 		constexpr float cLockedAngle = DegreesToRadians(0.5f);
 		constexpr float cFreeAngle = DegreesToRadians(179.5f);
 
 		// Assume sane input
 		JPH_ASSERT(inTwistMinAngle <= inTwistMinAngle);
-		JPH_ASSERT(inSwingYHalfAngle >= 0.0f && inSwingYHalfAngle <= JPH_PI);
-		JPH_ASSERT(inSwingZHalfAngle >= 0.0f && inSwingZHalfAngle <= JPH_PI);
+		JPH_ASSERT(inSwingYMinAngle <= inSwingYMaxAngle);
+		JPH_ASSERT(inSwingZMinAngle <= inSwingZMaxAngle);
+		JPH_ASSERT(inSwingYMinAngle >= -JPH_PI && inSwingYMaxAngle <= JPH_PI);
+		JPH_ASSERT(inSwingZMinAngle >= -JPH_PI && inSwingZMaxAngle <= JPH_PI);
 
 		// Calculate the sine and cosine of the half angles
-		Vec4 s, c;
-		(0.5f * Vec4(inTwistMinAngle, inTwistMaxAngle, inSwingYHalfAngle, inSwingZHalfAngle)).SinCos(s, c);
+		Vec4 half_twist = 0.5f * Vec4(inTwistMinAngle, inTwistMaxAngle, 0, 0);
+		Vec4 twist_s, twist_c;
+		half_twist.SinCos(twist_s, twist_c);
+		Vec4 half_swing = 0.5f * Vec4(inSwingYMinAngle, inSwingYMaxAngle, inSwingZMinAngle, inSwingZMaxAngle);
+		Vec4 swing_s, swing_c;
+		half_swing.SinCos(swing_s, swing_c);
+
+		// Store half angles for pyramid limit
+		mSwingYHalfMinAngle = half_swing.GetX();
+		mSwingYHalfMaxAngle = half_swing.GetY();
+		mSwingZHalfMinAngle = half_swing.GetZ();
+		mSwingZHalfMaxAngle = half_swing.GetW();
 
 		// Store axis flags which are used at runtime to quickly decided which contraints to apply
 		mRotationFlags = 0;
@@ -60,51 +91,91 @@ public:
 		}
 		else
 		{
-			mSinTwistHalfMinAngle = s.GetX();
-			mSinTwistHalfMaxAngle = s.GetY();
-			mCosTwistHalfMinAngle = c.GetX();
-			mCosTwistHalfMaxAngle = c.GetY();
+			mSinTwistHalfMinAngle = twist_s.GetX();
+			mSinTwistHalfMaxAngle = twist_s.GetY();
+			mCosTwistHalfMinAngle = twist_c.GetX();
+			mCosTwistHalfMaxAngle = twist_c.GetY();
 		}
 
-		if (inSwingYHalfAngle < cLockedAngle)
+		if (inSwingYMinAngle > -cLockedAngle && inSwingYMaxAngle < cLockedAngle)
 		{
 			mRotationFlags |= SwingYLocked;
-			mSinSwingYQuarterAngle = 0.0f;
+			mSinSwingYHalfMinAngle = 0.0f;
+			mSinSwingYHalfMaxAngle = 0.0f;
+			mCosSwingYHalfMinAngle = 1.0f;
+			mCosSwingYHalfMaxAngle = 1.0f;
 		}
-		else if (inSwingYHalfAngle > cFreeAngle)
+		else if (inSwingYMinAngle < -cFreeAngle && inSwingYMaxAngle > cFreeAngle)
 		{
 			mRotationFlags |= SwingYFree;
-			mSinSwingYQuarterAngle = 1.0f;
+			mSinSwingYHalfMinAngle = -1.0f;
+			mSinSwingYHalfMaxAngle = 1.0f;
+			mCosSwingYHalfMinAngle = 0.0f;
+			mCosSwingYHalfMaxAngle = 0.0f;
 		}
 		else
 		{
-			mSinSwingYQuarterAngle = s.GetZ();
+			mSinSwingYHalfMinAngle = swing_s.GetX();
+			mSinSwingYHalfMaxAngle = swing_s.GetY();
+			mCosSwingYHalfMinAngle = swing_c.GetX();
+			mCosSwingYHalfMaxAngle = swing_c.GetY();
+			JPH_ASSERT(mSinSwingYHalfMinAngle < mSinSwingYHalfMaxAngle);
 		}
 
-		if (inSwingZHalfAngle < cLockedAngle)
+		if (inSwingZMinAngle > -cLockedAngle && inSwingZMaxAngle < cLockedAngle)
 		{
 			mRotationFlags |= SwingZLocked;
-			mSinSwingZQuarterAngle = 0.0f;
+			mSinSwingZHalfMinAngle = 0.0f;
+			mSinSwingZHalfMaxAngle = 0.0f;
+			mCosSwingZHalfMinAngle = 1.0f;
+			mCosSwingZHalfMaxAngle = 1.0f;
 		}
-		else if (inSwingZHalfAngle > cFreeAngle)
+		else if (inSwingZMinAngle < -cFreeAngle && inSwingZMaxAngle > cFreeAngle)
 		{
 			mRotationFlags |= SwingZFree;
-			mSinSwingZQuarterAngle = 1.0f;
+			mSinSwingZHalfMinAngle = -1.0f;
+			mSinSwingZHalfMaxAngle = 1.0f;
+			mCosSwingZHalfMinAngle = 0.0f;
+			mCosSwingZHalfMaxAngle = 0.0f;
 		}
 		else
 		{
-			mSinSwingZQuarterAngle = s.GetW();
+			mSinSwingZHalfMinAngle = swing_s.GetZ();
+			mSinSwingZHalfMaxAngle = swing_s.GetW();
+			mCosSwingZHalfMinAngle = swing_c.GetZ();
+			mCosSwingZHalfMaxAngle = swing_c.GetW();
+			JPH_ASSERT(mSinSwingZHalfMinAngle < mSinSwingZHalfMaxAngle);
 		}
 	}
 
+	/// Flags to indicate which axis got clamped by ClampSwingTwist
+	static constexpr uint		cClampedTwistMin = 1 << 0;
+	static constexpr uint		cClampedTwistMax = 1 << 1;
+	static constexpr uint		cClampedSwingYMin = 1 << 2;
+	static constexpr uint		cClampedSwingYMax = 1 << 3;
+	static constexpr uint		cClampedSwingZMin = 1 << 4;
+	static constexpr uint		cClampedSwingZMax = 1 << 5;
+
+	/// Helper function to determine if we're clamped against the min or max limit
+	static JPH_INLINE bool		sDistanceToMinShorter(float inDeltaMin, float inDeltaMax)
+	{
+		// We're outside of the limits, get actual delta to min/max range
+		// Note that a swing/twist of -1 and 1 represent the same angle, so if the difference is bigger than 1, the shortest angle is the other way around (2 - difference)
+		// We should actually be working with angles rather than sin(angle / 2). When the difference is small the approximation is accurate, but
+		// when working with extreme values the calculation is off and e.g. when the limit is between 0 and 180 a value of approx -60 will clamp
+		// to 180 rather than 0 (you'd expect anything > -90 to go to 0).
+		inDeltaMin = abs(inDeltaMin);
+		if (inDeltaMin > 1.0f) inDeltaMin = 2.0f - inDeltaMin;
+		inDeltaMax = abs(inDeltaMax);
+		if (inDeltaMax > 1.0f) inDeltaMax = 2.0f - inDeltaMax;
+		return inDeltaMin < inDeltaMax;
+	}
+
 	/// Clamp twist and swing against the constraint limits, returns which parts were clamped (everything assumed in constraint space)
-	inline void					ClampSwingTwist(Quat &ioSwing, bool &outSwingYClamped, bool &outSwingZClamped, Quat &ioTwist, bool &outTwistClampedToMin, bool &outTwistClampedToMax) const
+	inline void					ClampSwingTwist(Quat &ioSwing, Quat &ioTwist, uint &outClampedAxis) const
 	{
 		// Start with not clamped
-		outTwistClampedToMin = false;
-		outTwistClampedToMax = false;
-		outSwingYClamped = false;
-		outSwingZClamped = false;
+		outClampedAxis = 0;
 
 		// Check that swing and twist quaternions don't contain rotations around the wrong axis
 		JPH_ASSERT(ioSwing.GetX() == 0.0f);
@@ -122,11 +193,8 @@ public:
 		if (mRotationFlags & TwistXLocked)
 		{
 			// Twist axis is locked, clamp whenever twist is not identity
-			if (ioTwist.GetX() != 0.0f)
-			{
-				ioTwist = Quat::sIdentity();
-				outTwistClampedToMin = outTwistClampedToMax = true;
-			}
+			outClampedAxis |= ioTwist.GetX() != 0.0f? (cClampedTwistMin | cClampedTwistMax) : 0;
+			ioTwist = Quat::sIdentity();
 		}
 		else if ((mRotationFlags & TwistXFree) == 0)
 		{
@@ -135,26 +203,16 @@ public:
 			float delta_max = ioTwist.GetX() - mSinTwistHalfMaxAngle;
 			if (delta_min > 0.0f || delta_max > 0.0f)
 			{
-				// We're outside of the limits, get actual delta to min/max range
-				// Note that a twist of -1 and 1 represent the same angle, so if the difference is bigger than 1, the shortest angle is the other way around (2 - difference)
-				// We should actually be working with angles rather than sin(angle / 2). When the difference is small the approximation is accurate, but
-				// when working with extreme values the calculation is off and e.g. when the limit is between 0 and 180 a value of approx -60 will clamp
-				// to 180 rather than 0 (you'd expect anything > -90 to go to 0).
-				delta_min = abs(delta_min);
-				if (delta_min > 1.0f) delta_min = 2.0f - delta_min;
-				delta_max = abs(delta_max);
-				if (delta_max > 1.0f) delta_max = 2.0f - delta_max;
-
 				// Pick the twist that corresponds to the smallest delta
-				if (delta_min < delta_max)
+				if (sDistanceToMinShorter(delta_min, delta_max))
 				{
 					ioTwist = Quat(mSinTwistHalfMinAngle, 0, 0, mCosTwistHalfMinAngle);
-					outTwistClampedToMin = true;
+					outClampedAxis |= cClampedTwistMin;
 				}
 				else
 				{
 					ioTwist = Quat(mSinTwistHalfMaxAngle, 0, 0, mCosTwistHalfMaxAngle);
-					outTwistClampedToMax = true;
+					outClampedAxis |= cClampedTwistMax;
 				}
 			}
 		}
@@ -165,41 +223,99 @@ public:
 			if (mRotationFlags & SwingZLocked)
 			{
 				// Both swing Y and Z are disabled, no degrees of freedom in swing
-				outSwingYClamped = ioSwing.GetY() != 0.0f;
-				outSwingZClamped = ioSwing.GetZ() != 0.0f;
-				if (outSwingYClamped || outSwingZClamped)
-					ioSwing = Quat::sIdentity();
+				outClampedAxis |= ioSwing.GetY() != 0.0f? (cClampedSwingYMin | cClampedSwingYMax) : 0;
+				outClampedAxis |= ioSwing.GetZ() != 0.0f? (cClampedSwingZMin | cClampedSwingZMax) : 0;
+				ioSwing = Quat::sIdentity();
 			}
 			else
 			{
 				// Swing Y angle disabled, only 1 degree of freedom in swing
-				float z = Clamp(ioSwing.GetZ(), -mSinSwingZQuarterAngle, mSinSwingZQuarterAngle);
-				outSwingYClamped = ioSwing.GetY() != 0.0f;
-				outSwingZClamped = z != ioSwing.GetZ();
-				if (outSwingYClamped || outSwingZClamped)
+				outClampedAxis |= ioSwing.GetY() != 0.0f? (cClampedSwingYMin | cClampedSwingYMax) : 0;
+				float delta_min = mSinSwingZHalfMinAngle - ioSwing.GetZ();
+				float delta_max = ioSwing.GetZ() - mSinSwingZHalfMaxAngle;
+				if (delta_min > 0.0f || delta_max > 0.0f)
+				{
+					// Pick the swing that corresponds to the smallest delta
+					if (sDistanceToMinShorter(delta_min, delta_max))
+					{
+						ioSwing = Quat(0, 0, mSinSwingZHalfMinAngle, mCosSwingZHalfMinAngle);
+						outClampedAxis |= cClampedSwingZMin;
+					}
+					else
+					{
+						ioSwing = Quat(0, 0, mSinSwingZHalfMaxAngle, mCosSwingZHalfMaxAngle);
+						outClampedAxis |= cClampedSwingZMax;
+					}
+				}
+				else if ((outClampedAxis & cClampedSwingYMin) != 0)
+				{
+					float z = ioSwing.GetZ();
 					ioSwing = Quat(0, 0, z, sqrt(1.0f - Square(z)));
+				}
 			}
 		}
 		else if (mRotationFlags & SwingZLocked)
 		{
 			// Swing Z angle disabled, only 1 degree of freedom in swing
-			float y = Clamp(ioSwing.GetY(), -mSinSwingYQuarterAngle, mSinSwingYQuarterAngle);
-			outSwingYClamped = y != ioSwing.GetY();
-			outSwingZClamped = ioSwing.GetZ() != 0.0f;
-			if (outSwingYClamped || outSwingZClamped)
+			outClampedAxis |= ioSwing.GetZ() != 0.0f? (cClampedSwingZMin | cClampedSwingZMax) : 0;
+			float delta_min = mSinSwingYHalfMinAngle - ioSwing.GetY();
+			float delta_max = ioSwing.GetY() - mSinSwingYHalfMaxAngle;
+			if (delta_min > 0.0f || delta_max > 0.0f)
+			{
+				// Pick the swing that corresponds to the smallest delta
+				if (sDistanceToMinShorter(delta_min, delta_max))
+				{
+					ioSwing = Quat(0, mSinSwingYHalfMinAngle, 0, mCosSwingYHalfMinAngle);
+					outClampedAxis |= cClampedSwingYMin;
+				}
+				else
+				{
+					ioSwing = Quat(0, mSinSwingYHalfMaxAngle, 0, mCosSwingYHalfMaxAngle);
+					outClampedAxis |= cClampedSwingYMax;
+				}
+			}
+			else if ((outClampedAxis & cClampedSwingZMin) != 0)
+			{
+				float y = ioSwing.GetY();
 				ioSwing = Quat(0, y, 0, sqrt(1.0f - Square(y)));
+			}
 		}
 		else
 		{
-			// Two degrees of freedom, use ellipse to solve limits
-			Ellipse ellipse(mSinSwingYQuarterAngle, mSinSwingZQuarterAngle);
-			Float2 point(ioSwing.GetY(), ioSwing.GetZ());
-			if (!ellipse.IsInside(point))
+			// Two degrees of freedom
+			if (mSwingType == ESwingType::Cone)
 			{
-				Float2 closest = ellipse.GetClosestPoint(point);
-				ioSwing = Quat(0, closest.x, closest.y, sqrt(max(0.0f, 1.0f - Square(closest.x) - Square(closest.y))));
-				outSwingYClamped = true;
-				outSwingZClamped = true;
+				// Use ellipse to solve limits
+				Ellipse ellipse(mSinSwingYHalfMaxAngle, mSinSwingZHalfMaxAngle);
+				Float2 point(ioSwing.GetY(), ioSwing.GetZ());
+				if (!ellipse.IsInside(point))
+				{
+					Float2 closest = ellipse.GetClosestPoint(point);
+					ioSwing = Quat(0, closest.x, closest.y, sqrt(max(0.0f, 1.0f - Square(closest.x) - Square(closest.y))));
+					outClampedAxis |= cClampedSwingYMin | cClampedSwingYMax | cClampedSwingZMin | cClampedSwingZMax; // We're not using the flags on which side we got clamped here
+				}
+			}
+			else
+			{
+				// Use pyramid to solve limits
+				// The quaterion rotating by angle y around the Y axis then rotating by angle z around the Z axis is:
+				// q = Quat::sRotation(Vec3::sAxisZ(), z) * Quat::sRotation(Vec3::sAxisY(), y)
+				// [q.x, q.y, q.z, q.w] = [-sin(y / 2) * sin(z / 2), sin(y / 2) * cos(z / 2), cos(y / 2) * sin(z / 2), cos(y / 2) * cos(z / 2)]
+				// So we can calculate y / 2 = atan2(q.y, q.w) and z / 2 = atan2(q.z, q.w)
+				Vec4 half_angle = Vec4::sATan2(ioSwing.GetXYZW().Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Z>(), ioSwing.GetXYZW().SplatW());
+				Vec4 min_half_angle(mSwingYHalfMinAngle, mSwingYHalfMinAngle, mSwingZHalfMinAngle, mSwingZHalfMinAngle);
+				Vec4 max_half_angle(mSwingYHalfMaxAngle, mSwingYHalfMaxAngle, mSwingZHalfMaxAngle, mSwingZHalfMaxAngle);
+				Vec4 clamped_half_angle = Vec4::sMin(Vec4::sMax(half_angle, min_half_angle), max_half_angle);
+				UVec4 unclamped = Vec4::sEquals(half_angle, clamped_half_angle);
+				if (!unclamped.TestAllTrue())
+				{
+					// We now calculate the quaternion again using the formula for q above,
+					// but we leave out the x component in order to not introduce twist
+					Vec4 s, c;
+					clamped_half_angle.SinCos(s, c);
+					ioSwing = Quat(0, s.GetY() * c.GetZ(), c.GetY() * s.GetZ(), c.GetY() * c.GetZ()).Normalized();
+					outClampedAxis |= cClampedSwingYMin | cClampedSwingYMax | cClampedSwingZMin | cClampedSwingZMax; // We're not using the flags on which side we got clamped here
+				}
 			}
 		}
 
@@ -226,8 +342,8 @@ public:
 
 		// Clamp against joint limits
 		Quat q_clamped_swing = q_swing, q_clamped_twist = q_twist;
-		bool swing_y_clamped, swing_z_clamped, twist_clamped_to_min, twist_clamped_to_max;
-		ClampSwingTwist(q_clamped_swing, swing_y_clamped, swing_z_clamped, q_clamped_twist, twist_clamped_to_min, twist_clamped_to_max);
+		uint clamped_axis;
+		ClampSwingTwist(q_clamped_swing, q_clamped_twist, clamped_axis);
 
 		if (mRotationFlags & SwingYLocked)
 		{
@@ -245,10 +361,10 @@ public:
 			{
 				// Swing only locked around Y
 				mSwingLimitYConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitYRotationAxis);
-				if (swing_z_clamped)
+				if ((clamped_axis & (cClampedSwingZMin | cClampedSwingZMax)) != 0)
 				{
-					if (Sign(q_swing.GetW()) * q_swing.GetZ() < 0.0f)
-						mWorldSpaceSwingLimitZRotationAxis = -mWorldSpaceSwingLimitZRotationAxis; // Flip axis if angle is negative because the impulse limit is going to be between [-FLT_MAX, 0]
+					if ((clamped_axis & cClampedSwingZMin) != 0)
+						mWorldSpaceSwingLimitZRotationAxis = -mWorldSpaceSwingLimitZRotationAxis; // Flip axis if hitting min limit because the impulse limit is going to be between [-FLT_MAX, 0]
 					mSwingLimitZConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitZRotationAxis);
 				}
 				else
@@ -262,10 +378,10 @@ public:
 			mWorldSpaceSwingLimitYRotationAxis = twist_to_world.RotateAxisY();
 			mWorldSpaceSwingLimitZRotationAxis = twist_to_world.RotateAxisZ();
 
-			if (swing_y_clamped)
+			if ((clamped_axis & (cClampedSwingYMin | cClampedSwingYMax)) != 0)
 			{
-				if (Sign(q_swing.GetW()) * q_swing.GetY() < 0.0f)
-					mWorldSpaceSwingLimitYRotationAxis = -mWorldSpaceSwingLimitYRotationAxis; // Flip axis if angle is negative because the impulse limit is going to be between [-FLT_MAX, 0]
+				if ((clamped_axis & cClampedSwingYMin) != 0)
+					mWorldSpaceSwingLimitYRotationAxis = -mWorldSpaceSwingLimitYRotationAxis; // Flip axis if hitting min limit because the impulse limit is going to be between [-FLT_MAX, 0]
 				mSwingLimitYConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitYRotationAxis);
 			}
 			else
@@ -275,7 +391,7 @@ public:
 		else if ((mRotationFlags & SwingYZFree) != SwingYZFree)
 		{
 			// Swing has limits around Y and Z
-			if (swing_y_clamped || swing_z_clamped)
+			if ((clamped_axis & (cClampedSwingYMin | cClampedSwingYMax | cClampedSwingZMin | cClampedSwingZMax)) != 0)
 			{
 				// Calculate axis of rotation from clamped swing to swing
 				Vec3 current = (inConstraintToWorld * q_swing).RotateAxisX();
@@ -310,10 +426,10 @@ public:
 		else if ((mRotationFlags & TwistXFree) == 0)
 		{
 			// Twist has limits
-			if (twist_clamped_to_min || twist_clamped_to_max)
+			if ((clamped_axis & (cClampedTwistMin | cClampedTwistMax)) != 0)
 			{
 				mWorldSpaceTwistLimitRotationAxis = (inConstraintToWorld * q_swing).RotateAxisX();
-				if (twist_clamped_to_min)
+				if ((clamped_axis & cClampedTwistMin) != 0)
 					mWorldSpaceTwistLimitRotationAxis = -mWorldSpaceTwistLimitRotationAxis; // Flip axis if hittin min limit because the impulse limit is going to be between [-FLT_MAX, 0]
 				mTwistLimitConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceTwistLimitRotationAxis);
 			}
@@ -379,11 +495,11 @@ public:
 		Quat q_swing, q_twist;
 		inConstraintRotation.GetSwingTwist(q_swing, q_twist);
 
-		bool swing_y_clamped, swing_z_clamped, twist_clamped_to_min, twist_clamped_to_max;
-		ClampSwingTwist(q_swing, swing_y_clamped, swing_z_clamped, q_twist, twist_clamped_to_min, twist_clamped_to_max);
+		uint clamped_axis;
+		ClampSwingTwist(q_swing, q_twist, clamped_axis);
 
 		// Solve rotation violations
-		if (swing_y_clamped || swing_z_clamped || twist_clamped_to_min || twist_clamped_to_max)
+		if (clamped_axis != 0)
 		{
 			RotationEulerConstraintPart part;
 			Quat inv_initial_orientation = inConstraintToBody2 * (inConstraintToBody1 * q_swing * q_twist).Conjugated();
@@ -447,12 +563,23 @@ private:
 	uint8						mRotationFlags;
 
 	// Constants
+	ESwingType					mSwingType = ESwingType::Cone;
 	float						mSinTwistHalfMinAngle;
 	float						mSinTwistHalfMaxAngle;
 	float						mCosTwistHalfMinAngle;
 	float						mCosTwistHalfMaxAngle;
-	float						mSinSwingYQuarterAngle;
-	float						mSinSwingZQuarterAngle;
+	float						mSwingYHalfMinAngle;
+	float						mSwingYHalfMaxAngle;
+	float						mSwingZHalfMinAngle;
+	float						mSwingZHalfMaxAngle;
+	float						mSinSwingYHalfMinAngle;
+	float						mSinSwingYHalfMaxAngle;
+	float						mSinSwingZHalfMinAngle;
+	float						mSinSwingZHalfMaxAngle;
+	float						mCosSwingYHalfMinAngle;
+	float						mCosSwingYHalfMaxAngle;
+	float						mCosSwingZHalfMinAngle;
+	float						mCosSwingZHalfMaxAngle;
 
 	// RUN TIME PROPERTIES FOLLOW
 

Jolt/Physics/Constraints/SixDOFConstraint.cpp

@@ -28,6 +28,7 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(SixDOFConstraintSettings)
 	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mAxisX2)
 	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mAxisY2)
 	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mMaxFriction)
+	JPH_ADD_ENUM_ATTRIBUTE(SixDOFConstraintSettings, mSwingType)
 	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mLimitMin)
 	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mLimitMax)
 	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mLimitsSpringSettings)
@@ -46,6 +47,7 @@ void SixDOFConstraintSettings::SaveBinaryState(StreamOut &inStream) const
 	inStream.Write(mAxisX2);
 	inStream.Write(mAxisY2);
 	inStream.Write(mMaxFriction);
+	inStream.Write(mSwingType);
 	inStream.Write(mLimitMin);
 	inStream.Write(mLimitMax);
 	for (const SpringSettings &s : mLimitsSpringSettings)
@@ -66,6 +68,7 @@ void SixDOFConstraintSettings::RestoreBinaryState(StreamIn &inStream)
 	inStream.Read(mAxisX2);
 	inStream.Read(mAxisY2);
 	inStream.Read(mMaxFriction);
+	inStream.Read(mSwingType);
 	inStream.Read(mLimitMin);
 	inStream.Read(mLimitMax);
 	for (SpringSettings &s : mLimitsSpringSettings)
@@ -91,20 +94,15 @@ void SixDOFConstraint::UpdateRotationLimits()
 			mLimitMax[i] = min(JPH_PI, mLimitMax[i]);
 		}
 
-	// The swing twist constraint part requires symmetrical rotations around Y and Z
-	JPH_ASSERT(mLimitMin[EAxis::RotationY] == -mLimitMax[EAxis::RotationY]);
-	JPH_ASSERT(mLimitMin[EAxis::RotationZ] == -mLimitMax[EAxis::RotationZ]);
-
 	// Pass limits on to constraint part
-	mSwingTwistConstraintPart.SetLimits(mLimitMin[EAxis::RotationX], mLimitMax[EAxis::RotationX], mLimitMax[EAxis::RotationY], mLimitMax[EAxis::RotationZ]);
+	mSwingTwistConstraintPart.SetLimits(mLimitMin[EAxis::RotationX], mLimitMax[EAxis::RotationX], mLimitMin[EAxis::RotationY], mLimitMax[EAxis::RotationY], mLimitMin[EAxis::RotationZ], mLimitMax[EAxis::RotationZ]);
 }
 
 SixDOFConstraint::SixDOFConstraint(Body &inBody1, Body &inBody2, const SixDOFConstraintSettings &inSettings) :
 	TwoBodyConstraint(inBody1, inBody2, inSettings)
 {
-	// Assert that input adheres to the limitations of this class
-	JPH_ASSERT(inSettings.mLimitMin[EAxis::RotationY] == -inSettings.mLimitMax[EAxis::RotationY]);
-	JPH_ASSERT(inSettings.mLimitMin[EAxis::RotationZ] == -inSettings.mLimitMax[EAxis::RotationZ]);
+	// Override swing type
+	mSwingTwistConstraintPart.SetSwingType(inSettings.mSwingType);
 
 	// Calculate rotation needed to go from constraint space to body1 local space
 	Vec3 axis_z1 = inSettings.mAxisX1.Cross(inSettings.mAxisY1);
@@ -294,10 +292,10 @@ void SixDOFConstraint::SetTargetOrientationCS(QuatArg inOrientation)
 	Quat q_swing, q_twist;
 	inOrientation.GetSwingTwist(q_swing, q_twist);
 
-	bool swing_y_clamped, swing_z_clamped, twist_clamped_to_min, twist_clamped_to_max;
-	mSwingTwistConstraintPart.ClampSwingTwist(q_swing, swing_y_clamped, swing_z_clamped, q_twist, twist_clamped_to_min, twist_clamped_to_max);
+	uint clamped_axis;
+	mSwingTwistConstraintPart.ClampSwingTwist(q_swing, q_twist, clamped_axis);
 
-	if (swing_y_clamped || swing_z_clamped || twist_clamped_to_min || twist_clamped_to_max)
+	if (clamped_axis != 0)
 		mTargetOrientation = q_swing * q_twist;
 	else
 		mTargetOrientation = inOrientation;
@@ -763,7 +761,12 @@ void SixDOFConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const
 	RMat44 constraint_body1_to_world = RMat44::sRotationTranslation(mBody1->GetRotation() * mConstraintToBody1, mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1);
 
 	// Draw limits
-	inRenderer->DrawSwingLimits(constraint_body1_to_world, mLimitMax[EAxis::RotationY], mLimitMax[EAxis::RotationZ], mDrawConstraintSize, Color::sGreen, DebugRenderer::ECastShadow::Off);
+	if (mSwingTwistConstraintPart.GetSwingType() == ESwingType::Pyramid
+		|| mLimitMin[EAxis::RotationY] >= mLimitMax[EAxis::RotationY]
+		|| mLimitMin[EAxis::RotationZ] >= mLimitMax[EAxis::RotationZ])
+		inRenderer->DrawSwingPyramidLimits(constraint_body1_to_world, mLimitMin[EAxis::RotationY], mLimitMax[EAxis::RotationY], mLimitMin[EAxis::RotationZ], mLimitMax[EAxis::RotationZ], mDrawConstraintSize, Color::sGreen, DebugRenderer::ECastShadow::Off);
+	else
+		inRenderer->DrawSwingConeLimits(constraint_body1_to_world, mLimitMax[EAxis::RotationY], mLimitMax[EAxis::RotationZ], mDrawConstraintSize, Color::sGreen, DebugRenderer::ECastShadow::Off);
 	inRenderer->DrawPie(constraint_body1_to_world.GetTranslation(), mDrawConstraintSize, constraint_body1_to_world.GetAxisX(), constraint_body1_to_world.GetAxisY(), mLimitMin[EAxis::RotationX], mLimitMax[EAxis::RotationX], Color::sPurple, DebugRenderer::ECastShadow::Off);
 }
 #endif // JPH_DEBUG_RENDERER

Jolt/Physics/Constraints/SixDOFConstraint.h

@@ -28,8 +28,8 @@ public:
 		TranslationZ,
 
 		RotationX,				///< When limited: Should be \f$\in [-\pi, \pi]\f$. Can by asymmetric.
-		RotationY,				///< When limited: MaxLimit \f$\in [0, \pi]\f$. MinLimit = -MaxLimit. Forms a cone shaped limit with Z.
-		RotationZ,				///< When limited: MaxLimit \f$\in [0, \pi]\f$. MinLimit = -MaxLimit. Forms a cone shaped limit with Y.
+		RotationY,				///< Forms a pyramid or cone shaped limit with Z. For pyramid, should be \f$\in [-\pi, \pi]\f$, for cone \f$\in [0, \pi]\f$.
+		RotationZ,				///< Forms a pyramid or cone shaped limit with Y. For pyramid, should be \f$\in [-\pi, \pi]\f$, for cone \f$\in [0, \pi]\f$.
 
 		Num,
 		NumTranslation = TranslationZ + 1,
@@ -59,6 +59,9 @@ public:
 	/// For rotation: Max friction torque in Nm. 0 = no friction.
 	float						mMaxFriction[EAxis::Num] = { 0, 0, 0, 0, 0, 0 };
 
+	/// The type of swing constraint that we want to use.
+	ESwingType					mSwingType = ESwingType::Cone;
+
 	/// Limits.
 	/// For translation: Min and max linear limits in m (0 is frame of body 1 and 2 coincide).
 	/// For rotation: Min and max angular limits in rad (0 is frame of body 1 and 2 coincide). See comments at Axis enum for limit ranges.

Jolt/Physics/Constraints/SwingTwistConstraint.cpp

@@ -26,6 +26,7 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(SwingTwistConstraintSettings)
 	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mPosition2)
 	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mTwistAxis2)
 	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mPlaneAxis2)
+	JPH_ADD_ENUM_ATTRIBUTE(SwingTwistConstraintSettings, mSwingType)
 	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mNormalHalfConeAngle)
 	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mPlaneHalfConeAngle)
 	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mTwistMinAngle)
@@ -46,6 +47,7 @@ void SwingTwistConstraintSettings::SaveBinaryState(StreamOut &inStream) const
 	inStream.Write(mPosition2);
 	inStream.Write(mTwistAxis2);
 	inStream.Write(mPlaneAxis2);
+	inStream.Write(mSwingType);
 	inStream.Write(mNormalHalfConeAngle);
 	inStream.Write(mPlaneHalfConeAngle);
 	inStream.Write(mTwistMinAngle);
@@ -66,6 +68,7 @@ void SwingTwistConstraintSettings::RestoreBinaryState(StreamIn &inStream)
 	inStream.Read(mPosition2);
 	inStream.Read(mTwistAxis2);
 	inStream.Read(mPlaneAxis2);
+	inStream.Read(mSwingType);
 	inStream.Read(mNormalHalfConeAngle);
 	inStream.Read(mPlaneHalfConeAngle);
 	inStream.Read(mTwistMinAngle);
@@ -83,7 +86,7 @@ TwoBodyConstraint *SwingTwistConstraintSettings::Create(Body &inBody1, Body &inB
 void SwingTwistConstraint::UpdateLimits()
 {
 	// Pass limits on to swing twist constraint part
-	mSwingTwistConstraintPart.SetLimits(mTwistMinAngle, mTwistMaxAngle, mPlaneHalfConeAngle, mNormalHalfConeAngle);
+	mSwingTwistConstraintPart.SetLimits(mTwistMinAngle, mTwistMaxAngle, -mPlaneHalfConeAngle, mPlaneHalfConeAngle, -mNormalHalfConeAngle, mNormalHalfConeAngle);
 }
 
 SwingTwistConstraint::SwingTwistConstraint(Body &inBody1, Body &inBody2, const SwingTwistConstraintSettings &inSettings) :
@@ -96,6 +99,9 @@ SwingTwistConstraint::SwingTwistConstraint(Body &inBody1, Body &inBody2, const S
 	mSwingMotorSettings(inSettings.mSwingMotorSettings),
 	mTwistMotorSettings(inSettings.mTwistMotorSettings)
 {
+	// Override swing type
+	mSwingTwistConstraintPart.SetSwingType(inSettings.mSwingType);
+
 	// Calculate rotation needed to go from constraint space to body1 local space
 	Vec3 normal_axis1 = inSettings.mPlaneAxis1.Cross(inSettings.mTwistAxis1);
 	Mat44 c_to_b1(Vec4(inSettings.mTwistAxis1, 0), Vec4(normal_axis1, 0), Vec4(inSettings.mPlaneAxis1, 0), Vec4(0, 0, 0, 1));
@@ -182,10 +188,10 @@ void SwingTwistConstraint::SetTargetOrientationCS(QuatArg inOrientation)
 	Quat q_swing, q_twist;
 	inOrientation.GetSwingTwist(q_swing, q_twist);
 
-	bool swing_y_clamped, swing_z_clamped, twist_clamped_to_min, twist_clamped_to_max;
-	mSwingTwistConstraintPart.ClampSwingTwist(q_swing, swing_y_clamped, swing_z_clamped, q_twist, twist_clamped_to_min, twist_clamped_to_max);
+	uint clamped_axis;
+	mSwingTwistConstraintPart.ClampSwingTwist(q_swing, q_twist, clamped_axis);
 
-	if (swing_y_clamped || swing_z_clamped || twist_clamped_to_min || twist_clamped_to_max)
+	if (clamped_axis != 0)
 		mTargetOrientation = q_swing * q_twist;
 	else
 		mTargetOrientation = inOrientation;
@@ -447,7 +453,10 @@ void SwingTwistConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const
 	RMat44 constraint_to_world = RMat44::sRotationTranslation(mBody1->GetRotation() * mConstraintToBody1, mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1);
 
 	// Draw limits
-	inRenderer->DrawSwingLimits(constraint_to_world, mPlaneHalfConeAngle, mNormalHalfConeAngle, mDrawConstraintSize, Color::sGreen, DebugRenderer::ECastShadow::Off);
+	if (mSwingTwistConstraintPart.GetSwingType() == ESwingType::Pyramid)
+		inRenderer->DrawSwingPyramidLimits(constraint_to_world, -mPlaneHalfConeAngle, mPlaneHalfConeAngle, -mNormalHalfConeAngle, mNormalHalfConeAngle, mDrawConstraintSize, Color::sGreen, DebugRenderer::ECastShadow::Off);
+	else
+		inRenderer->DrawSwingConeLimits(constraint_to_world, mPlaneHalfConeAngle, mNormalHalfConeAngle, mDrawConstraintSize, Color::sGreen, DebugRenderer::ECastShadow::Off);
 	inRenderer->DrawPie(constraint_to_world.GetTranslation(), mDrawConstraintSize, constraint_to_world.GetAxisX(), constraint_to_world.GetAxisY(), mTwistMinAngle, mTwistMaxAngle, Color::sPurple, DebugRenderer::ECastShadow::Off);
 }
 #endif // JPH_DEBUG_RENDERER

Jolt/Physics/Constraints/SwingTwistConstraint.h

@@ -41,6 +41,9 @@ public:
 	Vec3						mTwistAxis2 = Vec3::sAxisX();
 	Vec3						mPlaneAxis2 = Vec3::sAxisY();
 
+	/// The type of swing constraint that we want to use.
+	ESwingType					mSwingType = ESwingType::Cone;
+
 	///@name Swing rotation limits
 	float						mNormalHalfConeAngle = 0.0f;								///< See image at Detailed Description. Angle in radians.
 	float						mPlaneHalfConeAngle = 0.0f;									///< See image at Detailed Description. Angle in radians.

Jolt/Renderer/DebugRenderer.cpp

@@ -797,7 +797,63 @@ void DebugRenderer::DrawOpenCone(RVec3Arg inTop, Vec3Arg inAxis, Vec3Arg inPerpe
 	}
 }
 
-void DebugRenderer::DrawSwingLimits(RMat44Arg inMatrix, float inSwingYHalfAngle, float inSwingZHalfAngle, float inEdgeLength, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode)
+DebugRenderer::Geometry *DebugRenderer::CreateSwingLimitGeometry(int inNumSegments, const Vec3 *inVertices)
+{
+	// Allocate space for vertices
+	int num_vertices = 2 * inNumSegments;
+	Vertex *vertices_start = (Vertex *)JPH_STACK_ALLOC(num_vertices * sizeof(Vertex));
+	Vertex *vertices = vertices_start;
+
+	for (int i = 0; i < inNumSegments; ++i)
+	{
+		// Get output vertices
+		Vertex &top = *(vertices++);
+		Vertex &bottom = *(vertices++);
+
+		// Get local position
+		const Vec3 &pos = inVertices[i];
+
+		// Get local normal
+		const Vec3 &prev_pos = inVertices[(i + inNumSegments - 1) % inNumSegments];
+		const Vec3 &next_pos = inVertices[(i + 1) % inNumSegments];
+		Vec3 normal = 0.5f * (next_pos.Cross(pos).NormalizedOr(Vec3::sZero()) + pos.Cross(prev_pos).NormalizedOr(Vec3::sZero()));
+
+		// Store top vertex
+		top.mPosition = { 0, 0, 0 };
+		normal.StoreFloat3(&top.mNormal);
+		top.mColor = Color::sWhite;
+		top.mUV = { 0, 0 };
+
+		// Store bottom vertex
+		pos.StoreFloat3(&bottom.mPosition);
+		normal.StoreFloat3(&bottom.mNormal);
+		bottom.mColor = Color::sWhite;
+		bottom.mUV = { 0, 0 };
+	}
+
+	// Allocate space for indices
+	int num_indices = 3 * inNumSegments;
+	uint32 *indices_start = (uint32 *)JPH_STACK_ALLOC(num_indices * sizeof(uint32));
+	uint32 *indices = indices_start;
+
+	// Calculate indices
+	for (int i = 0; i < inNumSegments; ++i)
+	{
+		int first = 2 * i;
+		int second = (first + 3) % num_vertices;
+		int third = first + 1;
+
+		// Triangle
+		*indices++ = first;
+		*indices++ = second;
+		*indices++ = third;
+	}
+
+	// Convert to triangle batch
+	return new Geometry(CreateTriangleBatch(vertices_start, num_vertices, indices_start, num_indices), sCalculateBounds(vertices_start, num_vertices));
+}
+
+void DebugRenderer::DrawSwingConeLimits(RMat44Arg inMatrix, float inSwingYHalfAngle, float inSwingZHalfAngle, float inEdgeLength, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode)
 {
 	JPH_PROFILE_FUNCTION();
 
@@ -807,8 +863,8 @@ void DebugRenderer::DrawSwingLimits(RMat44Arg inMatrix, float inSwingYHalfAngle,
 	JPH_ASSERT(inEdgeLength > 0.0f);
 
 	// Check cache
-	SwingLimits limits { inSwingYHalfAngle, inSwingZHalfAngle };
-	GeometryRef &geometry = mSwingLimits[limits];
+	SwingConeLimits limits { inSwingYHalfAngle, inSwingZHalfAngle };
+	GeometryRef &geometry = mSwingConeLimits[limits];
 	if (geometry == nullptr)
 	{
 		// Number of segments to draw the cone with
@@ -827,11 +883,6 @@ void DebugRenderer::DrawSwingLimits(RMat44Arg inMatrix, float inSwingYHalfAngle,
 		float e1_sq = Square(e1);
 		float e2_sq = Square(e2);
 
-		// Allocate space for vertices
-		int num_vertices = 2 * num_segments;
-		Vertex *vertices_start = (Vertex *)JPH_STACK_ALLOC(num_vertices * sizeof(Vertex));
-		Vertex *vertices = vertices_start;
-
 		// Calculate local space vertices for shape
 		Vec3 ls_vertices[num_segments];
 		int tgt_vertex = 0;
@@ -874,59 +925,57 @@ void DebugRenderer::DrawSwingLimits(RMat44Arg inMatrix, float inSwingYHalfAngle,
 				ls_vertices[tgt_vertex++] = q.RotateAxisX();
 			}
 
-		for (int i = 0; i < num_segments; ++i)
-		{
-			// Get output vertices
-			Vertex &top = *(vertices++);
-			Vertex &bottom = *(vertices++);
-
-			// Get local position
-			Vec3 &pos = ls_vertices[i];
-
-			// Get local normal
-			Vec3 &prev_pos = ls_vertices[(i + num_segments - 1) % num_segments];
-			Vec3 &next_pos = ls_vertices[(i + 1) % num_segments];
-			Vec3 normal = 0.5f * (next_pos.Cross(pos).Normalized() + pos.Cross(prev_pos).Normalized());
-
-			// Store top vertex
-			top.mPosition = { 0, 0, 0 };
-			normal.StoreFloat3(&top.mNormal);
-			top.mColor = Color::sWhite;
-			top.mUV = { 0, 0 };
-
-			// Store bottom vertex
-			pos.StoreFloat3(&bottom.mPosition);
-			normal.StoreFloat3(&bottom.mNormal);
-			bottom.mColor = Color::sWhite;
-			bottom.mUV = { 0, 0 };
-		}
+		geometry = CreateSwingLimitGeometry(num_segments, ls_vertices);
+	}
 
-		// Allocate space for indices
-		int num_indices = 3 * num_segments;
-		uint32 *indices_start = (uint32 *)JPH_STACK_ALLOC(num_indices * sizeof(uint32));
-		uint32 *indices = indices_start;
+	DrawGeometry(inMatrix * Mat44::sScale(inEdgeLength), inColor, geometry, ECullMode::Off, inCastShadow, inDrawMode);
+}
 
-		// Calculate indices
-		for (int i = 0; i < num_segments; ++i)
-		{
-			int first = 2 * i;
-			int second = (first + 3) % num_vertices;
-			int third = first + 1;
-
-			// Triangle
-			*indices++ = first;
-			*indices++ = second;
-			*indices++ = third;
-		}
+void DebugRenderer::DrawSwingPyramidLimits(RMat44Arg inMatrix, float inMinSwingYAngle, float inMaxSwingYAngle, float inMinSwingZAngle, float inMaxSwingZAngle, float inEdgeLength, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode)
+{
+	JPH_PROFILE_FUNCTION();
 
-		// Convert to triangle batch
-		geometry = new Geometry(CreateTriangleBatch(vertices_start, num_vertices, indices_start, num_indices), sCalculateBounds(vertices_start, num_vertices));
+	// Assert sane input
+	JPH_ASSERT(inMinSwingYAngle <= inMaxSwingYAngle && inMinSwingZAngle <= inMaxSwingZAngle);
+	JPH_ASSERT(inEdgeLength > 0.0f);
+
+	// Check cache
+	SwingPyramidLimits limits { inMinSwingYAngle, inMaxSwingYAngle, inMinSwingZAngle, inMaxSwingZAngle };
+	GeometryRef &geometry = mSwingPyramidLimits[limits];
+	if (geometry == nullptr)
+	{
+		// Number of segments to draw the cone with
+		const int num_segments = 64;
+		int quarter_num_segments = num_segments / 4;
+
+		// Note that this is q = Quat::sRotation(Vec3::sAxisZ(), z) * Quat::sRotation(Vec3::sAxisY(), y) with q.x set to zero so we don't introduce twist
+		// This matches the calculation in SwingTwistConstraintPart::ClampSwingTwist
+		auto get_axis = [](float inY, float inZ) {
+			float hy = 0.5f * inY;
+			float hz = 0.5f * inZ;
+			float cos_hy = Cos(hy);
+			float cos_hz = Cos(hz);
+			return Quat(0, Sin(hy) * cos_hz, cos_hy * Sin(hz), cos_hy * cos_hz).Normalized().RotateAxisX();
+		};
+
+		// Calculate local space vertices for shape
+		Vec3 ls_vertices[num_segments];
+		int tgt_vertex = 0;
+		for (int segment_iter = 0; segment_iter < quarter_num_segments; ++segment_iter)
+			ls_vertices[tgt_vertex++] = get_axis(inMinSwingYAngle, inMaxSwingZAngle - (inMaxSwingZAngle - inMinSwingZAngle) * segment_iter / quarter_num_segments);
+		for (int segment_iter = 0; segment_iter < quarter_num_segments; ++segment_iter)
+			ls_vertices[tgt_vertex++] = get_axis(inMinSwingYAngle + (inMaxSwingYAngle - inMinSwingYAngle) * segment_iter / quarter_num_segments, inMinSwingZAngle);
+		for (int segment_iter = 0; segment_iter < quarter_num_segments; ++segment_iter)
+			ls_vertices[tgt_vertex++] = get_axis(inMaxSwingYAngle, inMinSwingZAngle + (inMaxSwingZAngle - inMinSwingZAngle) * segment_iter / quarter_num_segments);
+		for (int segment_iter = 0; segment_iter < quarter_num_segments; ++segment_iter)
+			ls_vertices[tgt_vertex++] = get_axis(inMaxSwingYAngle - (inMaxSwingYAngle - inMinSwingYAngle) * segment_iter / quarter_num_segments, inMaxSwingZAngle);
+
+		geometry = CreateSwingLimitGeometry(num_segments, ls_vertices);
 	}
 
 	DrawGeometry(inMatrix * Mat44::sScale(inEdgeLength), inColor, geometry, ECullMode::Off, inCastShadow, inDrawMode);
 }
 
-
 void DebugRenderer::DrawPie(RVec3Arg inCenter, float inRadius, Vec3Arg inNormal, Vec3Arg inAxis, float inMinAngle, float inMaxAngle, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode)
 {
 	if (inMinAngle >= inMaxAngle)

Jolt/Renderer/DebugRenderer.h

@@ -110,7 +110,7 @@ public:
 	/// @param inDrawMode determines if we draw the geometry solid or in wireframe.
 	void								DrawOpenCone(RVec3Arg inTop, Vec3Arg inAxis, Vec3Arg inPerpendicular, float inHalfAngle, float inLength, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid);
 
-	/// Draws rotation limits as used by the SwingTwistConstraintPart.
+	/// Draws cone rotation limits as used by the SwingTwistConstraintPart.
 	/// @param inMatrix Matrix that transforms from constraint space to world space
 	/// @param inSwingYHalfAngle See SwingTwistConstraintPart
 	/// @param inSwingZHalfAngle See SwingTwistConstraintPart
@@ -118,7 +118,19 @@ public:
 	/// @param inColor Color to use for drawing the cone.
 	/// @param inCastShadow determins if this geometry should cast a shadow or not.
 	/// @param inDrawMode determines if we draw the geometry solid or in wireframe.
-	void								DrawSwingLimits(RMat44Arg inMatrix, float inSwingYHalfAngle, float inSwingZHalfAngle, float inEdgeLength, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid);
+	void								DrawSwingConeLimits(RMat44Arg inMatrix, float inSwingYHalfAngle, float inSwingZHalfAngle, float inEdgeLength, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid);
+
+	/// Draws rotation limits as used by the SwingTwistConstraintPart.
+	/// @param inMatrix Matrix that transforms from constraint space to world space
+	/// @param inMinSwingYAngle See SwingTwistConstraintPart
+	/// @param inMaxSwingYAngle See SwingTwistConstraintPart
+	/// @param inMinSwingZAngle See SwingTwistConstraintPart
+	/// @param inMaxSwingZAngle See SwingTwistConstraintPart
+	/// @param inEdgeLength Size of the edge of the cone shape
+	/// @param inColor Color to use for drawing the cone.
+	/// @param inCastShadow determins if this geometry should cast a shadow or not.
+	/// @param inDrawMode determines if we draw the geometry solid or in wireframe.
+	void								DrawSwingPyramidLimits(RMat44Arg inMatrix, float inMinSwingYAngle, float inMaxSwingYAngle, float inMinSwingZAngle, float inMaxSwingZAngle, float inEdgeLength, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid);
 
 	/// Draw a pie (part of a circle).
 	/// @param inCenter The center of the circle.
@@ -240,6 +252,9 @@ private:
 	void								Create8thSphereRecursive(Array<uint32> &ioIndices, Array<Vertex> &ioVertices, Vec3Arg inDir1, uint32 &ioIdx1, Vec3Arg inDir2, uint32 &ioIdx2, Vec3Arg inDir3, uint32 &ioIdx3, const Float2 &inUV, SupportFunction inGetSupport, int inLevel);
 	void								Create8thSphere(Array<uint32> &ioIndices, Array<Vertex> &ioVertices, Vec3Arg inDir1, Vec3Arg inDir2, Vec3Arg inDir3, const Float2 &inUV, SupportFunction inGetSupport, int inLevel);
 
+	/// Helper function for DrawSwingConeLimits and DrawSwingPyramidLimits
+	Geometry *							CreateSwingLimitGeometry(int inNumSegments, const Vec3 *inVertices);
+
 	// Predefined shapes
 	GeometryRef							mBox;
 	GeometryRef							mSphere;
@@ -249,18 +264,43 @@ private:
 	GeometryRef							mOpenCone;
 	GeometryRef							mCylinder;
 
-	struct SwingLimits
+	struct SwingConeLimits
 	{
-		bool							operator == (const SwingLimits &inRHS) const	{ return mSwingYHalfAngle == inRHS.mSwingYHalfAngle && mSwingZHalfAngle == inRHS.mSwingZHalfAngle; }
+		bool							operator == (const SwingConeLimits &inRHS) const
+		{ 
+			return mSwingYHalfAngle == inRHS.mSwingYHalfAngle
+				&& mSwingZHalfAngle == inRHS.mSwingZHalfAngle; 
+		}
 
 		float							mSwingYHalfAngle;
 		float							mSwingZHalfAngle;
 	};
 
-	JPH_MAKE_HASH_STRUCT(SwingLimits, SwingLimitsHasher, t.mSwingYHalfAngle, t.mSwingZHalfAngle)
+	JPH_MAKE_HASH_STRUCT(SwingConeLimits, SwingConeLimitsHasher, t.mSwingYHalfAngle, t.mSwingZHalfAngle)
+
+	using SwingConeBatches = UnorderedMap<SwingConeLimits, GeometryRef, SwingConeLimitsHasher>;
+	SwingConeBatches					mSwingConeLimits;
+
+	struct SwingPyramidLimits
+	{
+		bool							operator == (const SwingPyramidLimits &inRHS) const
+		{
+			return mMinSwingYAngle == inRHS.mMinSwingYAngle
+				&& mMaxSwingYAngle == inRHS.mMaxSwingYAngle
+				&& mMinSwingZAngle == inRHS.mMinSwingZAngle
+				&& mMaxSwingZAngle == inRHS.mMaxSwingZAngle;
+		}
+
+		float							mMinSwingYAngle;
+		float							mMaxSwingYAngle;
+		float							mMinSwingZAngle;
+		float							mMaxSwingZAngle;
+	};
+
+	JPH_MAKE_HASH_STRUCT(SwingPyramidLimits, SwingPyramidLimitsHasher, t.mMinSwingYAngle, t.mMaxSwingYAngle, t.mMinSwingZAngle, t.mMaxSwingZAngle)
 
-	using SwingBatches = UnorderedMap<SwingLimits, GeometryRef, SwingLimitsHasher>;
-	SwingBatches						mSwingLimits;
+	using SwingPyramidBatches = UnorderedMap<SwingPyramidLimits, GeometryRef, SwingPyramidLimitsHasher>;
+	SwingPyramidBatches					mSwingPyramidLimits;
 
 	using PieBatces = UnorderedMap<float, GeometryRef>;
 	PieBatces							mPieLimits;

Samples/Tests/Constraints/SixDOFConstraintTest.cpp

@@ -91,10 +91,13 @@ void SixDOFConstraintTest::CreateSettingsMenu(DebugUI *inUI, UIElement *inSubMen
 {
 	Array<String> labels = { "Translation X", "Translation Y", "Translation Z", "Rotation X", "Rotation Y", "Rotation Z" };
 	Array<String> motor_states = { "Off", "Velocity", "Position" };
+	Array<String> swing_types = { "Cone", "Pyramid" };
 
-	inUI->CreateTextButton(inSubMenu, "Configuration Settings", [this, inUI, labels]() {
+	inUI->CreateTextButton(inSubMenu, "Configuration Settings (Limits)", [this, inUI, labels, swing_types]() {
 		UIElement *configuration_settings = inUI->CreateMenu();
 
+		inUI->CreateComboBox(configuration_settings, "Swing Type", swing_types, (int)sSettings->mSwingType, [](int inItem) { sSettings->mSwingType = (ESwingType)inItem; });
+
 		for (int i = 0; i < 3; ++i)
 		{
 			inUI->CreateCheckBox(configuration_settings, "Enable Limits " + labels[i], sEnableLimits[i], [=](UICheckBox::EState inState) { sEnableLimits[i] = inState == UICheckBox::STATE_CHECKED; });
@@ -107,17 +110,18 @@ void SixDOFConstraintTest::CreateSettingsMenu(DebugUI *inUI, UIElement *inSubMen
 		for (int i = 3; i < 6; ++i)
 		{
 			inUI->CreateCheckBox(configuration_settings, "Enable Limits " + labels[i], sEnableLimits[i], [=](UICheckBox::EState inState) { sEnableLimits[i] = inState == UICheckBox::STATE_CHECKED; });
-			if (i == 3)
-			{
-				inUI->CreateSlider(configuration_settings, "Limit Min", RadiansToDegrees(sLimitMin[i]), -180.0f, 180.0f, 1.0f, [=](float inValue) { sLimitMin[i] = DegreesToRadians(inValue); });
-				inUI->CreateSlider(configuration_settings, "Limit Max", RadiansToDegrees(sLimitMax[i]), -180.0f, 180.0f, 1.0f, [=](float inValue) { sLimitMax[i] = DegreesToRadians(inValue); });
-			}
-			else
-			{
-				inUI->CreateSlider(configuration_settings, "Limit Max", RadiansToDegrees(sLimitMax[i]), 0.0f, 180.0f, 1.0f, [=](float inValue) { sLimitMin[i] = -DegreesToRadians(inValue); sLimitMax[i] = DegreesToRadians(inValue); });
-			}
+			inUI->CreateSlider(configuration_settings, "Limit Min", RadiansToDegrees(sLimitMin[i]), -180.0f, 180.0f, 1.0f, [=](float inValue) { sLimitMin[i] = DegreesToRadians(inValue); });
+			inUI->CreateSlider(configuration_settings, "Limit Max", RadiansToDegrees(sLimitMax[i]), -180.0f, 180.0f, 1.0f, [=](float inValue) { sLimitMax[i] = DegreesToRadians(inValue); });
 		}
 
+		inUI->CreateTextButton(configuration_settings, "Accept Changes", [this]() { RestartTest(); });
+
+		inUI->ShowMenu(configuration_settings);
+	});
+
+	inUI->CreateTextButton(inSubMenu, "Configuration Settings (Other)", [this, inUI, labels]() {
+		UIElement *configuration_settings = inUI->CreateMenu();
+				
 		for (int i = 0; i < 6; ++i)
 			inUI->CreateSlider(configuration_settings, "Max Friction " + labels[i], sSettings->mMaxFriction[i], 0.0f, 500.0f, 1.0f, [=](float inValue) { sSettings->mMaxFriction[i] = inValue; });