@bloomengine/engine 0.3.3 → 0.4.1

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

@@ -1,35 +0,0 @@
-// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
-// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
-// SPDX-License-Identifier: MIT
-
-#include <Jolt/Jolt.h>
-
-#include <Jolt/Physics/Constraints/SpringSettings.h>
-#include <Jolt/ObjectStream/TypeDeclarations.h>
-#include <Jolt/Core/StreamIn.h>
-#include <Jolt/Core/StreamOut.h>
-
-JPH_NAMESPACE_BEGIN
-
-JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SpringSettings)
-{
-	JPH_ADD_ENUM_ATTRIBUTE(SpringSettings, mMode)
-	JPH_ADD_ATTRIBUTE(SpringSettings, mFrequency)
-	JPH_ADD_ATTRIBUTE(SpringSettings, mDamping)
-}
-
-void SpringSettings::SaveBinaryState(StreamOut &inStream) const
-{
-	inStream.Write(mMode);
-	inStream.Write(mFrequency);
-	inStream.Write(mDamping);
-}
-
-void SpringSettings::RestoreBinaryState(StreamIn &inStream)
-{
-	inStream.Read(mMode);
-	inStream.Read(mFrequency);
-	inStream.Read(mDamping);
-}
-
-JPH_NAMESPACE_END

package/native/third_party/JoltPhysics/Jolt/Physics/Constraints/SpringSettings.h

@@ -1,70 +0,0 @@
-// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
-// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
-// SPDX-License-Identifier: MIT
-
-#pragma once
-
-#include <Jolt/ObjectStream/SerializableObject.h>
-
-JPH_NAMESPACE_BEGIN
-
-class StreamIn;
-class StreamOut;
-
-/// Enum used by constraints to specify how the spring is defined
-enum class ESpringMode : uint8
-{
-	FrequencyAndDamping,		///< Frequency and damping are specified
-	StiffnessAndDamping,		///< Stiffness and damping are specified
-};
-
-/// Settings for a linear or angular spring
-class JPH_EXPORT SpringSettings
-{
-	JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, SpringSettings)
-
-public:
-	/// Constructor
-								SpringSettings() = default;
-								SpringSettings(const SpringSettings &) = default;
-	SpringSettings &			operator = (const SpringSettings &) = default;
-								SpringSettings(ESpringMode inMode, float inFrequencyOrStiffness, float inDamping) : mMode(inMode), mFrequency(inFrequencyOrStiffness), mDamping(inDamping) { }
-
-	/// Saves the contents of the spring settings in binary form to inStream.
-	void						SaveBinaryState(StreamOut &inStream) const;
-
-	/// Restores contents from the binary stream inStream.
-	void						RestoreBinaryState(StreamIn &inStream);
-
-	/// Check if the spring has a valid frequency / stiffness, if not the spring will be hard
-	inline bool					HasStiffness() const							{ return mFrequency > 0.0f; }
-
-	/// Selects the way in which the spring is defined
-	/// If the mode is StiffnessAndDamping then mFrequency becomes the stiffness (k) and mDamping becomes the damping ratio (c) in the spring equation F = -k * x - c * v. Otherwise the properties are as documented.
-	ESpringMode					mMode = ESpringMode::FrequencyAndDamping;
-
-	union
-	{
-		/// Valid when mSpringMode = ESpringMode::FrequencyAndDamping.
-		/// If mFrequency > 0 the constraint will be soft and mFrequency specifies the oscillation frequency in Hz.
-		/// If mFrequency <= 0, mDamping is ignored and the constraint will have hard limits (as hard as the time step / the number of velocity / position solver steps allows).
-		float					mFrequency = 0.0f;
-
-		/// Valid when mSpringMode = ESpringMode::StiffnessAndDamping.
-		/// If mStiffness > 0 the constraint will be soft and mStiffness specifies the stiffness (k) in the spring equation F = -k * x - c * v for a linear or T = -k * theta - c * w for an angular spring.
-		/// If mStiffness <= 0, mDamping is ignored and the constraint will have hard limits (as hard as the time step / the number of velocity / position solver steps allows).
-		///
-		/// Note that stiffness values are large numbers. To calculate a ballpark value for the needed stiffness you can use:
-		/// force = stiffness * delta_spring_length = mass * gravity <=> stiffness = mass * gravity / delta_spring_length.
-		/// So if your object weighs 1500 kg and the spring compresses by 2 meters, you need a stiffness in the order of 1500 * 9.81 / 2 ~ 7500 N/m.
-		float					mStiffness;
-	};
-
-	/// When mSpringMode = ESpringMode::FrequencyAndDamping mDamping is the damping ratio (0 = no damping, 1 = critical damping).
-	/// When mSpringMode = ESpringMode::StiffnessAndDamping mDamping is the damping (c) in the spring equation F = -k * x - c * v for a linear or T = -k * theta - c * w for an angular spring.
-	/// Note that if you set mDamping = 0, you will not get an infinite oscillation. Because we integrate physics using an explicit Euler scheme, there is always energy loss.
-	/// This is done to keep the simulation from exploding, because with a damping of 0 and even the slightest rounding error, the oscillation could become bigger and bigger until the simulation explodes.
-	float						mDamping = 0.0f;
-};
-
-JPH_NAMESPACE_END

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

@@ -1,524 +0,0 @@
-// 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/SwingTwistConstraint.h>
-#include <Jolt/Physics/Body/Body.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(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)
-	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)
-	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mTwistMaxAngle)
-	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mMaxFrictionTorque)
-	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mSwingMotorSettings)
-	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mTwistMotorSettings)
-}
-
-void SwingTwistConstraintSettings::SaveBinaryState(StreamOut &inStream) const
-{
-	ConstraintSettings::SaveBinaryState(inStream);
-
-	inStream.Write(mSpace);
-	inStream.Write(mPosition1);
-	inStream.Write(mTwistAxis1);
-	inStream.Write(mPlaneAxis1);
-	inStream.Write(mPosition2);
-	inStream.Write(mTwistAxis2);
-	inStream.Write(mPlaneAxis2);
-	inStream.Write(mSwingType);
-	inStream.Write(mNormalHalfConeAngle);
-	inStream.Write(mPlaneHalfConeAngle);
-	inStream.Write(mTwistMinAngle);
-	inStream.Write(mTwistMaxAngle);
-	inStream.Write(mMaxFrictionTorque);
-	mSwingMotorSettings.SaveBinaryState(inStream);
-	mTwistMotorSettings.SaveBinaryState(inStream);
-}
-
-void SwingTwistConstraintSettings::RestoreBinaryState(StreamIn &inStream)
-{
-	ConstraintSettings::RestoreBinaryState(inStream);
-
-	inStream.Read(mSpace);
-	inStream.Read(mPosition1);
-	inStream.Read(mTwistAxis1);
-	inStream.Read(mPlaneAxis1);
-	inStream.Read(mPosition2);
-	inStream.Read(mTwistAxis2);
-	inStream.Read(mPlaneAxis2);
-	inStream.Read(mSwingType);
-	inStream.Read(mNormalHalfConeAngle);
-	inStream.Read(mPlaneHalfConeAngle);
-	inStream.Read(mTwistMinAngle);
-	inStream.Read(mTwistMaxAngle);
-	inStream.Read(mMaxFrictionTorque);
-	mSwingMotorSettings.RestoreBinaryState(inStream);
-	mTwistMotorSettings.RestoreBinaryState(inStream);
-}
-
-TwoBodyConstraint *SwingTwistConstraintSettings::Create(Body &inBody1, Body &inBody2) const
-{
-	return new SwingTwistConstraint(inBody1, inBody2, *this);
-}
-
-void SwingTwistConstraint::UpdateLimits()
-{
-	// Pass limits on to swing twist constraint part
-	mSwingTwistConstraintPart.SetLimits(mTwistMinAngle, mTwistMaxAngle, -mPlaneHalfConeAngle, mPlaneHalfConeAngle, -mNormalHalfConeAngle, mNormalHalfConeAngle);
-}
-
-SwingTwistConstraint::SwingTwistConstraint(Body &inBody1, Body &inBody2, const SwingTwistConstraintSettings &inSettings) :
-	TwoBodyConstraint(inBody1, inBody2, inSettings),
-	mNormalHalfConeAngle(inSettings.mNormalHalfConeAngle),
-	mPlaneHalfConeAngle(inSettings.mPlaneHalfConeAngle),
-	mTwistMinAngle(inSettings.mTwistMinAngle),
-	mTwistMaxAngle(inSettings.mTwistMaxAngle),
-	mMaxFrictionTorque(inSettings.mMaxFrictionTorque),
-	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));
-	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 = 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);
-	}
-
-	UpdateLimits();
-}
-
-void SwingTwistConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
-{
-	if (mBody1->GetID() == inBodyID)
-		mLocalSpacePosition1 -= inDeltaCOM;
-	else if (mBody2->GetID() == inBodyID)
-		mLocalSpacePosition2 -= inDeltaCOM;
-}
-
-Quat SwingTwistConstraint::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
-	Quat constraint_body1_to_world = mBody1->GetRotation() * mConstraintToBody1;
-	Quat constraint_body2_to_world = mBody2->GetRotation() * mConstraintToBody2;
-	return constraint_body1_to_world.Conjugated() * constraint_body2_to_world;
-}
-
-void SwingTwistConstraint::SetSwingMotorState(EMotorState inState)
-{
-	JPH_ASSERT(inState == EMotorState::Off || mSwingMotorSettings.IsValid());
-
-	if (mSwingMotorState != inState)
-	{
-		mSwingMotorState = inState;
-
-		// Ensure that warm starting next frame doesn't apply any impulses (motor parts are repurposed for different modes)
-		for (AngleConstraintPart &c : mMotorConstraintPart)
-			c.Deactivate();
-	}
-}
-
-void SwingTwistConstraint::SetTwistMotorState(EMotorState inState)
-{
-	JPH_ASSERT(inState == EMotorState::Off || mTwistMotorSettings.IsValid());
-
-	if (mTwistMotorState != inState)
-	{
-		mTwistMotorState = inState;
-
-		// Ensure that warm starting next frame doesn't apply any impulses (motor parts are repurposed for different modes)
-		mMotorConstraintPart[0].Deactivate();
-	}
-}
-
-void SwingTwistConstraint::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 SwingTwistConstraint::SetupVelocityConstraint(float inDeltaTime)
-{
-	// Setup point constraint
-	Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation());
-	Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
-	mPointConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, mLocalSpacePosition1, *mBody2, rotation2, mLocalSpacePosition2);
-
-	// GetRotationInConstraintSpace written out since we reuse the sub expressions
-	Quat constraint_body1_to_world = mBody1->GetRotation() * mConstraintToBody1;
-	Quat constraint_body2_to_world = mBody2->GetRotation() * mConstraintToBody2;
-	Quat q = constraint_body1_to_world.Conjugated() * constraint_body2_to_world;
-
-	// Calculate constraint properties for the swing twist limit
-	mSwingTwistConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, q, constraint_body1_to_world);
-
-	if (mSwingMotorState != EMotorState::Off || mTwistMotorState != EMotorState::Off || mMaxFrictionTorque > 0.0f)
-	{
-		// Calculate rotation motor axis
-		Mat44 ws_axis = Mat44::sRotation(constraint_body2_to_world);
-		for (int i = 0; i < 3; ++i)
-			mWorldSpaceMotorAxis[i] = ws_axis.GetColumn3(i);
-
-		Vec3 rotation_error;
-		if (mSwingMotorState == EMotorState::Position || mTwistMotorState == EMotorState::Position)
-		{
-			// 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;
-
-			// 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
-			rotation_error = -2.0f * diff.GetXYZ();
-		}
-
-		// Swing motor
-		switch (mSwingMotorState)
-		{
-		case EMotorState::Off:
-			if (mMaxFrictionTorque > 0.0f)
-			{
-				// Enable friction
-				for (int i = 1; i < 3; ++i)
-					mMotorConstraintPart[i].CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceMotorAxis[i], 0.0f);
-			}
-			else
-			{
-				// Disable friction
-				for (AngleConstraintPart &c : mMotorConstraintPart)
-					c.Deactivate();
-			}
-			break;
-
-		case EMotorState::Velocity:
-			// Use motor to create angular velocity around desired axis
-			for (int i = 1; i < 3; ++i)
-				mMotorConstraintPart[i].CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceMotorAxis[i], -mTargetAngularVelocity[i]);
-			break;
-
-		case EMotorState::Position:
-			// Use motor to drive rotation error to zero
-			if (mSwingMotorSettings.mSpringSettings.HasStiffness())
-			{
-				for (int i = 1; i < 3; ++i)
-					mMotorConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, mWorldSpaceMotorAxis[i], 0.0f, rotation_error[i], mSwingMotorSettings.mSpringSettings);
-			}
-			else
-			{
-				for (int i = 1; i < 3; ++i)
-					mMotorConstraintPart[i].Deactivate();
-			}
-			break;
-		}
-
-		// Twist motor
-		switch (mTwistMotorState)
-		{
-		case EMotorState::Off:
-			if (mMaxFrictionTorque > 0.0f)
-			{
-				// Enable friction
-				mMotorConstraintPart[0].CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceMotorAxis[0], 0.0f);
-			}
-			else
-			{
-				// Disable friction
-				mMotorConstraintPart[0].Deactivate();
-			}
-			break;
-
-		case EMotorState::Velocity:
-			// Use motor to create angular velocity around desired axis
-			mMotorConstraintPart[0].CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceMotorAxis[0], -mTargetAngularVelocity[0]);
-			break;
-
-		case EMotorState::Position:
-			// Use motor to drive rotation error to zero
-			if (mTwistMotorSettings.mSpringSettings.HasStiffness())
-				mMotorConstraintPart[0].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, mWorldSpaceMotorAxis[0], 0.0f, rotation_error[0], mTwistMotorSettings.mSpringSettings);
-			else
-				mMotorConstraintPart[0].Deactivate();
-			break;
-		}
-	}
-	else
-	{
-		// Disable rotation motor
-		for (AngleConstraintPart &c : mMotorConstraintPart)
-			c.Deactivate();
-	}
-}
-
-void SwingTwistConstraint::ResetWarmStart()
-{
-	for (AngleConstraintPart &c : mMotorConstraintPart)
-		c.Deactivate();
-	mSwingTwistConstraintPart.Deactivate();
-	mPointConstraintPart.Deactivate();
-}
-
-void SwingTwistConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
-{
-	// Warm starting: Apply previous frame impulse
-	for (AngleConstraintPart &c : mMotorConstraintPart)
-		c.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
-	mSwingTwistConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
-	mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
-}
-
-bool SwingTwistConstraint::SolveVelocityConstraint(float inDeltaTime)
-{
-	bool impulse = false;
-
-	// Solve twist rotation motor
-	if (mMotorConstraintPart[0].IsActive())
-	{
-		// Twist limits
-		float min_twist_limit, max_twist_limit;
-		if (mTwistMotorState == EMotorState::Off)
-		{
-			max_twist_limit = inDeltaTime * mMaxFrictionTorque;
-			min_twist_limit = -max_twist_limit;
-		}
-		else
-		{
-			min_twist_limit = inDeltaTime * mTwistMotorSettings.mMinTorqueLimit;
-			max_twist_limit = inDeltaTime * mTwistMotorSettings.mMaxTorqueLimit;
-		}
-
-		impulse |= mMotorConstraintPart[0].SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceMotorAxis[0], min_twist_limit, max_twist_limit);
-	}
-
-	// Solve swing rotation motor
-	if (mMotorConstraintPart[1].IsActive())
-	{
-		// Swing parts should turn on / off together
-		JPH_ASSERT(mMotorConstraintPart[2].IsActive());
-
-		// Swing limits
-		float min_swing_limit, max_swing_limit;
-		if (mSwingMotorState == EMotorState::Off)
-		{
-			max_swing_limit = inDeltaTime * mMaxFrictionTorque;
-			min_swing_limit = -max_swing_limit;
-		}
-		else
-		{
-			min_swing_limit = inDeltaTime * mSwingMotorSettings.mMinTorqueLimit;
-			max_swing_limit = inDeltaTime * mSwingMotorSettings.mMaxTorqueLimit;
-		}
-
-		for (int i = 1; i < 3; ++i)
-			impulse |= mMotorConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceMotorAxis[i], min_swing_limit, max_swing_limit);
-	}
-	else
-	{
-		// Swing parts should turn on / off together
-		JPH_ASSERT(!mMotorConstraintPart[2].IsActive());
-	}
-
-	// Solve rotation limits
-	impulse |= mSwingTwistConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
-
-	// Solve position constraint
-	impulse |= mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
-
-	return impulse;
-}
-
-bool SwingTwistConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
-{
-	bool impulse = false;
-
-	// Solve rotation violations
-	Quat q = GetRotationInConstraintSpace();
-	impulse |= mSwingTwistConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, q, mConstraintToBody1, mConstraintToBody2, inBaumgarte);
-
-	// Solve position violations
-	mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), mLocalSpacePosition1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2);
-	impulse |= mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte);
-
-	return impulse;
-}
-
-#ifdef JPH_DEBUG_RENDERER
-void SwingTwistConstraint::DrawConstraint(DebugRenderer *inRenderer) const
-{
-	// Get constraint properties in world space
-	RMat44 transform1 = mBody1->GetCenterOfMassTransform();
-	RVec3 position1 = transform1 * mLocalSpacePosition1;
-	Quat rotation1 = mBody1->GetRotation() * mConstraintToBody1;
-	Quat rotation2 = mBody2->GetRotation() * mConstraintToBody2;
-
-	// Draw constraint orientation
-	inRenderer->DrawCoordinateSystem(RMat44::sRotationTranslation(rotation1, position1), mDrawConstraintSize);
-
-	// 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);
-
-	if (mSwingMotorState == EMotorState::Velocity || mTwistMotorState == EMotorState::Velocity)
-	{
-		// Draw target angular velocity
-		inRenderer->DrawArrow(position1, position1 + rotation2 * mTargetAngularVelocity, Color::sRed, 0.1f);
-	}
-	if (mSwingMotorState == EMotorState::Position || mTwistMotorState == EMotorState::Position)
-	{
-		// Draw motor swing and twist
-		Quat swing, twist;
-		mTargetOrientation.GetSwingTwist(swing, twist);
-		inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * twist).RotateAxisY(), Color::sYellow);
-		inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * swing).RotateAxisX(), Color::sCyan);
-	}
-}
-
-void SwingTwistConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const
-{
-	// Get matrix that transforms from constraint space to world space
-	RMat44 constraint_to_world = RMat44::sRotationTranslation(mBody1->GetRotation() * mConstraintToBody1, mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1);
-
-	// Draw limits
-	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
-
-void SwingTwistConstraint::SaveState(StateRecorder &inStream) const
-{
-	TwoBodyConstraint::SaveState(inStream);
-
-	mPointConstraintPart.SaveState(inStream);
-	mSwingTwistConstraintPart.SaveState(inStream);
-	for (const AngleConstraintPart &c : mMotorConstraintPart)
-		c.SaveState(inStream);
-
-	inStream.Write(mSwingMotorState);
-	inStream.Write(mTwistMotorState);
-	inStream.Write(mTargetAngularVelocity);
-	inStream.Write(mTargetOrientation);
-}
-
-void SwingTwistConstraint::RestoreState(StateRecorder &inStream)
-{
-	TwoBodyConstraint::RestoreState(inStream);
-
-	mPointConstraintPart.RestoreState(inStream);
-	mSwingTwistConstraintPart.RestoreState(inStream);
-	for (AngleConstraintPart &c : mMotorConstraintPart)
-		c.RestoreState(inStream);
-
-	inStream.Read(mSwingMotorState);
-	inStream.Read(mTwistMotorState);
-	inStream.Read(mTargetAngularVelocity);
-	inStream.Read(mTargetOrientation);
-}
-
-Ref<ConstraintSettings> SwingTwistConstraint::GetConstraintSettings() const
-{
-	SwingTwistConstraintSettings *settings = new SwingTwistConstraintSettings;
-	ToConstraintSettings(*settings);
-	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
-	settings->mPosition1 = RVec3(mLocalSpacePosition1);
-	settings->mTwistAxis1 = mConstraintToBody1.RotateAxisX();
-	settings->mPlaneAxis1 = mConstraintToBody1.RotateAxisZ();
-	settings->mPosition2 = RVec3(mLocalSpacePosition2);
-	settings->mTwistAxis2 = mConstraintToBody2.RotateAxisX();
-	settings->mPlaneAxis2 = mConstraintToBody2.RotateAxisZ();
-	settings->mSwingType = mSwingTwistConstraintPart.GetSwingType();
-	settings->mNormalHalfConeAngle = mNormalHalfConeAngle;
-	settings->mPlaneHalfConeAngle = mPlaneHalfConeAngle;
-	settings->mTwistMinAngle = mTwistMinAngle;
-	settings->mTwistMaxAngle = mTwistMaxAngle;
-	settings->mMaxFrictionTorque = mMaxFrictionTorque;
-	settings->mSwingMotorSettings = mSwingMotorSettings;
-	settings->mTwistMotorSettings = mTwistMotorSettings;
-	return settings;
-}
-
-JPH_NAMESPACE_END