initial commit, 4.5 stable

thirdparty/jolt_physics/Jolt/Physics/Constraints/PulleyConstraint.h

@@ -0,0 +1,137 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/IndependentAxisConstraintPart.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Pulley constraint settings, used to create a pulley constraint.
+/// A pulley connects two bodies via two fixed world points to each other similar to a distance constraint.
+/// We define Length1 = |BodyPoint1 - FixedPoint1| where Body1 is a point on body 1 in world space and FixedPoint1 a fixed point in world space
+/// Length2 = |BodyPoint2 - FixedPoint2|
+/// The constraint keeps the two line segments constrained so that
+/// MinDistance <= Length1 + Ratio * Length2 <= MaxDistance
+class JPH_EXPORT PulleyConstraintSettings final : public TwoBodyConstraintSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PulleyConstraintSettings)
+
+public:
+	// 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, specified properties below should be in the specified space
+	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
+
+	/// Body 1 constraint attachment point (space determined by mSpace).
+	RVec3						mBodyPoint1 = RVec3::sZero();
+
+	/// Fixed world point to which body 1 is connected (always world space)
+	RVec3						mFixedPoint1 = RVec3::sZero();
+
+	/// Body 2 constraint attachment point (space determined by mSpace)
+	RVec3						mBodyPoint2 = RVec3::sZero();
+
+	/// Fixed world point to which body 2 is connected (always world space)
+	RVec3						mFixedPoint2 = RVec3::sZero();
+
+	/// Ratio between the two line segments (see formula above), can be used to create a block and tackle
+	float						mRatio = 1.0f;
+
+	/// The minimum length of the line segments (see formula above), use -1 to calculate the length based on the positions of the objects when the constraint is created.
+	float						mMinLength = 0.0f;
+
+	/// The maximum length of the line segments (see formula above), use -1 to calculate the length based on the positions of the objects when the constraint is created.
+	float						mMaxLength = -1.0f;
+
+protected:
+	// See: ConstraintSettings::RestoreBinaryState
+	virtual void				RestoreBinaryState(StreamIn &inStream) override;
+};
+
+/// A pulley constraint.
+class JPH_EXPORT PulleyConstraint final : public TwoBodyConstraint
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Construct distance constraint
+								PulleyConstraint(Body &inBody1, Body &inBody2, const PulleyConstraintSettings &inSettings);
+
+	// Generic interface of a constraint
+	virtual EConstraintSubType	GetSubType() const override									{ return EConstraintSubType::Pulley; }
+	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;
+#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::sTranslation(mLocalSpacePosition1); }
+	virtual Mat44				GetConstraintToBody2Matrix() const override					{ return Mat44::sTranslation(mLocalSpacePosition2); } // Note: Incorrect rotation as we don't track the original rotation difference, should not matter though as the constraint is not limiting rotation.
+
+	/// Update the minimum and maximum length for the constraint
+	void						SetLength(float inMinLength, float inMaxLength)				{ JPH_ASSERT(inMinLength >= 0.0f && inMinLength <= inMaxLength); mMinLength = inMinLength; mMaxLength = inMaxLength; }
+	float						GetMinLength() const										{ return mMinLength; }
+	float						GetMaxLength() const										{ return mMaxLength; }
+
+	/// Get the current length of both segments (multiplied by the ratio for segment 2)
+	float						GetCurrentLength() const									{ return Vec3(mWorldSpacePosition1 - mFixedPosition1).Length() + mRatio * Vec3(mWorldSpacePosition2 - mFixedPosition2).Length(); }
+
+	///@name Get Lagrange multiplier from last physics update (the linear impulse applied to satisfy the constraint)
+	inline float				GetTotalLambdaPosition() const								{ return mIndependentAxisConstraintPart.GetTotalLambda(); }
+
+private:
+	// Calculates world positions and normals and returns current length
+	float						CalculatePositionsNormalsAndLength();
+
+	// Internal helper function to calculate the values below
+	void						CalculateConstraintProperties();
+
+	// CONFIGURATION PROPERTIES FOLLOW
+
+	// Local space constraint positions on the bodies
+	Vec3						mLocalSpacePosition1;
+	Vec3						mLocalSpacePosition2;
+
+	// World space fixed positions
+	RVec3						mFixedPosition1;
+	RVec3						mFixedPosition2;
+
+	/// Ratio between the two line segments
+	float						mRatio;
+
+	// The minimum/maximum length of the line segments
+	float						mMinLength;
+	float						mMaxLength;
+
+	// RUN TIME PROPERTIES FOLLOW
+
+	// World space positions and normal
+	RVec3						mWorldSpacePosition1;
+	RVec3						mWorldSpacePosition2;
+	Vec3						mWorldSpaceNormal1;
+	Vec3						mWorldSpaceNormal2;
+
+	// Depending on if the length < min or length > max we can apply forces to prevent further violations
+	float						mMinLambda;
+	float						mMaxLambda;
+
+	// The constraint part
+	IndependentAxisConstraintPart mIndependentAxisConstraintPart;
+};
+
+JPH_NAMESPACE_END