@bloomengine/engine 0.3.1

package/native/third_party/JoltPhysics/Jolt/Physics/Constraints/SixDOFConstraint.cpp

@@ -0,0 +1,900 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/SixDOFConstraint.h>
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Geometry/Ellipse.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+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)
+	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mPosition2)
+	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)
+	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mMotorSettings)
+}
+
+void SixDOFConstraintSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	ConstraintSettings::SaveBinaryState(inStream);
+
+	inStream.Write(mSpace);
+	inStream.Write(mPosition1);
+	inStream.Write(mAxisX1);
+	inStream.Write(mAxisY1);
+	inStream.Write(mPosition2);
+	inStream.Write(mAxisX2);
+	inStream.Write(mAxisY2);
+	inStream.Write(mMaxFriction);
+	inStream.Write(mSwingType);
+	inStream.Write(mLimitMin);
+	inStream.Write(mLimitMax);
+	for (const SpringSettings &s : mLimitsSpringSettings)
+		s.SaveBinaryState(inStream);
+	for (const MotorSettings &m : mMotorSettings)
+		m.SaveBinaryState(inStream);
+}
+
+void SixDOFConstraintSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	ConstraintSettings::RestoreBinaryState(inStream);
+
+	inStream.Read(mSpace);
+	inStream.Read(mPosition1);
+	inStream.Read(mAxisX1);
+	inStream.Read(mAxisY1);
+	inStream.Read(mPosition2);
+	inStream.Read(mAxisX2);
+	inStream.Read(mAxisY2);
+	inStream.Read(mMaxFriction);
+	inStream.Read(mSwingType);
+	inStream.Read(mLimitMin);
+	inStream.Read(mLimitMax);
+	for (SpringSettings &s : mLimitsSpringSettings)
+		s.RestoreBinaryState(inStream);
+	for (MotorSettings &m : mMotorSettings)
+		m.RestoreBinaryState(inStream);
+}
+
+TwoBodyConstraint *SixDOFConstraintSettings::Create(Body &inBody1, Body &inBody2) const
+{
+	return new SixDOFConstraint(inBody1, inBody2, *this);
+}
+
+void SixDOFConstraint::UpdateTranslationLimits()
+{
+	// Set to zero if the limits are inversed
+	for (int i = EAxis::TranslationX; i <= EAxis::TranslationZ; ++i)
+		if (mLimitMin[i] > mLimitMax[i])
+			mLimitMin[i] = mLimitMax[i] = 0.0f;
+}
+
+void SixDOFConstraint::UpdateRotationLimits()
+{
+	if (mSwingTwistConstraintPart.GetSwingType() == ESwingType::Cone)
+	{
+		// Cone swing upper limit needs to be positive
+		mLimitMax[EAxis::RotationY] = max(0.0f, mLimitMax[EAxis::RotationY]);
+		mLimitMax[EAxis::RotationZ] = max(0.0f, mLimitMax[EAxis::RotationZ]);
+
+		// Cone swing limits only support symmetric ranges
+		mLimitMin[EAxis::RotationY] = -mLimitMax[EAxis::RotationY];
+		mLimitMin[EAxis::RotationZ] = -mLimitMax[EAxis::RotationZ];
+	}
+
+	for (int i = EAxis::RotationX; i <= EAxis::RotationZ; ++i)
+	{
+		// Clamp to [-PI, PI] range
+		mLimitMin[i] = Clamp(mLimitMin[i], -JPH_PI, JPH_PI);
+		mLimitMax[i] = Clamp(mLimitMax[i], -JPH_PI, JPH_PI);
+
+		// Set to zero if the limits are inversed
+		if (mLimitMin[i] > mLimitMax[i])
+			mLimitMin[i] = mLimitMax[i] = 0.0f;
+	}
+
+	// Pass limits on to constraint part
+	mSwingTwistConstraintPart.SetLimits(mLimitMin[EAxis::RotationX], mLimitMax[EAxis::RotationX], mLimitMin[EAxis::RotationY], mLimitMax[EAxis::RotationY], mLimitMin[EAxis::RotationZ], mLimitMax[EAxis::RotationZ]);
+}
+
+void SixDOFConstraint::UpdateFixedFreeAxis()
+{
+	uint8 old_free_axis = mFreeAxis;
+	uint8 old_fixed_axis = mFixedAxis;
+
+	// Cache which axis are fixed and which ones are free
+	mFreeAxis = 0;
+	mFixedAxis = 0;
+	for (int a = 0; a < EAxis::Num; ++a)
+	{
+		float limit = a >= EAxis::RotationX? JPH_PI : FLT_MAX;
+
+		if (mLimitMin[a] >= mLimitMax[a])
+			mFixedAxis |= 1 << a;
+		else if (mLimitMin[a] <= -limit && mLimitMax[a] >= limit)
+			mFreeAxis |= 1 << a;
+	}
+
+	// On change we deactivate all constraints to reset warm starting
+	if (old_free_axis != mFreeAxis || old_fixed_axis != mFixedAxis)
+	{
+		for (AxisConstraintPart &c : mTranslationConstraintPart)
+			c.Deactivate();
+		mPointConstraintPart.Deactivate();
+		mSwingTwistConstraintPart.Deactivate();
+		mRotationConstraintPart.Deactivate();
+		for (AxisConstraintPart &c : mMotorTranslationConstraintPart)
+			c.Deactivate();
+		for (AngleConstraintPart &c : mMotorRotationConstraintPart)
+			c.Deactivate();
+	}
+}
+
+SixDOFConstraint::SixDOFConstraint(Body &inBody1, Body &inBody2, const SixDOFConstraintSettings &inSettings) :
+	TwoBodyConstraint(inBody1, inBody2, inSettings)
+{
+	// 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);
+	Mat44 c_to_b1(Vec4(inSettings.mAxisX1, 0), Vec4(inSettings.mAxisY1, 0), Vec4(axis_z1, 0), Vec4(0, 0, 0, 1));
+	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 = c_to_b2.GetQuaternion();
+
+	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
+	{
+		// If all properties were specified in world space, take them to local space now
+		mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mPosition1);
+		mConstraintToBody1 = inBody1.GetRotation().Conjugated() * mConstraintToBody1;
+
+		mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mPosition2);
+		mConstraintToBody2 = inBody2.GetRotation().Conjugated() * mConstraintToBody2;
+	}
+	else
+	{
+		mLocalSpacePosition1 = Vec3(inSettings.mPosition1);
+		mLocalSpacePosition2 = Vec3(inSettings.mPosition2);
+	}
+
+	// Copy translation and rotation limits
+	memcpy(mLimitMin, inSettings.mLimitMin, sizeof(mLimitMin));
+	memcpy(mLimitMax, inSettings.mLimitMax, sizeof(mLimitMax));
+	memcpy(mLimitsSpringSettings, inSettings.mLimitsSpringSettings, sizeof(mLimitsSpringSettings));
+	UpdateTranslationLimits();
+	UpdateRotationLimits();
+	UpdateFixedFreeAxis();
+	CacheHasSpringLimits();
+
+	// Store friction settings
+	memcpy(mMaxFriction, inSettings.mMaxFriction, sizeof(mMaxFriction));
+
+	// Store motor settings
+	for (int i = 0; i < EAxis::Num; ++i)
+		mMotorSettings[i] = inSettings.mMotorSettings[i];
+
+	// Cache if motors are active (motors are off initially, but we may have friction)
+	CacheTranslationMotorActive();
+	CacheRotationMotorActive();
+}
+
+void SixDOFConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
+{
+	if (mBody1->GetID() == inBodyID)
+		mLocalSpacePosition1 -= inDeltaCOM;
+	else if (mBody2->GetID() == inBodyID)
+		mLocalSpacePosition2 -= inDeltaCOM;
+}
+
+void SixDOFConstraint::SetTranslationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax)
+{
+	mLimitMin[EAxis::TranslationX] = inLimitMin.GetX();
+	mLimitMin[EAxis::TranslationY] = inLimitMin.GetY();
+	mLimitMin[EAxis::TranslationZ] = inLimitMin.GetZ();
+	mLimitMax[EAxis::TranslationX] = inLimitMax.GetX();
+	mLimitMax[EAxis::TranslationY] = inLimitMax.GetY();
+	mLimitMax[EAxis::TranslationZ] = inLimitMax.GetZ();
+
+	UpdateTranslationLimits();
+	UpdateFixedFreeAxis();
+}
+
+void SixDOFConstraint::SetRotationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax)
+{
+	mLimitMin[EAxis::RotationX] = inLimitMin.GetX();
+	mLimitMin[EAxis::RotationY] = inLimitMin.GetY();
+	mLimitMin[EAxis::RotationZ] = inLimitMin.GetZ();
+	mLimitMax[EAxis::RotationX] = inLimitMax.GetX();
+	mLimitMax[EAxis::RotationY] = inLimitMax.GetY();
+	mLimitMax[EAxis::RotationZ] = inLimitMax.GetZ();
+
+	UpdateRotationLimits();
+	UpdateFixedFreeAxis();
+}
+
+void SixDOFConstraint::SetMaxFriction(EAxis inAxis, float inFriction)
+{
+	mMaxFriction[inAxis] = inFriction;
+
+	if (inAxis >= EAxis::TranslationX && inAxis <= EAxis::TranslationZ)
+		CacheTranslationMotorActive();
+	else
+		CacheRotationMotorActive();
+}
+
+void SixDOFConstraint::GetPositionConstraintProperties(Vec3 &outR1PlusU, Vec3 &outR2, Vec3 &outU) const
+{
+	RVec3 p1 = mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1;
+	RVec3 p2 = mBody2->GetCenterOfMassTransform() * mLocalSpacePosition2;
+	outR1PlusU = Vec3(p2 - mBody1->GetCenterOfMassPosition()); // r1 + u = (p1 - x1) + (p2 - p1) = p2 - x1
+	outR2 = Vec3(p2 - mBody2->GetCenterOfMassPosition());
+	outU = Vec3(p2 - p1);
+}
+
+Quat SixDOFConstraint::GetRotationInConstraintSpace() const
+{
+	// Let b1, b2 be the center of mass transform of body1 and body2 (For body1 this is mBody1->GetCenterOfMassTransform())
+	// Let c1, c2 be the transform that takes a vector from constraint space to local space of body1 and body2 (For body1 this is Mat44::sRotationTranslation(mConstraintToBody1, mLocalSpacePosition1))
+	// Let q be the rotation of the constraint in constraint space
+	// b2 takes a vector from the local space of body2 to world space
+	// To express this in terms of b1: b2 = b1 * c1 * q * c2^-1
+	// c2^-1 goes from local body 2 space to constraint space
+	// q rotates the constraint
+	// c1 goes from constraint space to body 1 local space
+	// b1 goes from body 1 local space to world space
+	// So when the body rotations are given, q = (b1 * c1)^-1 * b2 c2
+	// Or: q = (q1 * c1)^-1 * (q2 * c2) if we're only interested in rotations
+	return (mBody1->GetRotation() * mConstraintToBody1).Conjugated() * mBody2->GetRotation() * mConstraintToBody2;
+}
+
+void SixDOFConstraint::CacheTranslationMotorActive()
+{
+	mTranslationMotorActive = mMotorState[EAxis::TranslationX] != EMotorState::Off
+		|| mMotorState[EAxis::TranslationY] != EMotorState::Off
+		|| mMotorState[EAxis::TranslationZ] != EMotorState::Off
+		|| HasFriction(EAxis::TranslationX)
+		|| HasFriction(EAxis::TranslationY)
+		|| HasFriction(EAxis::TranslationZ);
+}
+
+void SixDOFConstraint::CacheRotationMotorActive()
+{
+	mRotationMotorActive = mMotorState[EAxis::RotationX] != EMotorState::Off
+		|| mMotorState[EAxis::RotationY] != EMotorState::Off
+		|| mMotorState[EAxis::RotationZ] != EMotorState::Off
+		|| HasFriction(EAxis::RotationX)
+		|| HasFriction(EAxis::RotationY)
+		|| HasFriction(EAxis::RotationZ);
+}
+
+void SixDOFConstraint::CacheRotationPositionMotorActive()
+{
+	mRotationPositionMotorActive = 0;
+	for (int i = 0; i < 3; ++i)
+		if (mMotorState[EAxis::RotationX + i] == EMotorState::Position)
+			mRotationPositionMotorActive |= 1 << i;
+}
+
+void SixDOFConstraint::CacheHasSpringLimits()
+{
+	mHasSpringLimits = mLimitsSpringSettings[EAxis::TranslationX].mFrequency > 0.0f
+		|| mLimitsSpringSettings[EAxis::TranslationY].mFrequency > 0.0f
+		|| mLimitsSpringSettings[EAxis::TranslationZ].mFrequency > 0.0f;
+}
+
+void SixDOFConstraint::SetMotorState(EAxis inAxis, EMotorState inState)
+{
+	JPH_ASSERT(inState == EMotorState::Off || mMotorSettings[inAxis].IsValid());
+
+	if (mMotorState[inAxis] != inState)
+	{
+		mMotorState[inAxis] = inState;
+
+		// Ensure that warm starting next frame doesn't apply any impulses (motor parts are repurposed for different modes)
+		if (inAxis >= EAxis::TranslationX && inAxis <= EAxis::TranslationZ)
+		{
+			mMotorTranslationConstraintPart[inAxis - EAxis::TranslationX].Deactivate();
+
+			CacheTranslationMotorActive();
+		}
+		else
+		{
+			JPH_ASSERT(inAxis >= EAxis::RotationX && inAxis <= EAxis::RotationZ);
+
+			mMotorRotationConstraintPart[inAxis - EAxis::RotationX].Deactivate();
+
+			CacheRotationMotorActive();
+			CacheRotationPositionMotorActive();
+		}
+	}
+}
+
+void SixDOFConstraint::SetTargetOrientationCS(QuatArg inOrientation)
+{
+	Quat q_swing, q_twist;
+	inOrientation.GetSwingTwist(q_swing, q_twist);
+
+	uint clamped_axis;
+	mSwingTwistConstraintPart.ClampSwingTwist(q_swing, q_twist, clamped_axis);
+
+	if (clamped_axis != 0)
+		mTargetOrientation = q_swing * q_twist;
+	else
+		mTargetOrientation = inOrientation;
+}
+
+void SixDOFConstraint::SetupVelocityConstraint(float inDeltaTime)
+{
+	// Get body rotations
+	Quat rotation1 = mBody1->GetRotation();
+	Quat rotation2 = mBody2->GetRotation();
+
+	// Quaternion that rotates from body1's constraint space to world space
+	Quat constraint_body1_to_world = rotation1 * mConstraintToBody1;
+
+	// Store world space axis of constraint space
+	Mat44 translation_axis_mat = Mat44::sRotation(constraint_body1_to_world);
+	for (int i = 0; i < 3; ++i)
+		mTranslationAxis[i] = translation_axis_mat.GetColumn3(i);
+
+	if (IsTranslationFullyConstrained())
+	{
+		// All translation locked: Setup point constraint
+		mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(rotation1), mLocalSpacePosition1, *mBody2, Mat44::sRotation(rotation2), mLocalSpacePosition2);
+	}
+	else if (IsTranslationConstrained() || mTranslationMotorActive)
+	{
+		// Update world space positions (the bodies may have moved)
+		Vec3 r1_plus_u, r2, u;
+		GetPositionConstraintProperties(r1_plus_u, r2, u);
+
+		// Setup axis constraint parts
+		for (int i = 0; i < 3; ++i)
+		{
+			EAxis axis = EAxis(EAxis::TranslationX + i);
+
+			Vec3 translation_axis = mTranslationAxis[i];
+
+			// Calculate displacement along this axis
+			float d = translation_axis.Dot(u);
+			mDisplacement[i] = d; // Store for SolveVelocityConstraint
+
+			// Setup limit constraint
+			bool constraint_active = false;
+			float constraint_value = 0.0f;
+			if (IsFixedAxis(axis))
+			{
+				// When constraint is fixed it is always active
+				constraint_value = d - mLimitMin[i];
+				constraint_active = true;
+			}
+			else if (!IsFreeAxis(axis))
+			{
+				// When constraint is limited, it is only active when outside of the allowed range
+				if (d <= mLimitMin[i])
+				{
+					constraint_value = d - mLimitMin[i];
+					constraint_active = true;
+				}
+				else if (d >= mLimitMax[i])
+				{
+					constraint_value = d - mLimitMax[i];
+					constraint_active = true;
+				}
+			}
+
+			if (constraint_active)
+				mTranslationConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, translation_axis, 0.0f, constraint_value, mLimitsSpringSettings[i]);
+			else
+				mTranslationConstraintPart[i].Deactivate();
+
+			// Setup motor constraint
+			switch (mMotorState[i])
+			{
+			case EMotorState::Off:
+				if (HasFriction(axis))
+					mMotorTranslationConstraintPart[i].CalculateConstraintProperties(*mBody1, r1_plus_u, *mBody2, r2, translation_axis);
+				else
+					mMotorTranslationConstraintPart[i].Deactivate();
+				break;
+
+			case EMotorState::Velocity:
+				mMotorTranslationConstraintPart[i].CalculateConstraintProperties(*mBody1, r1_plus_u, *mBody2, r2, translation_axis, -mTargetVelocity[i]);
+				break;
+
+			case EMotorState::Position:
+				{
+					const SpringSettings &spring_settings = mMotorSettings[i].mSpringSettings;
+					if (spring_settings.HasStiffness())
+						mMotorTranslationConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, translation_axis, 0.0f, translation_axis.Dot(u) - mTargetPosition[i], spring_settings);
+					else
+						mMotorTranslationConstraintPart[i].Deactivate();
+					break;
+				}
+			}
+		}
+	}
+
+	// Setup rotation constraints
+	if (IsRotationFullyConstrained())
+	{
+		// All rotation locked: Setup rotation constraint
+		mRotationConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(rotation1), *mBody2, Mat44::sRotation(rotation2));
+	}
+	else if (IsRotationConstrained() || mRotationMotorActive)
+	{
+		// GetRotationInConstraintSpace without redoing the calculation of constraint_body1_to_world
+		Quat constraint_body2_to_world = rotation2 * mConstraintToBody2;
+		Quat q = constraint_body1_to_world.Conjugated() * constraint_body2_to_world;
+
+		// Use swing twist constraint part
+		if (IsRotationConstrained())
+			mSwingTwistConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, q, constraint_body1_to_world);
+		else
+			mSwingTwistConstraintPart.Deactivate();
+
+		if (mRotationMotorActive)
+		{
+			// Calculate rotation motor axis
+			Mat44 ws_axis = Mat44::sRotation(constraint_body2_to_world);
+			for (int i = 0; i < 3; ++i)
+				mRotationAxis[i] = ws_axis.GetColumn3(i);
+
+			// Get target orientation along the shortest path from q
+			Quat target_orientation = q.Dot(mTargetOrientation) > 0.0f? mTargetOrientation : -mTargetOrientation;
+
+			// The definition of the constraint rotation q:
+			// R2 * ConstraintToBody2 = R1 * ConstraintToBody1 * q (1)
+			//
+			// R2' is the rotation of body 2 when reaching the target_orientation:
+			// R2' * ConstraintToBody2 = R1 * ConstraintToBody1 * target_orientation (2)
+			//
+			// The difference in body 2 space:
+			// R2' = R2 * diff_body2 (3)
+			//
+			// We want to specify the difference in the constraint space of body 2:
+			// diff_body2 = ConstraintToBody2 * diff * ConstraintToBody2^* (4)
+			//
+			// Extracting R2' from 2: R2' = R1 * ConstraintToBody1 * target_orientation * ConstraintToBody2^* (5)
+			// Combining 3 & 4: R2' = R2 * ConstraintToBody2 * diff * ConstraintToBody2^* (6)
+			// Combining 1 & 6: R2' = R1 * ConstraintToBody1 * q * diff * ConstraintToBody2^* (7)
+			// Combining 5 & 7: R1 * ConstraintToBody1 * target_orientation * ConstraintToBody2^* = R1 * ConstraintToBody1 * q * diff * ConstraintToBody2^*
+			// <=> target_orientation = q * diff
+			// <=> diff = q^* * target_orientation
+			Quat diff = q.Conjugated() * target_orientation;
+
+			// Project diff so that only rotation around axis that have a position motor are remaining
+			Quat projected_diff;
+			switch (mRotationPositionMotorActive)
+			{
+			case 0b001:
+				// Keep only rotation around X
+				projected_diff = diff.GetTwist(Vec3::sAxisX());
+				break;
+
+			case 0b010:
+				// Keep only rotation around Y
+				projected_diff = diff.GetTwist(Vec3::sAxisY());
+				break;
+
+			case 0b100:
+				// Keep only rotation around Z
+				projected_diff = diff.GetTwist(Vec3::sAxisZ());
+				break;
+
+			case 0b011:
+				// Remove rotation around Z
+				// q = swing_xy * twist_z <=> swing_xy = q * twist_z^*
+				projected_diff = diff * diff.GetTwist(Vec3::sAxisZ()).Conjugated();
+				break;
+
+			case 0b101:
+				// Remove rotation around Y
+				// q = swing_xz * twist_y <=> swing_xz = q * twist_y^*
+				projected_diff = diff * diff.GetTwist(Vec3::sAxisY()).Conjugated();
+				break;
+
+			case 0b110:
+				// Remove rotation around X
+				// q = swing_yz * twist_x <=> swing_yz = q * twist_x^*
+				projected_diff = diff * diff.GetTwist(Vec3::sAxisX()).Conjugated();
+				break;
+
+			case 0b111:
+			default: // All motors off is handled here but the results are unused
+				// Keep entire rotation
+				projected_diff = diff;
+				break;
+			}
+
+			// Approximate error angles
+			// The imaginary part of a quaternion is rotation_axis * sin(angle / 2)
+			// If angle is small, sin(x) = x so angle[i] ~ 2.0f * rotation_axis[i]
+			// We'll be making small time steps, so if the angle is not small at least the sign will be correct and we'll move in the right direction
+			Vec3 rotation_error = -2.0f * projected_diff.GetXYZ();
+
+			// Setup motors
+			for (int i = 0; i < 3; ++i)
+			{
+				EAxis axis = EAxis(EAxis::RotationX + i);
+
+				Vec3 rotation_axis = mRotationAxis[i];
+
+				switch (mMotorState[axis])
+				{
+				case EMotorState::Off:
+					if (HasFriction(axis))
+						mMotorRotationConstraintPart[i].CalculateConstraintProperties(*mBody1, *mBody2, rotation_axis);
+					else
+						mMotorRotationConstraintPart[i].Deactivate();
+					break;
+
+				case EMotorState::Velocity:
+					mMotorRotationConstraintPart[i].CalculateConstraintProperties(*mBody1, *mBody2, rotation_axis, -mTargetAngularVelocity[i]);
+					break;
+
+				case EMotorState::Position:
+					{
+						const SpringSettings &spring_settings = mMotorSettings[axis].mSpringSettings;
+						if (spring_settings.HasStiffness())
+							mMotorRotationConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, rotation_axis, 0.0f, rotation_error[i], spring_settings);
+						else
+							mMotorRotationConstraintPart[i].Deactivate();
+						break;
+					}
+				}
+			}
+		}
+	}
+}
+
+void SixDOFConstraint::ResetWarmStart()
+{
+	for (AxisConstraintPart &c : mMotorTranslationConstraintPart)
+		c.Deactivate();
+	for (AngleConstraintPart &c : mMotorRotationConstraintPart)
+		c.Deactivate();
+	mRotationConstraintPart.Deactivate();
+	mSwingTwistConstraintPart.Deactivate();
+	mPointConstraintPart.Deactivate();
+	for (AxisConstraintPart &c : mTranslationConstraintPart)
+		c.Deactivate();
+}
+
+void SixDOFConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
+{
+	// Warm start translation motors
+	if (mTranslationMotorActive)
+		for (int i = 0; i < 3; ++i)
+			if (mMotorTranslationConstraintPart[i].IsActive())
+				mMotorTranslationConstraintPart[i].WarmStart(*mBody1, *mBody2, mTranslationAxis[i], inWarmStartImpulseRatio);
+
+	// Warm start rotation motors
+	if (mRotationMotorActive)
+		for (AngleConstraintPart &c : mMotorRotationConstraintPart)
+			if (c.IsActive())
+				c.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+
+	// Warm start rotation constraints
+	if (IsRotationFullyConstrained())
+		mRotationConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+	else if (IsRotationConstrained())
+		mSwingTwistConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+
+	// Warm start translation constraints
+	if (IsTranslationFullyConstrained())
+		mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+	else if (IsTranslationConstrained())
+		for (int i = 0; i < 3; ++i)
+			if (mTranslationConstraintPart[i].IsActive())
+				mTranslationConstraintPart[i].WarmStart(*mBody1, *mBody2, mTranslationAxis[i], inWarmStartImpulseRatio);
+}
+
+bool SixDOFConstraint::SolveVelocityConstraint(float inDeltaTime)
+{
+	bool impulse = false;
+
+	// Solve translation motor
+	if (mTranslationMotorActive)
+		for (int i = 0; i < 3; ++i)
+			if (mMotorTranslationConstraintPart[i].IsActive())
+				switch (mMotorState[i])
+				{
+				case EMotorState::Off:
+				{
+					// Apply friction only
+					float max_lambda = mMaxFriction[i] * inDeltaTime;
+					impulse |= mMotorTranslationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mTranslationAxis[i], -max_lambda, max_lambda);
+					break;
+				}
+
+				case EMotorState::Velocity:
+				case EMotorState::Position:
+					// Drive motor
+					impulse |= mMotorTranslationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mTranslationAxis[i], inDeltaTime * mMotorSettings[i].mMinForceLimit, inDeltaTime * mMotorSettings[i].mMaxForceLimit);
+					break;
+				}
+
+	// Solve rotation motor
+	if (mRotationMotorActive)
+		for (int i = 0; i < 3; ++i)
+		{
+			EAxis axis = EAxis(EAxis::RotationX + i);
+			if (mMotorRotationConstraintPart[i].IsActive())
+				switch (mMotorState[axis])
+				{
+				case EMotorState::Off:
+				{
+					// Apply friction only
+					float max_lambda = mMaxFriction[axis] * inDeltaTime;
+					impulse |= mMotorRotationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mRotationAxis[i], -max_lambda, max_lambda);
+					break;
+				}
+
+				case EMotorState::Velocity:
+				case EMotorState::Position:
+					// Drive motor
+					impulse |= mMotorRotationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mRotationAxis[i], inDeltaTime * mMotorSettings[axis].mMinTorqueLimit, inDeltaTime * mMotorSettings[axis].mMaxTorqueLimit);
+					break;
+				}
+		}
+
+	// Solve rotation constraint
+	if (IsRotationFullyConstrained())
+		impulse |= mRotationConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+	else if (IsRotationConstrained())
+		impulse |= mSwingTwistConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+
+	// Solve position constraint
+	if (IsTranslationFullyConstrained())
+		impulse |= mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+	else if (IsTranslationConstrained())
+		for (int i = 0; i < 3; ++i)
+			if (mTranslationConstraintPart[i].IsActive())
+			{
+				// If the axis is not fixed it must be limited (or else the constraint would not be active)
+				// Calculate the min and max constraint force based on on which side we're limited
+				float limit_min = -FLT_MAX, limit_max = FLT_MAX;
+				if (!IsFixedAxis(EAxis(EAxis::TranslationX + i)))
+				{
+					JPH_ASSERT(!IsFreeAxis(EAxis(EAxis::TranslationX + i)));
+					if (mDisplacement[i] <= mLimitMin[i])
+						limit_min = 0;
+					else if (mDisplacement[i] >= mLimitMax[i])
+						limit_max = 0;
+				}
+
+				impulse |= mTranslationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mTranslationAxis[i], limit_min, limit_max);
+			}
+
+	return impulse;
+}
+
+bool SixDOFConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
+{
+	bool impulse = false;
+
+	if (IsRotationFullyConstrained())
+	{
+		// Rotation locked: Solve rotation constraint
+
+		// Inverse of initial rotation from body 1 to body 2 in body 1 space
+		// Definition of initial orientation r0: q2 = q1 r0
+		// Initial rotation (see: GetRotationInConstraintSpace): q2 = q1 c1 c2^-1
+		// So: r0^-1 = (c1 c2^-1)^-1 = c2 * c1^-1
+		Quat constraint_to_body1 = mConstraintToBody1 * Quat::sEulerAngles(GetRotationLimitsMin());
+		Quat inv_initial_orientation = mConstraintToBody2 * constraint_to_body1.Conjugated();
+
+		// Solve rotation violations
+		mRotationConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), *mBody2, Mat44::sRotation(mBody2->GetRotation()));
+		impulse |= mRotationConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inv_initial_orientation, inBaumgarte);
+	}
+	else if (IsRotationConstrained())
+	{
+		// Rotation partially constraint
+
+		// Solve rotation violations
+		Quat q = GetRotationInConstraintSpace();
+		impulse |= mSwingTwistConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, q, mConstraintToBody1, mConstraintToBody2, inBaumgarte);
+	}
+
+	// Solve position violations
+	if (IsTranslationFullyConstrained())
+	{
+		// Translation locked: Solve point constraint
+		Vec3 local_space_position1 = mLocalSpacePosition1 + mConstraintToBody1 * GetTranslationLimitsMin();
+		mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), local_space_position1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2);
+		impulse |= mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte);
+	}
+	else if (IsTranslationConstrained())
+	{
+		// Translation partially locked: Solve per axis
+		for (int i = 0; i < 3; ++i)
+			if (mLimitsSpringSettings[i].mFrequency <= 0.0f) // If not soft limit
+			{
+				// Update world space positions (the bodies may have moved)
+				Vec3 r1_plus_u, r2, u;
+				GetPositionConstraintProperties(r1_plus_u, r2, u);
+
+				// Quaternion that rotates from body1's constraint space to world space
+				Quat constraint_body1_to_world = mBody1->GetRotation() * mConstraintToBody1;
+
+				// Calculate axis
+				Vec3 translation_axis;
+				switch (i)
+				{
+				case 0:							translation_axis = constraint_body1_to_world.RotateAxisX(); break;
+				case 1:							translation_axis = constraint_body1_to_world.RotateAxisY(); break;
+				default:	JPH_ASSERT(i == 2); translation_axis = constraint_body1_to_world.RotateAxisZ(); break;
+				}
+
+				// Determine position error
+				float error = 0.0f;
+				EAxis axis(EAxis(EAxis::TranslationX + i));
+				if (IsFixedAxis(axis))
+					error = u.Dot(translation_axis) - mLimitMin[axis];
+				else if (!IsFreeAxis(axis))
+				{
+					float displacement = u.Dot(translation_axis);
+					if (displacement <= mLimitMin[axis])
+						error = displacement - mLimitMin[axis];
+					else if (displacement >= mLimitMax[axis])
+						error = displacement - mLimitMax[axis];
+				}
+
+				if (error != 0.0f)
+				{
+					// Setup axis constraint part and solve it
+					mTranslationConstraintPart[i].CalculateConstraintProperties(*mBody1, r1_plus_u, *mBody2, r2, translation_axis);
+					impulse |= mTranslationConstraintPart[i].SolvePositionConstraint(*mBody1, *mBody2, translation_axis, error, inBaumgarte);
+				}
+			}
+	}
+
+	return impulse;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void SixDOFConstraint::DrawConstraint(DebugRenderer *inRenderer) const
+{
+	// Get constraint properties in world space
+	RVec3 position1 = mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1;
+	Quat rotation1 = mBody1->GetRotation() * mConstraintToBody1;
+	Quat rotation2 = mBody2->GetRotation() * mConstraintToBody2;
+
+	// Draw constraint orientation
+	inRenderer->DrawCoordinateSystem(RMat44::sRotationTranslation(rotation1, position1), mDrawConstraintSize);
+
+	if ((IsRotationConstrained() || mRotationPositionMotorActive != 0) && !IsRotationFullyConstrained())
+	{
+		// Draw current swing and twist
+		Quat q = GetRotationInConstraintSpace();
+		Quat q_swing, q_twist;
+		q.GetSwingTwist(q_swing, q_twist);
+		inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * q_twist).RotateAxisY(), Color::sWhite);
+		inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * q_swing).RotateAxisX(), Color::sWhite);
+	}
+
+	// Draw target rotation
+	Quat m_swing, m_twist;
+	mTargetOrientation.GetSwingTwist(m_swing, m_twist);
+	if (mMotorState[EAxis::RotationX] == EMotorState::Position)
+		inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * m_twist).RotateAxisY(), Color::sYellow);
+	if (mMotorState[EAxis::RotationY] == EMotorState::Position || mMotorState[EAxis::RotationZ] == EMotorState::Position)
+		inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * m_swing).RotateAxisX(), Color::sYellow);
+
+	// Draw target angular velocity
+	Vec3 target_angular_velocity = Vec3::sZero();
+	for (int i = 0; i < 3; ++i)
+		if (mMotorState[EAxis::RotationX + i] == EMotorState::Velocity)
+			target_angular_velocity.SetComponent(i, mTargetAngularVelocity[i]);
+	if (target_angular_velocity != Vec3::sZero())
+		inRenderer->DrawArrow(position1, position1 + rotation2 * target_angular_velocity, Color::sRed, 0.1f);
+}
+
+void SixDOFConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const
+{
+	// Get matrix that transforms from constraint space to world space
+	RMat44 constraint_body1_to_world = RMat44::sRotationTranslation(mBody1->GetRotation() * mConstraintToBody1, mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1);
+
+	// Draw limits
+	if (mSwingTwistConstraintPart.GetSwingType() == ESwingType::Pyramid)
+		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
+
+void SixDOFConstraint::SaveState(StateRecorder &inStream) const
+{
+	TwoBodyConstraint::SaveState(inStream);
+
+	for (const AxisConstraintPart &c : mTranslationConstraintPart)
+		c.SaveState(inStream);
+	mPointConstraintPart.SaveState(inStream);
+	mSwingTwistConstraintPart.SaveState(inStream);
+	mRotationConstraintPart.SaveState(inStream);
+	for (const AxisConstraintPart &c : mMotorTranslationConstraintPart)
+		c.SaveState(inStream);
+	for (const AngleConstraintPart &c : mMotorRotationConstraintPart)
+		c.SaveState(inStream);
+
+	inStream.Write(mMotorState);
+	inStream.Write(mTargetVelocity);
+	inStream.Write(mTargetAngularVelocity);
+	inStream.Write(mTargetPosition);
+	inStream.Write(mTargetOrientation);
+}
+
+void SixDOFConstraint::RestoreState(StateRecorder &inStream)
+{
+	TwoBodyConstraint::RestoreState(inStream);
+
+	for (AxisConstraintPart &c : mTranslationConstraintPart)
+		c.RestoreState(inStream);
+	mPointConstraintPart.RestoreState(inStream);
+	mSwingTwistConstraintPart.RestoreState(inStream);
+	mRotationConstraintPart.RestoreState(inStream);
+	for (AxisConstraintPart &c : mMotorTranslationConstraintPart)
+		c.RestoreState(inStream);
+	for (AngleConstraintPart &c : mMotorRotationConstraintPart)
+		c.RestoreState(inStream);
+
+	inStream.Read(mMotorState);
+	inStream.Read(mTargetVelocity);
+	inStream.Read(mTargetAngularVelocity);
+	inStream.Read(mTargetPosition);
+	inStream.Read(mTargetOrientation);
+
+	CacheTranslationMotorActive();
+	CacheRotationMotorActive();
+	CacheRotationPositionMotorActive();
+}
+
+Ref<ConstraintSettings> SixDOFConstraint::GetConstraintSettings() const
+{
+	SixDOFConstraintSettings *settings = new SixDOFConstraintSettings;
+	ToConstraintSettings(*settings);
+	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
+	settings->mPosition1 = RVec3(mLocalSpacePosition1);
+	settings->mAxisX1 = mConstraintToBody1.RotateAxisX();
+	settings->mAxisY1 = mConstraintToBody1.RotateAxisY();
+	settings->mPosition2 = RVec3(mLocalSpacePosition2);
+	settings->mAxisX2 = mConstraintToBody2.RotateAxisX();
+	settings->mAxisY2 = mConstraintToBody2.RotateAxisY();
+	settings->mSwingType = mSwingTwistConstraintPart.GetSwingType();
+	memcpy(settings->mLimitMin, mLimitMin, sizeof(mLimitMin));
+	memcpy(settings->mLimitMax, mLimitMax, sizeof(mLimitMax));
+	memcpy(settings->mMaxFriction, mMaxFriction, sizeof(mMaxFriction));
+	for (int i = 0; i < EAxis::Num; ++i)
+		settings->mMotorSettings[i] = mMotorSettings[i];
+	return settings;
+}
+
+JPH_NAMESPACE_END