Ability to specify reference space for constraint creation (#90)

This avoids having to transform constraint properties to world space when they are stored in local space (avoids constraint having to transform it back to local space)

Jolt/Physics/Constraints/ConeConstraint.cpp

@@ -18,6 +18,7 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(ConeConstraintSettings)
 {
 	JPH_ADD_BASE_CLASS(ConeConstraintSettings, TwoBodyConstraintSettings)
 
+	JPH_ADD_ENUM_ATTRIBUTE(ConeConstraintSettings, mSpace)
 	JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mPoint1)
 	JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mTwistAxis1)
 	JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mPoint2)
@@ -29,6 +30,7 @@ void ConeConstraintSettings::SaveBinaryState(StreamOut &inStream) const
 { 
 	ConstraintSettings::SaveBinaryState(inStream);
 
+	inStream.Write(mSpace);
 	inStream.Write(mPoint1);
 	inStream.Write(mTwistAxis1);
 	inStream.Write(mPoint2);
@@ -40,6 +42,7 @@ void ConeConstraintSettings::RestoreBinaryState(StreamIn &inStream)
 {
 	ConstraintSettings::RestoreBinaryState(inStream);
 
+	inStream.Read(mSpace);
 	inStream.Read(mPoint1);
 	inStream.Read(mTwistAxis1);
 	inStream.Read(mPoint2);
@@ -53,24 +56,34 @@ TwoBodyConstraint *ConeConstraintSettings::Create(Body &inBody1, Body &inBody2)
 }
 
 ConeConstraint::ConeConstraint(Body &inBody1, Body &inBody2, const ConeConstraintSettings &inSettings) :
-	TwoBodyConstraint(inBody1, inBody2, inSettings)
+	TwoBodyConstraint(inBody1, inBody2, inSettings),
+	mLocalSpacePosition1(inSettings.mPoint1),
+	mLocalSpacePosition2(inSettings.mPoint2),
+	mLocalSpaceTwistAxis1(inSettings.mTwistAxis1),
+	mLocalSpaceTwistAxis2(inSettings.mTwistAxis2)
 {
-	Mat44 inv_transform1 = inBody1.GetInverseCenterOfMassTransform();
-	Mat44 inv_transform2 = inBody2.GetInverseCenterOfMassTransform();
-
-	// Store local positions
-	mLocalSpacePosition1 = inv_transform1 * inSettings.mPoint1;
-	mLocalSpacePosition2 = inv_transform2 * inSettings.mPoint2;
-
-	// Store axis
-	mLocalSpaceTwistAxis1 = inv_transform1.Multiply3x3(inSettings.mTwistAxis1);
-	mLocalSpaceTwistAxis2 = inv_transform2.Multiply3x3(inSettings.mTwistAxis2);
-
 	// Store limits
 	SetHalfConeAngle(inSettings.mHalfConeAngle);
 
 	// Initialize rotation axis to perpendicular of twist axis in case the angle between the twist axis is 0 in the first frame
 	mWorldSpaceRotationAxis = inSettings.mTwistAxis1.GetNormalizedPerpendicular();
+
+	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
+	{
+		// If all properties were specified in world space, take them to local space now
+		Mat44 inv_transform1 = inBody1.GetInverseCenterOfMassTransform();
+		mLocalSpacePosition1 = inv_transform1 * mLocalSpacePosition1;
+		mLocalSpaceTwistAxis1 = inv_transform1.Multiply3x3(mLocalSpaceTwistAxis1);
+
+		Mat44 inv_transform2 = inBody2.GetInverseCenterOfMassTransform();
+		mLocalSpacePosition2 = inv_transform2 * mLocalSpacePosition2;
+		mLocalSpaceTwistAxis2 = inv_transform2.Multiply3x3(mLocalSpaceTwistAxis2);
+	}
+	else
+	{
+		// If they were in local space, we need to take the initial rotation axis to world space
+		mWorldSpaceRotationAxis = inBody1.GetRotation() * mWorldSpaceRotationAxis;
+	}
 }
 
 void ConeConstraint::CalculateRotationConstraintProperties(float inDeltaTime, Mat44Arg inRotation1, Mat44Arg inRotation2)

Jolt/Physics/Constraints/ConeConstraint.h

@@ -21,11 +21,14 @@ public:
 	/// Create an an instance of this constraint
 	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
 
-	/// Body 1 constraint reference frame (in world space)
+	/// This determines in which space the constraint is setup, all properties below should be in the specified space
+	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
+
+	/// Body 1 constraint reference frame (space determined by mSpace)
 	Vec3						mPoint1 = Vec3::sZero();
 	Vec3						mTwistAxis1 = Vec3::sAxisX();
 
-	/// Body 2 constraint reference frame (in world space)
+	/// Body 2 constraint reference frame (space determined by mSpace)
 	Vec3						mPoint2 = Vec3::sZero();
 	Vec3						mTwistAxis2 = Vec3::sAxisX();
 

Jolt/Physics/Constraints/Constraint.h

@@ -19,6 +19,7 @@ class StreamOut;
 class DebugRenderer;
 #endif // JPH_DEBUG_RENDERER
 
+/// Enum to identify constraint type
 enum class EConstraintType
 {
 	Fixed,
@@ -39,6 +40,13 @@ enum class EConstraintType
 	User4
 };
 
+/// Certain constraints support setting them up in local or world space. This governs what is used.
+enum class EConstraintSpace
+{
+	LocalToBodyCOM,				///< All constraint properties are specified in local space to center of mass of the bodies that are being constrained (so e.g. 'constraint position 1' will be local to body 1 COM, 'constraint position 2' will be local to body 2 COM). Note that this means you need to subtract Shape::GetCenterOfMass() from positions!
+	WorldSpace,					///< All constraint properties are specified in world space
+};
+
 /// Class used to store the configuration of a constraint. Allows run-time creation of constraints.
 class ConstraintSettings : public SerializableObject, public RefTarget<ConstraintSettings>, public NonCopyable
 {

Jolt/Physics/Constraints/DistanceConstraint.cpp

@@ -18,6 +18,7 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(DistanceConstraintSettings)
 {
 	JPH_ADD_BASE_CLASS(DistanceConstraintSettings, TwoBodyConstraintSettings)
 
+	JPH_ADD_ENUM_ATTRIBUTE(DistanceConstraintSettings, mSpace)
 	JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mPoint1)
 	JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mPoint2)
 	JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mMinDistance)
@@ -30,6 +31,7 @@ void DistanceConstraintSettings::SaveBinaryState(StreamOut &inStream) const
 { 
 	ConstraintSettings::SaveBinaryState(inStream);
 
+	inStream.Write(mSpace);
 	inStream.Write(mPoint1);
 	inStream.Write(mPoint2);
 	inStream.Write(mMinDistance);
@@ -42,6 +44,7 @@ void DistanceConstraintSettings::RestoreBinaryState(StreamIn &inStream)
 {
 	ConstraintSettings::RestoreBinaryState(inStream);
 
+	inStream.Read(mSpace);
 	inStream.Read(mPoint1);
 	inStream.Read(mPoint2);
 	inStream.Read(mMinDistance);
@@ -56,22 +59,29 @@ TwoBodyConstraint *DistanceConstraintSettings::Create(Body &inBody1, Body &inBod
 }
 
 DistanceConstraint::DistanceConstraint(Body &inBody1, Body &inBody2, const DistanceConstraintSettings &inSettings) :
-	TwoBodyConstraint(inBody1, inBody2, inSettings)
+	TwoBodyConstraint(inBody1, inBody2, inSettings),
+	mLocalSpacePosition1(inSettings.mPoint1),
+	mLocalSpacePosition2(inSettings.mPoint2),
+	mMinDistance(inSettings.mMinDistance),
+	mMaxDistance(inSettings.mMaxDistance),
+	mWorldSpacePosition1(inSettings.mPoint1),
+	mWorldSpacePosition2(inSettings.mPoint2)
 {
-	// Store world space positions
-	mWorldSpacePosition1 = inSettings.mPoint1;
-	mWorldSpacePosition2 = inSettings.mPoint2;
-
-	// Store distance
-	mMinDistance = inSettings.mMinDistance;
-	mMaxDistance = inSettings.mMaxDistance;
-
-	// Store local positions
-	mLocalSpacePosition1 = inBody1.GetInverseCenterOfMassTransform() * inSettings.mPoint1;
-	mLocalSpacePosition2 = inBody2.GetInverseCenterOfMassTransform() * inSettings.mPoint2;
+	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
+	{
+		// If all properties were specified in world space, take them to local space now
+		mLocalSpacePosition1 = inBody1.GetInverseCenterOfMassTransform() * mLocalSpacePosition1;
+		mLocalSpacePosition2 = inBody2.GetInverseCenterOfMassTransform() * mLocalSpacePosition2;
+	}
+	else
+	{
+		// If properties were specified in local space, we need to calculate world space positions
+		mWorldSpacePosition1 = inBody1.GetCenterOfMassTransform() * mWorldSpacePosition1;
+		mWorldSpacePosition2 = inBody2.GetCenterOfMassTransform() * mWorldSpacePosition2;
+	}
 
 	// Store distance we want to keep between the world space points
-	float distance = (inSettings.mPoint2 - inSettings.mPoint1).Length();
+	float distance = (mWorldSpacePosition2 - mWorldSpacePosition1).Length();
 	SetDistance(mMinDistance < 0.0f? distance : mMinDistance, mMaxDistance < 0.0f? distance : mMaxDistance);
 
 	// Most likely gravity is going to tear us apart (this is only used when the distance between the points = 0)

Jolt/Physics/Constraints/DistanceConstraint.h

@@ -20,15 +20,18 @@ public:
 	/// Create an an instance of this constraint
 	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
 
-	/// Body 1 constraint reference frame (in world space).
-	/// Constraint will keep mPoint1 (a point on body 1, world space) and mPoint2 (a point on body 2, world space) at the same distance.
+	/// This determines in which space the constraint is setup, all properties below should be in the specified space
+	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
+
+	/// Body 1 constraint reference frame (space determined by mSpace).
+	/// Constraint will keep mPoint1 (a point on body 1) and mPoint2 (a point on body 2) at the same distance.
 	/// Note that this constraint can be used as a cheap PointConstraint by setting mPoint1 = mPoint2 (but this removes only 1 degree of freedom instead of 3).
 	Vec3						mPoint1 = Vec3::sZero();
 
-	/// Body 2 constraint reference frame (in world space)
+	/// Body 2 constraint reference frame (space determined by mSpace)
 	Vec3						mPoint2 = Vec3::sZero();
 
-	/// Ability to override the distance range at which the two points are kept apart. If the value is negative, it will be replaced by the distance between mPoint1 and mPoint2.
+	/// Ability to override the distance range at which the two points are kept apart. If the value is negative, it will be replaced by the distance between mPoint1 and mPoint2 (works only if mSpace is world space).
 	float						mMinDistance = -1.0f;
 	float						mMaxDistance = -1.0f;
 

Jolt/Physics/Constraints/HingeConstraint.cpp

@@ -18,6 +18,7 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(HingeConstraintSettings)
 {
 	JPH_ADD_BASE_CLASS(HingeConstraintSettings, TwoBodyConstraintSettings)
 
+	JPH_ADD_ENUM_ATTRIBUTE(HingeConstraintSettings, mSpace)
 	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mPoint1)
 	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mHingeAxis1)
 	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mNormalAxis1)
@@ -34,6 +35,7 @@ void HingeConstraintSettings::SaveBinaryState(StreamOut &inStream) const
 { 
 	ConstraintSettings::SaveBinaryState(inStream);
 
+	inStream.Write(mSpace);
 	inStream.Write(mPoint1);
 	inStream.Write(mHingeAxis1);
 	inStream.Write(mNormalAxis1);
@@ -50,6 +52,7 @@ void HingeConstraintSettings::RestoreBinaryState(StreamIn &inStream)
 {
 	ConstraintSettings::RestoreBinaryState(inStream);
 
+	inStream.Read(mSpace);
 	inStream.Read(mPoint1);
 	inStream.Read(mHingeAxis1);
 	inStream.Read(mNormalAxis1);
@@ -68,24 +71,15 @@ TwoBodyConstraint *HingeConstraintSettings::Create(Body &inBody1, Body &inBody2)
 
 HingeConstraint::HingeConstraint(Body &inBody1, Body &inBody2, const HingeConstraintSettings &inSettings) :
 	TwoBodyConstraint(inBody1, inBody2, inSettings),
+	mLocalSpacePosition1(inSettings.mPoint1),
+	mLocalSpacePosition2(inSettings.mPoint2),
+	mLocalSpaceHingeAxis1(inSettings.mHingeAxis1),
+	mLocalSpaceHingeAxis2(inSettings.mHingeAxis2),
+	mLocalSpaceNormalAxis1(inSettings.mNormalAxis1),
+	mLocalSpaceNormalAxis2(inSettings.mNormalAxis2),
 	mMaxFrictionTorque(inSettings.mMaxFrictionTorque),
 	mMotorSettings(inSettings.mMotorSettings)
 {
-	Mat44 inv_transform1 = inBody1.GetInverseCenterOfMassTransform();
-	Mat44 inv_transform2 = inBody2.GetInverseCenterOfMassTransform();
-
-	// Store local positions
-	mLocalSpacePosition1 = inv_transform1 * inSettings.mPoint1;
-	mLocalSpacePosition2 = inv_transform2 * inSettings.mPoint2;
-
-	// Store local hinge axis
-	mLocalSpaceHingeAxis1 = inv_transform1.Multiply3x3(inSettings.mHingeAxis1).Normalized();
-	mLocalSpaceHingeAxis2 = inv_transform2.Multiply3x3(inSettings.mHingeAxis2).Normalized();
-	
-	// Store local normal axis
-	mLocalSpaceNormalAxis1 = inv_transform1.Multiply3x3(inSettings.mNormalAxis1).Normalized();
-	mLocalSpaceNormalAxis2 = inv_transform2.Multiply3x3(inSettings.mNormalAxis2).Normalized();
-
 	// Store limits
 	JPH_ASSERT(inSettings.mLimitsMin != inSettings.mLimitsMax, "Better use a fixed constraint in this case");
 	SetLimits(inSettings.mLimitsMin, inSettings.mLimitsMax);
@@ -98,23 +92,47 @@ HingeConstraint::HingeConstraint(Body &inBody1, Body &inBody2, const HingeConstr
 	//
 	// where:
 	//
-	// q20 = initial orientation of body 2
-	// q10 = initial orientation of body 1
-	// r0 = initial rotation rotation from body 1 to body 2
+	// q10, q20 = world space initial orientation of body 1 and 2
+	// r0 = initial rotation rotation from body 1 to body 2 in local space of body 1
+	//
+	// We can also write this in terms of the constraint matrices:
+	// 
+	// q20 c2 = q10 c1
+	// <=> q20 = q10 c1 c2^-1
+	// => r0 = c1 c2^-1
+	// <=> r0^-1 = c2 c1^-1
+	// 
+	// where:
+	// 
+	// c1, c2 = matrix that takes us from body 1 and 2 COM to constraint space 1 and 2
 	if (inSettings.mHingeAxis1 == inSettings.mHingeAxis2 && inSettings.mNormalAxis1 == inSettings.mNormalAxis2)
 	{
-		// Bodies are in their neutral poses, no need to take hinge and normal axis into account
-		mInvInitialOrientation = inBody2.GetRotation().Conjugated() * inBody1.GetRotation();
+		// Axis are the same -> identity transform
+		mInvInitialOrientation = Quat::sIdentity();
 	}
 	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.mNormalAxis1, 0), Vec4(inSettings.mHingeAxis1.Cross(inSettings.mNormalAxis1), 0), Vec4(inSettings.mHingeAxis1, 0), Vec4(0, 0, 0, 1));
 		Mat44 constraint2(Vec4(inSettings.mNormalAxis2, 0), Vec4(inSettings.mHingeAxis2.Cross(inSettings.mNormalAxis2), 0), Vec4(inSettings.mHingeAxis2, 0), Vec4(0, 0, 0, 1));
-		mInvInitialOrientation = inBody2.GetRotation().Conjugated() * constraint2.GetQuaternion() * constraint1.GetQuaternion().Conjugated() * inBody1.GetRotation();
+		mInvInitialOrientation = constraint2.GetQuaternion() * constraint1.GetQuaternion().Conjugated();
+	}
+
+	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
+	{
+		// If all properties were specified in world space, take them to local space now
+		Mat44 inv_transform1 = inBody1.GetInverseCenterOfMassTransform();
+		mLocalSpacePosition1 = inv_transform1 * mLocalSpacePosition1;
+		mLocalSpaceHingeAxis1 = inv_transform1.Multiply3x3(mLocalSpaceHingeAxis1).Normalized();
+		mLocalSpaceNormalAxis1 = inv_transform1.Multiply3x3(mLocalSpaceNormalAxis1).Normalized();
+
+		Mat44 inv_transform2 = inBody2.GetInverseCenterOfMassTransform();
+		mLocalSpacePosition2 = inv_transform2 * mLocalSpacePosition2;
+		mLocalSpaceHingeAxis2 = inv_transform2.Multiply3x3(mLocalSpaceHingeAxis2).Normalized();
+		mLocalSpaceNormalAxis2 = inv_transform2.Multiply3x3(mLocalSpaceNormalAxis2).Normalized();
+
+		// Constraints were specified in world space, so we should have replaced c1 with q10^-1 c1 and c2 with q20^-1 c2
+		// => r0^-1 = (q20^-1 c2) (q10^-1 c1)^1 = q20^-1 (c2 c1^-1) q10
+		mInvInitialOrientation = inBody2.GetRotation().Conjugated() * mInvInitialOrientation * inBody1.GetRotation();
 	}
 }
 

Jolt/Physics/Constraints/HingeConstraint.h

@@ -23,13 +23,16 @@ public:
 	/// Create an an instance of this constraint
 	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
 
-	/// Body 1 constraint reference frame (in world space).
+	/// This determines in which space the constraint is setup, all properties below should be in the specified space
+	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
+
+	/// Body 1 constraint reference frame (space determined by mSpace).
 	/// Hinge axis is the axis where rotation is allowed, normal axis defines the 0 angle of the hinge.
 	Vec3						mPoint1 = Vec3::sZero();
 	Vec3						mHingeAxis1 = Vec3::sAxisY();
 	Vec3						mNormalAxis1 = Vec3::sAxisX();
 	
-	/// Body 2 constraint reference frame (in world space)
+	/// Body 2 constraint reference frame (space determined by mSpace)
 	Vec3						mPoint2 = Vec3::sZero();
 	Vec3						mHingeAxis2 = Vec3::sAxisY();
 	Vec3						mNormalAxis2 = Vec3::sAxisX();

Jolt/Physics/Constraints/PointConstraint.cpp

@@ -18,6 +18,7 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PointConstraintSettings)
 {
 	JPH_ADD_BASE_CLASS(PointConstraintSettings, TwoBodyConstraintSettings)
 
+	JPH_ADD_ENUM_ATTRIBUTE(PointConstraintSettings, mSpace)
 	JPH_ADD_ATTRIBUTE(PointConstraintSettings, mPoint1)
 	JPH_ADD_ATTRIBUTE(PointConstraintSettings, mPoint2)
 }
@@ -26,6 +27,7 @@ void PointConstraintSettings::SaveBinaryState(StreamOut &inStream) const
 { 
 	ConstraintSettings::SaveBinaryState(inStream);
 
+	inStream.Write(mSpace);
 	inStream.Write(mPoint1);	
 	inStream.Write(mPoint2);
 }
@@ -34,6 +36,7 @@ void PointConstraintSettings::RestoreBinaryState(StreamIn &inStream)
 {
 	ConstraintSettings::RestoreBinaryState(inStream);
 
+	inStream.Read(mSpace);
 	inStream.Read(mPoint1);
 	inStream.Read(mPoint2);
 }
@@ -44,11 +47,16 @@ TwoBodyConstraint *PointConstraintSettings::Create(Body &inBody1, Body &inBody2)
 }
 
 PointConstraint::PointConstraint(Body &inBody1, Body &inBody2, const PointConstraintSettings &inSettings) :
-	TwoBodyConstraint(inBody1, inBody2, inSettings)
+	TwoBodyConstraint(inBody1, inBody2, inSettings),
+	mLocalSpacePosition1(inSettings.mPoint1),
+	mLocalSpacePosition2(inSettings.mPoint2)
 {
-	// Store local positions
-	mLocalSpacePosition1 = inBody1.GetInverseCenterOfMassTransform() * inSettings.mPoint1;
-	mLocalSpacePosition2 = inBody2.GetInverseCenterOfMassTransform() * inSettings.mPoint2;
+	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
+	{
+		// If all properties were specified in world space, take them to local space now
+		mLocalSpacePosition1 = inBody1.GetInverseCenterOfMassTransform() * mLocalSpacePosition1;
+		mLocalSpacePosition2 = inBody2.GetInverseCenterOfMassTransform() * mLocalSpacePosition2;
+	}
 }
 
 void PointConstraint::CalculateConstraintProperties()

Jolt/Physics/Constraints/PointConstraint.h

@@ -20,11 +20,14 @@ public:
 	/// Create an an instance of this constraint
 	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
 
-	/// Body 1 constraint position (world space position).
+	/// This determines in which space the constraint is setup, all properties below should be in the specified space
+	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
+
+	/// Body 1 constraint position (space determined by mSpace).
 	Vec3						mPoint1 = Vec3::sZero();
 
-	/// Body 2 constraint position (world space position).
-	/// Note: Normally you would set mPoint1 = mPoint2 if the bodies are already placed how you want to constrain them.
+	/// Body 2 constraint position (space determined by mSpace).
+	/// Note: Normally you would set mPoint1 = mPoint2 if the bodies are already placed how you want to constrain them (if mSpace = world space).
 	Vec3						mPoint2 = Vec3::sZero();
 
 protected:

Jolt/Physics/Constraints/SixDOFConstraint.cpp

@@ -19,6 +19,7 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(SixDOFConstraintSettings)
 {
 	JPH_ADD_BASE_CLASS(SixDOFConstraintSettings, TwoBodyConstraintSettings)
 
+	JPH_ADD_ENUM_ATTRIBUTE(SixDOFConstraintSettings, mSpace)
 	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mPosition1)
 	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mAxisX1)
 	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mAxisY1)
@@ -35,6 +36,7 @@ void SixDOFConstraintSettings::SaveBinaryState(StreamOut &inStream) const
 { 
 	ConstraintSettings::SaveBinaryState(inStream);
 
+	inStream.Write(mSpace);
 	inStream.Write(mPosition1);
 	inStream.Write(mAxisX1);
 	inStream.Write(mAxisY1);
@@ -52,6 +54,7 @@ void SixDOFConstraintSettings::RestoreBinaryState(StreamIn &inStream)
 {
 	ConstraintSettings::RestoreBinaryState(inStream);
 
+	inStream.Read(mSpace);
 	inStream.Read(mPosition1);
 	inStream.Read(mAxisX1);
 	inStream.Read(mAxisY1);
@@ -91,29 +94,33 @@ void SixDOFConstraint::UpdateRotationLimits()
 }
 
 SixDOFConstraint::SixDOFConstraint(Body &inBody1, Body &inBody2, const SixDOFConstraintSettings &inSettings) :
-	TwoBodyConstraint(inBody1, inBody2, inSettings)
+	TwoBodyConstraint(inBody1, inBody2, inSettings),
+	mLocalSpacePosition1(inSettings.mPosition1),
+	mLocalSpacePosition2(inSettings.mPosition2)
 {
 	// 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]);
 
-	// Calculate position of the constraint in body1 local space
-	Mat44 inv_transform1 = inBody1.GetInverseCenterOfMassTransform();
-	mLocalSpacePosition1 = inv_transform1 * inSettings.mPosition1;
-
-	// Calculate position of the constraint in body2 local space
-	Mat44 inv_transform2 = inBody2.GetInverseCenterOfMassTransform();
-	mLocalSpacePosition2 = inv_transform2 * inSettings.mPosition2;
-
 	// Calculate rotation needed to go from constraint space to body1 local space
 	Vec3 axis_z1 = inSettings.mAxisX1.Cross(inSettings.mAxisY1);
 	Mat44 c_to_b1(Vec4(inSettings.mAxisX1, 0), Vec4(inSettings.mAxisY1, 0), Vec4(axis_z1, 0), Vec4(0, 0, 0, 1));
-	mConstraintToBody1 = inBody1.GetRotation().Conjugated() * c_to_b1.GetQuaternion();
+	mConstraintToBody1 = c_to_b1.GetQuaternion();
 
 	// Calculate rotation needed to go from constraint space to body2 local space
 	Vec3 axis_z2 = inSettings.mAxisX2.Cross(inSettings.mAxisY2);
 	Mat44 c_to_b2(Vec4(inSettings.mAxisX2, 0), Vec4(inSettings.mAxisY2, 0), Vec4(axis_z2, 0), Vec4(0, 0, 0, 1));
-	mConstraintToBody2 = inBody2.GetRotation().Conjugated() * c_to_b2.GetQuaternion();
+	mConstraintToBody2 = c_to_b2.GetQuaternion();
+
+	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
+	{
+		// If all properties were specified in world space, take them to local space now
+		mLocalSpacePosition1 = inBody1.GetInverseCenterOfMassTransform() * mLocalSpacePosition1;
+		mConstraintToBody1 = inBody1.GetRotation().Conjugated() * mConstraintToBody1;
+
+		mLocalSpacePosition2 = inBody2.GetInverseCenterOfMassTransform() * mLocalSpacePosition2;
+		mConstraintToBody2 = inBody2.GetRotation().Conjugated() * mConstraintToBody2;
+	}
 
 	// Cache which axis are fixed and which ones are free
 	mFreeAxis = 0;

Jolt/Physics/Constraints/SixDOFConstraint.h

@@ -39,12 +39,15 @@ public:
 	/// Create an an instance of this constraint
 	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
 
-	/// Body 1 constraint reference frame (in world space)
+	/// This determines in which space the constraint is setup, all properties below should be in the specified space
+	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
+
+	/// Body 1 constraint reference frame (space determined by mSpace)
 	Vec3						mPosition1 = Vec3::sZero();
 	Vec3						mAxisX1 = Vec3::sAxisX();
 	Vec3						mAxisY1 = Vec3::sAxisY();
 
-	/// Body 2 constraint reference frame (in world space)
+	/// Body 2 constraint reference frame (space determined by mSpace)
 	Vec3						mPosition2 = Vec3::sZero();
 	Vec3						mAxisX2 = Vec3::sAxisX();
 	Vec3						mAxisY2 = Vec3::sAxisY();

Jolt/Physics/Constraints/SliderConstraint.cpp

@@ -18,6 +18,7 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(SliderConstraintSettings)
 {
 	JPH_ADD_BASE_CLASS(SliderConstraintSettings, TwoBodyConstraintSettings)
 
+	JPH_ADD_ENUM_ATTRIBUTE(SliderConstraintSettings, mSpace)
 	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mPoint1)
 	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mSliderAxis1)
 	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mNormalAxis1)
@@ -32,6 +33,8 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(SliderConstraintSettings)
 
 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
 	Vec3 anchor;
 	if (!inBody1.CanBeKinematicOrDynamic())
@@ -46,6 +49,8 @@ void SliderConstraintSettings::SetPoint(const Body &inBody1, const Body &inBody2
 
 void SliderConstraintSettings::SetSliderAxis(Vec3Arg inSliderAxis)
 {
+	JPH_ASSERT(mSpace == EConstraintSpace::WorldSpace);
+
 	mSliderAxis1 = mSliderAxis2 = inSliderAxis;
 	mNormalAxis1 = mNormalAxis2 = inSliderAxis.GetNormalizedPerpendicular();
 }
@@ -54,6 +59,7 @@ void SliderConstraintSettings::SaveBinaryState(StreamOut &inStream) const
 { 
 	ConstraintSettings::SaveBinaryState(inStream);
 
+	inStream.Write(mSpace);
 	inStream.Write(mPoint1);
 	inStream.Write(mSliderAxis1);
 	inStream.Write(mNormalAxis1);
@@ -70,6 +76,7 @@ void SliderConstraintSettings::RestoreBinaryState(StreamIn &inStream)
 {
 	ConstraintSettings::RestoreBinaryState(inStream);
 
+	inStream.Read(mSpace);
 	inStream.Read(mPoint1);
 	inStream.Read(mSliderAxis1);
 	inStream.Read(mNormalAxis1);
@@ -89,39 +96,64 @@ TwoBodyConstraint *SliderConstraintSettings::Create(Body &inBody1, Body &inBody2
 
 SliderConstraint::SliderConstraint(Body &inBody1, Body &inBody2, const SliderConstraintSettings &inSettings) :
 	TwoBodyConstraint(inBody1, inBody2, inSettings),
+	mLocalSpacePosition1(inSettings.mPoint1),
+	mLocalSpacePosition2(inSettings.mPoint2),
+	mLocalSpaceSliderAxis1(inSettings.mSliderAxis1),
+	mLocalSpaceNormal1(inSettings.mNormalAxis1),
 	mMaxFrictionForce(inSettings.mMaxFrictionForce),
 	mMotorSettings(inSettings.mMotorSettings)
 {
-	Mat44 inv_transform1 = inBody1.GetInverseCenterOfMassTransform();
-
-	// Store local positions
-	mLocalSpacePosition1 = inv_transform1 * inSettings.mPoint1;
-	mLocalSpacePosition2 = inBody2.GetInverseCenterOfMassTransform() * inSettings.mPoint2;
-
-	// Store local sliding axis
-	mLocalSpaceSliderAxis1 = inv_transform1.Multiply3x3(inSettings.mSliderAxis1).Normalized();
-
-	// Store local space normals
-	mLocalSpaceNormal1 = inv_transform1.Multiply3x3(inSettings.mNormalAxis1).Normalized();
-	mLocalSpaceNormal2 = mLocalSpaceSliderAxis1.Cross(mLocalSpaceNormal1);
-	
-	// Store inverse of initial rotation from body 1 to body 2 in body 1 space
+	// Store inverse of initial rotation from body 1 to body 2 in body 1 space:
+	//
+	// q20 = q10 r0 
+	// <=> r0 = q10^-1 q20 
+	// <=> r0^-1 = q20^-1 q10
+	//
+	// where:
+	//
+	// q10, q20 = world space initial orientation of body 1 and 2
+	// r0 = initial rotation rotation from body 1 to body 2 in local space of body 1
+	//
+	// We can also write this in terms of the constraint matrices:
+	// 
+	// q20 c2 = q10 c1
+	// <=> q20 = q10 c1 c2^-1
+	// => r0 = c1 c2^-1
+	// <=> r0^-1 = c2 c1^-1
+	// 
+	// where:
+	// 
+	// c1, c2 = matrix that takes us from body 1 and 2 COM to constraint space 1 and 2
 	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);
+		// Axis are the same -> identity transform
+		mInvInitialOrientation = Quat::sIdentity();
 	}
 	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();
+		mInvInitialOrientation = constraint2.GetQuaternion() * constraint1.GetQuaternion().Conjugated();
 	}
 
+	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
+	{
+		// If all properties were specified in world space, take them to local space now
+		Mat44 inv_transform1 = inBody1.GetInverseCenterOfMassTransform();
+		mLocalSpacePosition1 = inv_transform1 * mLocalSpacePosition1;
+		mLocalSpaceSliderAxis1 = inv_transform1.Multiply3x3(mLocalSpaceSliderAxis1).Normalized();
+		mLocalSpaceNormal1 = inv_transform1.Multiply3x3(mLocalSpaceNormal1).Normalized();
+
+		mLocalSpacePosition2 = inBody2.GetInverseCenterOfMassTransform() * mLocalSpacePosition2;
+
+		// Constraints were specified in world space, so we should have replaced c1 with q10^-1 c1 and c2 with q20^-1 c2
+		// => r0^-1 = (q20^-1 c2) (q10^-1 c1)^1 = q20^-1 (c2 c1^-1) q10
+		mInvInitialOrientation = inBody2.GetRotation().Conjugated() * mInvInitialOrientation * inBody1.GetRotation();
+	}
+
+	// Calculate 2nd local space normal
+	mLocalSpaceNormal2 = mLocalSpaceSliderAxis1.Cross(mLocalSpaceNormal1);
+
 	// Store limits
 	JPH_ASSERT(inSettings.mLimitsMin != inSettings.mLimitsMax, "Better use a fixed constraint");
 	SetLimits(inSettings.mLimitsMin, inSettings.mLimitsMax);

Jolt/Physics/Constraints/SliderConstraint.h

@@ -23,19 +23,22 @@ public:
 	/// Create an an instance of this constraint
 	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
 
-	/// Simple way of setting the anchor points so that the current relative position is chosen as the '0' position
+	/// 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);
 
-	/// 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.
+	/// This determines in which space the constraint is setup, all properties below should be in the specified space
+	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
+
+	/// 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();
 	Vec3						mSliderAxis1 = Vec3::sAxisX();
 	Vec3						mNormalAxis1 = Vec3::sAxisY();
 	
-	/// Body 2 constraint reference frame (in world space)
+	/// Body 2 constraint reference frame (space determined by mSpace)
 	Vec3						mPoint2 = Vec3::sZero();
 	Vec3						mSliderAxis2 = Vec3::sAxisX();
 	Vec3						mNormalAxis2 = Vec3::sAxisY();
@@ -124,7 +127,7 @@ private:
 	// Local space sliding direction
 	Vec3						mLocalSpaceSliderAxis1;
 
-	// Local space normals to the sliding direction
+	// Local space normals to the sliding direction (in body 1 space)
 	Vec3						mLocalSpaceNormal1;
 	Vec3						mLocalSpaceNormal2;
 

Jolt/Physics/Constraints/SwingTwistConstraint.cpp

@@ -18,6 +18,7 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(SwingTwistConstraintSettings)
 {
 	JPH_ADD_BASE_CLASS(SwingTwistConstraintSettings, TwoBodyConstraintSettings)
 
+	JPH_ADD_ENUM_ATTRIBUTE(SwingTwistConstraintSettings, mSpace)
 	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mPosition1)
 	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mTwistAxis1)
 	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mPlaneAxis1)
@@ -37,6 +38,7 @@ void SwingTwistConstraintSettings::SaveBinaryState(StreamOut &inStream) const
 { 
 	ConstraintSettings::SaveBinaryState(inStream);
 
+	inStream.Write(mSpace);
 	inStream.Write(mPosition1);
 	inStream.Write(mTwistAxis1);
 	inStream.Write(mPlaneAxis1);
@@ -56,6 +58,7 @@ void SwingTwistConstraintSettings::RestoreBinaryState(StreamIn &inStream)
 {
 	ConstraintSettings::RestoreBinaryState(inStream);
 
+	inStream.Read(mSpace);
 	inStream.Read(mPosition1);
 	inStream.Read(mTwistAxis1);
 	inStream.Read(mPlaneAxis1);
@@ -84,6 +87,8 @@ void SwingTwistConstraint::UpdateLimits()
 
 SwingTwistConstraint::SwingTwistConstraint(Body &inBody1, Body &inBody2, const SwingTwistConstraintSettings &inSettings) :
 	TwoBodyConstraint(inBody1, inBody2, inSettings),
+	mLocalSpacePosition1(inSettings.mPosition1),
+	mLocalSpacePosition2(inSettings.mPosition2),
 	mNormalHalfConeAngle(inSettings.mNormalHalfConeAngle),
 	mPlaneHalfConeAngle(inSettings.mPlaneHalfConeAngle),
 	mTwistMinAngle(inSettings.mTwistMinAngle),
@@ -92,23 +97,25 @@ SwingTwistConstraint::SwingTwistConstraint(Body &inBody1, Body &inBody2, const S
 	mSwingMotorSettings(inSettings.mSwingMotorSettings),
 	mTwistMotorSettings(inSettings.mTwistMotorSettings)
 {
-	// Calculate position of the constraint in body1 local space
-	Mat44 inv_transform1 = inBody1.GetInverseCenterOfMassTransform();
-	mLocalSpacePosition1 = inv_transform1 * inSettings.mPosition1;
-
-	// Calculate position of the constraint in body2 local space
-	Mat44 inv_transform2 = inBody2.GetInverseCenterOfMassTransform();
-	mLocalSpacePosition2 = inv_transform2 * inSettings.mPosition2;
-
 	// 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));
-	mConstraintToBody1 = inBody1.GetRotation().Conjugated() * c_to_b1.GetQuaternion();
+	mConstraintToBody1 = c_to_b1.GetQuaternion();
 
 	// Calculate rotation needed to go from constraint space to body2 local space
 	Vec3 normal_axis2 = inSettings.mPlaneAxis2.Cross(inSettings.mTwistAxis2);
 	Mat44 c_to_b2(Vec4(inSettings.mTwistAxis2, 0), Vec4(normal_axis2, 0), Vec4(inSettings.mPlaneAxis2, 0), Vec4(0, 0, 0, 1));
-	mConstraintToBody2 = inBody2.GetRotation().Conjugated() * c_to_b2.GetQuaternion();
+	mConstraintToBody2 = c_to_b2.GetQuaternion();
+
+	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
+	{
+		// If all properties were specified in world space, take them to local space now
+		mLocalSpacePosition1 = inBody1.GetInverseCenterOfMassTransform() * mLocalSpacePosition1;
+		mConstraintToBody1 = inBody1.GetRotation().Conjugated() * mConstraintToBody1;
+
+		mLocalSpacePosition2 = inBody2.GetInverseCenterOfMassTransform() * mLocalSpacePosition2;
+		mConstraintToBody2 = inBody2.GetRotation().Conjugated() * mConstraintToBody2;
+	}
 
 	UpdateLimits();
 }

Jolt/Physics/Constraints/SwingTwistConstraint.h

@@ -28,12 +28,15 @@ public:
 	/// Create an an instance of this constraint
 	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
 
-	///@name Body 1 constraint reference frame (in world space)
+	/// This determines in which space the constraint is setup, all properties below should be in the specified space
+	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
+
+	///@name Body 1 constraint reference frame (space determined by mSpace)
 	Vec3						mPosition1 = Vec3::sZero();
 	Vec3						mTwistAxis1 = Vec3::sAxisX();
 	Vec3						mPlaneAxis1 = Vec3::sAxisY();
 
-	///@name Body 2 constraint reference frame (in world space)
+	///@name Body 2 constraint reference frame (space determined by mSpace)
 	Vec3						mPosition2 = Vec3::sZero();
 	Vec3						mTwistAxis2 = Vec3::sAxisX();
 	Vec3						mPlaneAxis2 = Vec3::sAxisY();