@bloomengine/engine 0.3.1

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

@@ -0,0 +1,289 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+#include <Jolt/Physics/Constraints/MotorSettings.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/PointConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/SwingTwistConstraintPart.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// 6 Degree Of Freedom Constraint setup structure. Allows control over each of the 6 degrees of freedom.
+class JPH_EXPORT SixDOFConstraintSettings final : public TwoBodyConstraintSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, SixDOFConstraintSettings)
+
+public:
+	/// Constraint is split up into translation/rotation around X, Y and Z axis.
+	enum EAxis
+	{
+		TranslationX,
+		TranslationY,
+		TranslationZ,
+
+		RotationX,
+		RotationY,
+		RotationZ,
+
+		Num,
+		NumTranslation = TranslationZ + 1,
+	};
+
+	// See: ConstraintSettings::SaveBinaryState
+	virtual void				SaveBinaryState(StreamOut &inStream) const override;
+
+	/// Create an instance of this constraint
+	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
+
+	/// 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)
+	RVec3						mPosition1 = RVec3::sZero();
+	Vec3						mAxisX1 = Vec3::sAxisX();
+	Vec3						mAxisY1 = Vec3::sAxisY();
+
+	/// Body 2 constraint reference frame (space determined by mSpace)
+	RVec3						mPosition2 = RVec3::sZero();
+	Vec3						mAxisX2 = Vec3::sAxisX();
+	Vec3						mAxisY2 = Vec3::sAxisY();
+
+	/// Friction settings.
+	/// For translation: Max friction force in N. 0 = no friction.
+	/// For rotation: Max friction torque in Nm. 0 = no friction.
+	float						mMaxFriction[EAxis::Num] = { 0, 0, 0, 0, 0, 0 };
+
+	/// The type of swing constraint that we want to use.
+	ESwingType					mSwingType = ESwingType::Cone;
+
+	/// Limits.
+	/// For translation: Min and max linear limits in m (0 is frame of body 1 and 2 coincide).
+	/// For rotation: Min and max angular limits in rad (0 is frame of body 1 and 2 coincide). See comments at Axis enum for limit ranges.
+	///
+	/// Remove degree of freedom by setting min = FLT_MAX and max = -FLT_MAX. The constraint will be driven to 0 for this axis.
+	///
+	/// Free movement over an axis is allowed when min = -FLT_MAX and max = FLT_MAX.
+	///
+	/// Rotation limit around X-Axis: When limited, should be \f$\in [-\pi, \pi]\f$. Can be asymmetric around zero.
+	///
+	/// Rotation limit around Y-Z Axis: Forms a pyramid or cone shaped limit:
+	/// * For pyramid, should be \f$\in [-\pi, \pi]\f$ and does not need to be symmetrical around zero.
+	/// * For cone should be \f$\in [0, \pi]\f$ and needs to be symmetrical around zero (min limit is assumed to be -max limit).
+	float						mLimitMin[EAxis::Num] = { -FLT_MAX, -FLT_MAX, -FLT_MAX, -FLT_MAX, -FLT_MAX, -FLT_MAX };
+	float						mLimitMax[EAxis::Num] = { FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX };
+
+	/// When enabled, this makes the limits soft. When the constraint exceeds the limits, a spring force will pull it back.
+	/// Only soft translation limits are supported, soft rotation limits are not currently supported.
+	SpringSettings				mLimitsSpringSettings[EAxis::NumTranslation];
+
+	/// Make axis free (unconstrained)
+	void						MakeFreeAxis(EAxis inAxis)									{ mLimitMin[inAxis] = -FLT_MAX; mLimitMax[inAxis] = FLT_MAX; }
+	bool						IsFreeAxis(EAxis inAxis) const								{ return mLimitMin[inAxis] == -FLT_MAX && mLimitMax[inAxis] == FLT_MAX; }
+
+	/// Make axis fixed (fixed at value 0)
+	void						MakeFixedAxis(EAxis inAxis)									{ mLimitMin[inAxis] = FLT_MAX; mLimitMax[inAxis] = -FLT_MAX; }
+	bool						IsFixedAxis(EAxis inAxis) const								{ return mLimitMin[inAxis] >= mLimitMax[inAxis]; }
+
+	/// Set a valid range for the constraint (if inMax < inMin, the axis will become fixed)
+	void						SetLimitedAxis(EAxis inAxis, float inMin, float inMax)		{ mLimitMin[inAxis] = inMin; mLimitMax[inAxis] = inMax; }
+
+	/// Motor settings for each axis
+	MotorSettings				mMotorSettings[EAxis::Num];
+
+protected:
+	// See: ConstraintSettings::RestoreBinaryState
+	virtual void				RestoreBinaryState(StreamIn &inStream) override;
+};
+
+/// 6 Degree Of Freedom Constraint. Allows control over each of the 6 degrees of freedom.
+class JPH_EXPORT SixDOFConstraint final : public TwoBodyConstraint
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Get Axis from settings class
+	using EAxis = SixDOFConstraintSettings::EAxis;
+
+	/// Construct six DOF constraint
+								SixDOFConstraint(Body &inBody1, Body &inBody2, const SixDOFConstraintSettings &inSettings);
+
+	/// Generic interface of a constraint
+	virtual EConstraintSubType	GetSubType() const override									{ return EConstraintSubType::SixDOF; }
+	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
+	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
+	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
+	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
+	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;
+#ifdef JPH_DEBUG_RENDERER
+	virtual void				DrawConstraint(DebugRenderer *inRenderer) const override;
+	virtual void				DrawConstraintLimits(DebugRenderer *inRenderer) const override;
+#endif // JPH_DEBUG_RENDERER
+	virtual void				SaveState(StateRecorder &inStream) const override;
+	virtual void				RestoreState(StateRecorder &inStream) override;
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
+
+	// See: TwoBodyConstraint
+	virtual Mat44				GetConstraintToBody1Matrix() const override					{ return Mat44::sRotationTranslation(mConstraintToBody1, mLocalSpacePosition1); }
+	virtual Mat44				GetConstraintToBody2Matrix() const override					{ return Mat44::sRotationTranslation(mConstraintToBody2, mLocalSpacePosition2); }
+
+	/// Update the translation limits for this constraint
+	void						SetTranslationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax);
+
+	/// Update the rotational limits for this constraint
+	void						SetRotationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax);
+
+	/// Get constraint Limits
+	float						GetLimitsMin(EAxis inAxis) const							{ return mLimitMin[inAxis]; }
+	float						GetLimitsMax(EAxis inAxis) const							{ return mLimitMax[inAxis]; }
+	Vec3						GetTranslationLimitsMin() const								{ return Vec3::sLoadFloat3Unsafe(*reinterpret_cast<const Float3 *>(&mLimitMin[EAxis::TranslationX])); }
+	Vec3						GetTranslationLimitsMax() const								{ return Vec3::sLoadFloat3Unsafe(*reinterpret_cast<const Float3 *>(&mLimitMax[EAxis::TranslationX])); }
+	Vec3						GetRotationLimitsMin() const								{ return Vec3::sLoadFloat3Unsafe(*reinterpret_cast<const Float3 *>(&mLimitMin[EAxis::RotationX])); }
+	Vec3						GetRotationLimitsMax() const								{ return Vec3::sLoadFloat3Unsafe(*reinterpret_cast<const Float3 *>(&mLimitMax[EAxis::RotationX])); }
+
+	/// Check which axis are fixed/free
+	inline bool					IsFixedAxis(EAxis inAxis) const								{ return (mFixedAxis & (1 << inAxis)) != 0; }
+	inline bool					IsFreeAxis(EAxis inAxis) const								{ return (mFreeAxis & (1 << inAxis)) != 0; }
+
+	/// Update the limits spring settings
+	const SpringSettings &		GetLimitsSpringSettings(EAxis inAxis) const					{ JPH_ASSERT(inAxis < EAxis::NumTranslation); return mLimitsSpringSettings[inAxis]; }
+	void						SetLimitsSpringSettings(EAxis inAxis, const SpringSettings& inLimitsSpringSettings) { JPH_ASSERT(inAxis < EAxis::NumTranslation); mLimitsSpringSettings[inAxis] = inLimitsSpringSettings; CacheHasSpringLimits(); }
+
+	/// Set the max friction for each axis
+	void						SetMaxFriction(EAxis inAxis, float inFriction);
+	float						GetMaxFriction(EAxis inAxis) const							{ return mMaxFriction[inAxis]; }
+
+	/// Get rotation of constraint in constraint space
+	Quat						GetRotationInConstraintSpace() const;
+
+	/// Motor settings
+	MotorSettings &				GetMotorSettings(EAxis inAxis)								{ return mMotorSettings[inAxis]; }
+	const MotorSettings &		GetMotorSettings(EAxis inAxis) const						{ return mMotorSettings[inAxis]; }
+
+	/// Motor controls.
+	/// Translation motors work in constraint space of body 1.
+	/// Rotation motors work in constraint space of body 2 (!).
+	void						SetMotorState(EAxis inAxis, EMotorState inState);
+	EMotorState					GetMotorState(EAxis inAxis) const							{ return mMotorState[inAxis]; }
+
+	/// Set the target velocity in body 1 constraint space
+	Vec3						GetTargetVelocityCS() const									{ return mTargetVelocity; }
+	void						SetTargetVelocityCS(Vec3Arg inVelocity)						{ mTargetVelocity = inVelocity; }
+
+	/// Set the target angular velocity in body 2 constraint space (!)
+	void						SetTargetAngularVelocityCS(Vec3Arg inAngularVelocity)		{ mTargetAngularVelocity = inAngularVelocity; }
+	Vec3						GetTargetAngularVelocityCS() const							{ return mTargetAngularVelocity; }
+
+	/// Set the target position in body 1 constraint space
+	Vec3						GetTargetPositionCS() const									{ return mTargetPosition; }
+	void						SetTargetPositionCS(Vec3Arg inPosition)						{ mTargetPosition = inPosition; }
+
+	/// Set the target orientation in body 1 constraint space
+	void						SetTargetOrientationCS(QuatArg inOrientation);
+	Quat						GetTargetOrientationCS() const								{ return mTargetOrientation; }
+
+	/// Set the target orientation in body space (R2 = R1 * inOrientation, where R1 and R2 are the world space rotations for body 1 and 2).
+	/// Solve: R2 * ConstraintToBody2 = R1 * ConstraintToBody1 * q (see SwingTwistConstraint::GetSwingTwist) and R2 = R1 * inOrientation for q.
+	void						SetTargetOrientationBS(QuatArg inOrientation)				{ SetTargetOrientationCS(mConstraintToBody1.Conjugated() * inOrientation * mConstraintToBody2); }
+
+	///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint)
+	inline Vec3					GetTotalLambdaPosition() const								{ return IsTranslationFullyConstrained()? mPointConstraintPart.GetTotalLambda() : Vec3(mTranslationConstraintPart[0].GetTotalLambda(), mTranslationConstraintPart[1].GetTotalLambda(), mTranslationConstraintPart[2].GetTotalLambda()); }
+	inline Vec3					GetTotalLambdaRotation() const								{ return IsRotationFullyConstrained()? mRotationConstraintPart.GetTotalLambda() : Vec3(mSwingTwistConstraintPart.GetTotalTwistLambda(), mSwingTwistConstraintPart.GetTotalSwingYLambda(), mSwingTwistConstraintPart.GetTotalSwingZLambda()); }
+	inline Vec3					GetTotalLambdaMotorTranslation() const						{ return Vec3(mMotorTranslationConstraintPart[0].GetTotalLambda(), mMotorTranslationConstraintPart[1].GetTotalLambda(), mMotorTranslationConstraintPart[2].GetTotalLambda()); }
+	inline Vec3					GetTotalLambdaMotorRotation() const							{ return Vec3(mMotorRotationConstraintPart[0].GetTotalLambda(), mMotorRotationConstraintPart[1].GetTotalLambda(), mMotorRotationConstraintPart[2].GetTotalLambda()); }
+
+private:
+	// Calculate properties needed for the position constraint
+	inline void					GetPositionConstraintProperties(Vec3 &outR1PlusU, Vec3 &outR2, Vec3 &outU) const;
+
+	// Sanitize the translation limits
+	inline void					UpdateTranslationLimits();
+
+	// Propagate the rotation limits to the constraint part
+	inline void					UpdateRotationLimits();
+
+	// Update the cached state of which axis are free and which ones are fixed
+	inline void					UpdateFixedFreeAxis();
+
+	// Cache the state of mTranslationMotorActive
+	void						CacheTranslationMotorActive();
+
+	// Cache the state of mRotationMotorActive
+	void						CacheRotationMotorActive();
+
+	// Cache the state of mRotationPositionMotorActive
+	void						CacheRotationPositionMotorActive();
+
+	/// Cache the state of mHasSpringLimits
+	void						CacheHasSpringLimits();
+
+	// Constraint settings helper functions
+	inline bool					IsTranslationConstrained() const							{ return (mFreeAxis & 0b111) != 0b111; }
+	inline bool					IsTranslationFullyConstrained() const						{ return (mFixedAxis & 0b111) == 0b111 && !mHasSpringLimits; }
+	inline bool					IsRotationConstrained() const								{ return (mFreeAxis & 0b111000) != 0b111000; }
+	inline bool					IsRotationFullyConstrained() const							{ return (mFixedAxis & 0b111000) == 0b111000; }
+	inline bool					HasFriction(EAxis inAxis) const								{ return !IsFixedAxis(inAxis) && mMaxFriction[inAxis] > 0.0f; }
+
+	// CONFIGURATION PROPERTIES FOLLOW
+
+	// Local space constraint positions
+	Vec3						mLocalSpacePosition1;
+	Vec3						mLocalSpacePosition2;
+
+	// Transforms from constraint space to body space
+	Quat						mConstraintToBody1;
+	Quat						mConstraintToBody2;
+
+	// Limits
+	uint8						mFreeAxis = 0;												// Bitmask of free axis (bit 0 = TranslationX)
+	uint8						mFixedAxis = 0;												// Bitmask of fixed axis (bit 0 = TranslationX)
+	bool						mTranslationMotorActive = false;							// If any of the translational frictions / motors are active
+	bool						mRotationMotorActive = false;								// If any of the rotational frictions / motors are active
+	uint8						mRotationPositionMotorActive = 0;							// Bitmask of axis that have position motor active (bit 0 = RotationX)
+	bool						mHasSpringLimits = false;									// If any of the limit springs have a non-zero frequency/stiffness
+	float						mLimitMin[EAxis::Num];
+	float						mLimitMax[EAxis::Num];
+	SpringSettings				mLimitsSpringSettings[EAxis::NumTranslation];
+
+	// Motor settings for each axis
+	MotorSettings				mMotorSettings[EAxis::Num];
+
+	// Friction settings for each axis
+	float						mMaxFriction[EAxis::Num];
+
+	// Motor controls
+	EMotorState					mMotorState[EAxis::Num] = { EMotorState::Off, EMotorState::Off, EMotorState::Off, EMotorState::Off, EMotorState::Off, EMotorState::Off };
+	Vec3						mTargetVelocity = Vec3::sZero();
+	Vec3						mTargetAngularVelocity = Vec3::sZero();
+	Vec3						mTargetPosition = Vec3::sZero();
+	Quat						mTargetOrientation = Quat::sIdentity();
+
+	// RUN TIME PROPERTIES FOLLOW
+
+	// Constraint space axis in world space
+	Vec3						mTranslationAxis[3];
+	Vec3						mRotationAxis[3];
+
+	// Translation displacement (valid when translation axis has a range limit)
+	float						mDisplacement[3];
+
+	// Individual constraint parts for translation, or a combined point constraint part if all axis are fixed
+	AxisConstraintPart			mTranslationConstraintPart[3];
+	PointConstraintPart			mPointConstraintPart;
+
+	// Individual constraint parts for rotation or a combined constraint part if rotation is fixed
+	SwingTwistConstraintPart	mSwingTwistConstraintPart;
+	RotationEulerConstraintPart	mRotationConstraintPart;
+
+	// Motor or friction constraints
+	AxisConstraintPart			mMotorTranslationConstraintPart[3];
+	AngleConstraintPart			mMotorRotationConstraintPart[3];
+};
+
+JPH_NAMESPACE_END