@bloomengine/engine 0.3.1

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

@@ -0,0 +1,501 @@
+// 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/SliderConstraint.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
+
+using namespace literals;
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(SliderConstraintSettings)
+{
+	JPH_ADD_BASE_CLASS(SliderConstraintSettings, TwoBodyConstraintSettings)
+
+	JPH_ADD_ENUM_ATTRIBUTE(SliderConstraintSettings, mSpace)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mAutoDetectPoint)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mPoint1)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mSliderAxis1)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mNormalAxis1)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mPoint2)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mSliderAxis2)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mNormalAxis2)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mLimitsMin)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mLimitsMax)
+	JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(SliderConstraintSettings, mLimitsSpringSettings.mMode, "mSpringMode")
+	JPH_ADD_ATTRIBUTE_WITH_ALIAS(SliderConstraintSettings, mLimitsSpringSettings.mFrequency, "mFrequency") // Renaming attributes to stay compatible with old versions of the library
+	JPH_ADD_ATTRIBUTE_WITH_ALIAS(SliderConstraintSettings, mLimitsSpringSettings.mDamping, "mDamping")
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mMaxFrictionForce)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mMotorSettings)
+}
+
+void SliderConstraintSettings::SetSliderAxis(Vec3Arg inSliderAxis)
+{
+	JPH_ASSERT(mSpace == EConstraintSpace::WorldSpace);
+
+	mSliderAxis1 = mSliderAxis2 = inSliderAxis;
+	mNormalAxis1 = mNormalAxis2 = inSliderAxis.GetNormalizedPerpendicular();
+}
+
+void SliderConstraintSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	ConstraintSettings::SaveBinaryState(inStream);
+
+	inStream.Write(mSpace);
+	inStream.Write(mAutoDetectPoint);
+	inStream.Write(mPoint1);
+	inStream.Write(mSliderAxis1);
+	inStream.Write(mNormalAxis1);
+	inStream.Write(mPoint2);
+	inStream.Write(mSliderAxis2);
+	inStream.Write(mNormalAxis2);
+	inStream.Write(mLimitsMin);
+	inStream.Write(mLimitsMax);
+	inStream.Write(mMaxFrictionForce);
+	mLimitsSpringSettings.SaveBinaryState(inStream);
+	mMotorSettings.SaveBinaryState(inStream);
+}
+
+void SliderConstraintSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	ConstraintSettings::RestoreBinaryState(inStream);
+
+	inStream.Read(mSpace);
+	inStream.Read(mAutoDetectPoint);
+	inStream.Read(mPoint1);
+	inStream.Read(mSliderAxis1);
+	inStream.Read(mNormalAxis1);
+	inStream.Read(mPoint2);
+	inStream.Read(mSliderAxis2);
+	inStream.Read(mNormalAxis2);
+	inStream.Read(mLimitsMin);
+	inStream.Read(mLimitsMax);
+	inStream.Read(mMaxFrictionForce);
+	mLimitsSpringSettings.RestoreBinaryState(inStream);
+	mMotorSettings.RestoreBinaryState(inStream);
+}
+
+TwoBodyConstraint *SliderConstraintSettings::Create(Body &inBody1, Body &inBody2) const
+{
+	return new SliderConstraint(inBody1, inBody2, *this);
+}
+
+SliderConstraint::SliderConstraint(Body &inBody1, Body &inBody2, const SliderConstraintSettings &inSettings) :
+	TwoBodyConstraint(inBody1, inBody2, inSettings),
+	mMaxFrictionForce(inSettings.mMaxFrictionForce),
+	mMotorSettings(inSettings.mMotorSettings)
+{
+	// Store inverse of initial rotation from body 1 to body 2 in body 1 space
+	mInvInitialOrientation = RotationEulerConstraintPart::sGetInvInitialOrientationXY(inSettings.mSliderAxis1, inSettings.mNormalAxis1, inSettings.mSliderAxis2, inSettings.mNormalAxis2);
+
+	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
+	{
+		RMat44 inv_transform1 = inBody1.GetInverseCenterOfMassTransform();
+		RMat44 inv_transform2 = inBody2.GetInverseCenterOfMassTransform();
+
+		if (inSettings.mAutoDetectPoint)
+		{
+			// Determine anchor point: If any of the bodies can never be dynamic use the other body as anchor point
+			RVec3 anchor;
+			if (!inBody1.CanBeKinematicOrDynamic())
+				anchor = inBody2.GetCenterOfMassPosition();
+			else if (!inBody2.CanBeKinematicOrDynamic())
+				anchor = inBody1.GetCenterOfMassPosition();
+			else
+			{
+				// Otherwise use weighted anchor point towards the lightest body
+				Real inv_m1 = Real(inBody1.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked());
+				Real inv_m2 = Real(inBody2.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked());
+				Real total_inv_mass = inv_m1 + inv_m2;
+				if (total_inv_mass != 0.0_r)
+					anchor = (inv_m1 * inBody1.GetCenterOfMassPosition() + inv_m2 * inBody2.GetCenterOfMassPosition()) / total_inv_mass;
+				else
+					anchor = inBody1.GetCenterOfMassPosition();
+			}
+
+			// Store local positions
+			mLocalSpacePosition1 = Vec3(inv_transform1 * anchor);
+			mLocalSpacePosition2 = Vec3(inv_transform2 * anchor);
+		}
+		else
+		{
+			// Store local positions
+			mLocalSpacePosition1 = Vec3(inv_transform1 * inSettings.mPoint1);
+			mLocalSpacePosition2 = Vec3(inv_transform2 * inSettings.mPoint2);
+		}
+
+		// If all properties were specified in world space, take them to local space now
+		mLocalSpaceSliderAxis1 = inv_transform1.Multiply3x3(inSettings.mSliderAxis1).Normalized();
+		mLocalSpaceNormal1 = inv_transform1.Multiply3x3(inSettings.mNormalAxis1).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();
+	}
+	else
+	{
+		// Store local positions
+		mLocalSpacePosition1 = Vec3(inSettings.mPoint1);
+		mLocalSpacePosition2 = Vec3(inSettings.mPoint2);
+
+		// Store local space axis
+		mLocalSpaceSliderAxis1 = inSettings.mSliderAxis1;
+		mLocalSpaceNormal1 = inSettings.mNormalAxis1;
+	}
+
+	// Calculate 2nd local space normal
+	mLocalSpaceNormal2 = mLocalSpaceSliderAxis1.Cross(mLocalSpaceNormal1);
+
+	// Store limits
+	JPH_ASSERT(inSettings.mLimitsMin != inSettings.mLimitsMax || inSettings.mLimitsSpringSettings.mFrequency > 0.0f, "Better use a fixed constraint");
+	SetLimits(inSettings.mLimitsMin, inSettings.mLimitsMax);
+
+	// Store spring settings
+	SetLimitsSpringSettings(inSettings.mLimitsSpringSettings);
+}
+
+void SliderConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
+{
+	if (mBody1->GetID() == inBodyID)
+		mLocalSpacePosition1 -= inDeltaCOM;
+	else if (mBody2->GetID() == inBodyID)
+		mLocalSpacePosition2 -= inDeltaCOM;
+}
+
+float SliderConstraint::GetCurrentPosition() const
+{
+	// See: CalculateR1R2U and CalculateSlidingAxisAndPosition
+	Vec3 r1 = mBody1->GetRotation() * mLocalSpacePosition1;
+	Vec3 r2 = mBody2->GetRotation() * mLocalSpacePosition2;
+	Vec3 u = Vec3(mBody2->GetCenterOfMassPosition() - mBody1->GetCenterOfMassPosition()) + r2 - r1;
+	return u.Dot(mBody1->GetRotation() * mLocalSpaceSliderAxis1);
+}
+
+void SliderConstraint::SetLimits(float inLimitsMin, float inLimitsMax)
+{
+	JPH_ASSERT(inLimitsMin <= 0.0f);
+	JPH_ASSERT(inLimitsMax >= 0.0f);
+	mLimitsMin = inLimitsMin;
+	mLimitsMax = inLimitsMax;
+	mHasLimits = mLimitsMin != -FLT_MAX || mLimitsMax != FLT_MAX;
+}
+
+void SliderConstraint::CalculateR1R2U(Mat44Arg inRotation1, Mat44Arg inRotation2)
+{
+	// Calculate points relative to body
+	mR1 = inRotation1 * mLocalSpacePosition1;
+	mR2 = inRotation2 * mLocalSpacePosition2;
+
+	// Calculate X2 + R2 - X1 - R1
+	mU = Vec3(mBody2->GetCenterOfMassPosition() - mBody1->GetCenterOfMassPosition()) + mR2 - mR1;
+}
+
+void SliderConstraint::CalculatePositionConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2)
+{
+	// Calculate world space normals
+	mN1 = inRotation1 * mLocalSpaceNormal1;
+	mN2 = inRotation1 * mLocalSpaceNormal2;
+
+	mPositionConstraintPart.CalculateConstraintProperties(*mBody1, inRotation1, mR1 + mU, *mBody2, inRotation2, mR2, mN1, mN2);
+}
+
+void SliderConstraint::CalculateSlidingAxisAndPosition(Mat44Arg inRotation1)
+{
+	if (mHasLimits || mMotorState != EMotorState::Off || mMaxFrictionForce > 0.0f)
+	{
+		// Calculate world space slider axis
+		mWorldSpaceSliderAxis = inRotation1 * mLocalSpaceSliderAxis1;
+
+		// Calculate slide distance along axis
+		mD = mU.Dot(mWorldSpaceSliderAxis);
+	}
+}
+
+void SliderConstraint::CalculatePositionLimitsConstraintProperties(float inDeltaTime)
+{
+	// Check if distance is within limits
+	bool below_min = mD <= mLimitsMin;
+	if (mHasLimits && (below_min || mD >= mLimitsMax))
+		mPositionLimitsConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis, 0.0f, mD - (below_min? mLimitsMin : mLimitsMax), mLimitsSpringSettings);
+	else
+		mPositionLimitsConstraintPart.Deactivate();
+}
+
+void SliderConstraint::CalculateMotorConstraintProperties(float inDeltaTime)
+{
+	switch (mMotorState)
+	{
+	case EMotorState::Off:
+		if (mMaxFrictionForce > 0.0f)
+			mMotorConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis);
+		else
+			mMotorConstraintPart.Deactivate();
+		break;
+
+	case EMotorState::Velocity:
+		mMotorConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis, -mTargetVelocity);
+		break;
+
+	case EMotorState::Position:
+		if (mMotorSettings.mSpringSettings.HasStiffness())
+			mMotorConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis, 0.0f, mD - mTargetPosition, mMotorSettings.mSpringSettings);
+		else
+			mMotorConstraintPart.Deactivate();
+		break;
+	}
+}
+
+void SliderConstraint::SetupVelocityConstraint(float inDeltaTime)
+{
+	// Calculate constraint properties that are constant while bodies don't move
+	Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation());
+	Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
+	CalculateR1R2U(rotation1, rotation2);
+	CalculatePositionConstraintProperties(rotation1, rotation2);
+	mRotationConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, *mBody2, rotation2);
+	CalculateSlidingAxisAndPosition(rotation1);
+	CalculatePositionLimitsConstraintProperties(inDeltaTime);
+	CalculateMotorConstraintProperties(inDeltaTime);
+}
+
+void SliderConstraint::ResetWarmStart()
+{
+	mMotorConstraintPart.Deactivate();
+	mPositionConstraintPart.Deactivate();
+	mRotationConstraintPart.Deactivate();
+	mPositionLimitsConstraintPart.Deactivate();
+}
+
+void SliderConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
+{
+	// Warm starting: Apply previous frame impulse
+	mMotorConstraintPart.WarmStart(*mBody1, *mBody2, mWorldSpaceSliderAxis, inWarmStartImpulseRatio);
+	mPositionConstraintPart.WarmStart(*mBody1, *mBody2, mN1, mN2, inWarmStartImpulseRatio);
+	mRotationConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+	mPositionLimitsConstraintPart.WarmStart(*mBody1, *mBody2, mWorldSpaceSliderAxis, inWarmStartImpulseRatio);
+}
+
+bool SliderConstraint::SolveVelocityConstraint(float inDeltaTime)
+{
+	// Solve motor
+	bool motor = false;
+	if (mMotorConstraintPart.IsActive())
+	{
+		switch (mMotorState)
+		{
+		case EMotorState::Off:
+			{
+				float max_lambda = mMaxFrictionForce * inDeltaTime;
+				motor = mMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, -max_lambda, max_lambda);
+				break;
+			}
+
+		case EMotorState::Velocity:
+		case EMotorState::Position:
+			motor = mMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, inDeltaTime * mMotorSettings.mMinForceLimit, inDeltaTime * mMotorSettings.mMaxForceLimit);
+			break;
+		}
+	}
+
+	// Solve position constraint along 2 axis
+	bool pos = mPositionConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mN1, mN2);
+
+	// Solve rotation constraint
+	bool rot = mRotationConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+
+	// Solve limits along slider axis
+	bool limit = false;
+	if (mPositionLimitsConstraintPart.IsActive())
+	{
+		float min_lambda, max_lambda;
+		if (mLimitsMin == mLimitsMax)
+		{
+			min_lambda = -FLT_MAX;
+			max_lambda = FLT_MAX;
+		}
+		else if (mD <= mLimitsMin)
+		{
+			min_lambda = 0.0f;
+			max_lambda = FLT_MAX;
+		}
+		else
+		{
+			min_lambda = -FLT_MAX;
+			max_lambda = 0.0f;
+		}
+		limit = mPositionLimitsConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, min_lambda, max_lambda);
+	}
+
+	return motor || pos || rot || limit;
+}
+
+bool SliderConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
+{
+	// Motor operates on velocities only, don't call SolvePositionConstraint
+
+	// Solve position constraint along 2 axis
+	Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation());
+	Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
+	CalculateR1R2U(rotation1, rotation2);
+	CalculatePositionConstraintProperties(rotation1, rotation2);
+	bool pos = mPositionConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mU, mN1, mN2, inBaumgarte);
+
+	// Solve rotation constraint
+	mRotationConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), *mBody2, Mat44::sRotation(mBody2->GetRotation()));
+	bool rot = mRotationConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mInvInitialOrientation, inBaumgarte);
+
+	// Solve limits along slider axis
+	bool limit = false;
+	if (mHasLimits && mLimitsSpringSettings.mFrequency <= 0.0f)
+	{
+		rotation1 = Mat44::sRotation(mBody1->GetRotation());
+		rotation2 = Mat44::sRotation(mBody2->GetRotation());
+		CalculateR1R2U(rotation1, rotation2);
+		CalculateSlidingAxisAndPosition(rotation1);
+		CalculatePositionLimitsConstraintProperties(inDeltaTime);
+		if (mPositionLimitsConstraintPart.IsActive())
+		{
+			if (mD <= mLimitsMin)
+				limit = mPositionLimitsConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, mD - mLimitsMin, inBaumgarte);
+			else
+			{
+				JPH_ASSERT(mD >= mLimitsMax);
+				limit = mPositionLimitsConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, mD - mLimitsMax, inBaumgarte);
+			}
+		}
+	}
+
+	return pos || rot || limit;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void SliderConstraint::DrawConstraint(DebugRenderer *inRenderer) const
+{
+	RMat44 transform1 = mBody1->GetCenterOfMassTransform();
+	RMat44 transform2 = mBody2->GetCenterOfMassTransform();
+
+	// Transform the local positions into world space
+	Vec3 slider_axis = transform1.Multiply3x3(mLocalSpaceSliderAxis1);
+	RVec3 position1 = transform1 * mLocalSpacePosition1;
+	RVec3 position2 = transform2 * mLocalSpacePosition2;
+
+	// Draw constraint
+	inRenderer->DrawMarker(position1, Color::sRed, 0.1f);
+	inRenderer->DrawMarker(position2, Color::sGreen, 0.1f);
+	inRenderer->DrawLine(position1, position2, Color::sGreen);
+
+	// Draw motor
+	switch (mMotorState)
+	{
+	case EMotorState::Position:
+		inRenderer->DrawMarker(position1 + mTargetPosition * slider_axis, Color::sYellow, 1.0f);
+		break;
+
+	case EMotorState::Velocity:
+		{
+			Vec3 cur_vel = (mBody2->GetLinearVelocity() - mBody1->GetLinearVelocity()).Dot(slider_axis) * slider_axis;
+			inRenderer->DrawLine(position2, position2 + cur_vel, Color::sBlue);
+			inRenderer->DrawArrow(position2 + cur_vel, position2 + mTargetVelocity * slider_axis, Color::sRed, 0.1f);
+			break;
+		}
+
+	case EMotorState::Off:
+		break;
+	}
+}
+
+void SliderConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const
+{
+	if (mHasLimits)
+	{
+		RMat44 transform1 = mBody1->GetCenterOfMassTransform();
+		RMat44 transform2 = mBody2->GetCenterOfMassTransform();
+
+		// Transform the local positions into world space
+		Vec3 slider_axis = transform1.Multiply3x3(mLocalSpaceSliderAxis1);
+		RVec3 position1 = transform1 * mLocalSpacePosition1;
+		RVec3 position2 = transform2 * mLocalSpacePosition2;
+
+		// Calculate the limits in world space
+		RVec3 limits_min = position1 + mLimitsMin * slider_axis;
+		RVec3 limits_max = position1 + mLimitsMax * slider_axis;
+
+		inRenderer->DrawLine(limits_min, position1, Color::sWhite);
+		inRenderer->DrawLine(position2, limits_max, Color::sWhite);
+
+		inRenderer->DrawMarker(limits_min, Color::sWhite, 0.1f);
+		inRenderer->DrawMarker(limits_max, Color::sWhite, 0.1f);
+	}
+}
+#endif // JPH_DEBUG_RENDERER
+
+void SliderConstraint::SaveState(StateRecorder &inStream) const
+{
+	TwoBodyConstraint::SaveState(inStream);
+
+	mMotorConstraintPart.SaveState(inStream);
+	mPositionConstraintPart.SaveState(inStream);
+	mRotationConstraintPart.SaveState(inStream);
+	mPositionLimitsConstraintPart.SaveState(inStream);
+
+	inStream.Write(mMotorState);
+	inStream.Write(mTargetVelocity);
+	inStream.Write(mTargetPosition);
+}
+
+void SliderConstraint::RestoreState(StateRecorder &inStream)
+{
+	TwoBodyConstraint::RestoreState(inStream);
+
+	mMotorConstraintPart.RestoreState(inStream);
+	mPositionConstraintPart.RestoreState(inStream);
+	mRotationConstraintPart.RestoreState(inStream);
+	mPositionLimitsConstraintPart.RestoreState(inStream);
+
+	inStream.Read(mMotorState);
+	inStream.Read(mTargetVelocity);
+	inStream.Read(mTargetPosition);
+}
+
+Ref<ConstraintSettings> SliderConstraint::GetConstraintSettings() const
+{
+	SliderConstraintSettings *settings = new SliderConstraintSettings;
+	ToConstraintSettings(*settings);
+	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
+	settings->mPoint1 = RVec3(mLocalSpacePosition1);
+	settings->mSliderAxis1 = mLocalSpaceSliderAxis1;
+	settings->mNormalAxis1 = mLocalSpaceNormal1;
+	settings->mPoint2 = RVec3(mLocalSpacePosition2);
+	Mat44 inv_initial_rotation = Mat44::sRotation(mInvInitialOrientation);
+	settings->mSliderAxis2 = inv_initial_rotation.Multiply3x3(mLocalSpaceSliderAxis1);
+	settings->mNormalAxis2 = inv_initial_rotation.Multiply3x3(mLocalSpaceNormal1);
+	settings->mLimitsMin = mLimitsMin;
+	settings->mLimitsMax = mLimitsMax;
+	settings->mLimitsSpringSettings = mLimitsSpringSettings;
+	settings->mMaxFrictionForce = mMaxFrictionForce;
+	settings->mMotorSettings = mMotorSettings;
+	return settings;
+}
+
+Mat44 SliderConstraint::GetConstraintToBody1Matrix() const
+{
+	return Mat44(Vec4(mLocalSpaceSliderAxis1, 0), Vec4(mLocalSpaceNormal1, 0), Vec4(mLocalSpaceNormal2, 0), Vec4(mLocalSpacePosition1, 1));
+}
+
+Mat44 SliderConstraint::GetConstraintToBody2Matrix() const
+{
+	Mat44 mat = Mat44::sRotation(mInvInitialOrientation).Multiply3x3(Mat44(Vec4(mLocalSpaceSliderAxis1, 0), Vec4(mLocalSpaceNormal1, 0), Vec4(mLocalSpaceNormal2, 0), Vec4(0, 0, 0, 1)));
+	mat.SetTranslation(mLocalSpacePosition2);
+	return mat;
+}
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,198 @@
+// 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/DualAxisConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Slider constraint settings, used to create a slider constraint
+class JPH_EXPORT SliderConstraintSettings final : public TwoBodyConstraintSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, SliderConstraintSettings)
+
+public:
+	// See: ConstraintSettings::SaveBinaryState
+	virtual void				SaveBinaryState(StreamOut &inStream) const override;
+
+	/// Create an instance of this constraint.
+	/// Note that the rotation constraint will be solved from body 1. This means that if body 1 and body 2 have different masses / inertias (kinematic body = infinite mass / inertia), body 1 should be the heaviest body.
+	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
+
+	/// 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);
+
+	/// This determines in which space the constraint is setup, all properties below should be in the specified space
+	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
+
+	/// When mSpace is WorldSpace mPoint1 and mPoint2 can be automatically calculated based on the positions of the bodies when the constraint is created (the current relative position/orientation is chosen as the '0' position). Set this to false if you want to supply the attachment points yourself.
+	bool						mAutoDetectPoint = false;
+
+	/// 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.
+	RVec3						mPoint1 = RVec3::sZero();
+	Vec3						mSliderAxis1 = Vec3::sAxisX();
+	Vec3						mNormalAxis1 = Vec3::sAxisY();
+
+	/// Body 2 constraint reference frame (space determined by mSpace)
+	RVec3						mPoint2 = RVec3::sZero();
+	Vec3						mSliderAxis2 = Vec3::sAxisX();
+	Vec3						mNormalAxis2 = Vec3::sAxisY();
+
+	/// When the bodies move so that mPoint1 coincides with mPoint2 the slider position is defined to be 0, movement will be limited between [mLimitsMin, mLimitsMax] where mLimitsMin e [-inf, 0] and mLimitsMax e [0, inf]
+	float						mLimitsMin = -FLT_MAX;
+	float						mLimitsMax = FLT_MAX;
+
+	/// When enabled, this makes the limits soft. When the constraint exceeds the limits, a spring force will pull it back.
+	SpringSettings				mLimitsSpringSettings;
+
+	/// Maximum amount of friction force to apply (N) when not driven by a motor.
+	float						mMaxFrictionForce = 0.0f;
+
+	/// In case the constraint is powered, this determines the motor settings around the sliding axis
+	MotorSettings				mMotorSettings;
+
+protected:
+	// See: ConstraintSettings::RestoreBinaryState
+	virtual void				RestoreBinaryState(StreamIn &inStream) override;
+};
+
+/// A slider constraint allows movement in only 1 axis (and no rotation). Also known as a prismatic constraint.
+class JPH_EXPORT SliderConstraint final : public TwoBodyConstraint
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Construct slider constraint
+								SliderConstraint(Body &inBody1, Body &inBody2, const SliderConstraintSettings &inSettings);
+
+	// Generic interface of a constraint
+	virtual EConstraintSubType	GetSubType() const override								{ return EConstraintSubType::Slider; }
+	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;
+	virtual Mat44				GetConstraintToBody2Matrix() const override;
+
+	/// Get the current distance from the rest position
+	float						GetCurrentPosition() const;
+
+	/// Friction control
+	void						SetMaxFrictionForce(float inFrictionForce)				{ mMaxFrictionForce = inFrictionForce; }
+	float						GetMaxFrictionForce() const								{ return mMaxFrictionForce; }
+
+	/// Motor settings
+	MotorSettings &				GetMotorSettings()										{ return mMotorSettings; }
+	const MotorSettings &		GetMotorSettings() const								{ return mMotorSettings; }
+
+	// Motor controls
+	void						SetMotorState(EMotorState inState)						{ JPH_ASSERT(inState == EMotorState::Off || mMotorSettings.IsValid()); mMotorState = inState; }
+	EMotorState					GetMotorState() const									{ return mMotorState; }
+	void						SetTargetVelocity(float inVelocity)						{ mTargetVelocity = inVelocity; }
+	float						GetTargetVelocity() const								{ return mTargetVelocity; }
+	void						SetTargetPosition(float inPosition)						{ mTargetPosition = mHasLimits? Clamp(inPosition, mLimitsMin, mLimitsMax) : inPosition; }
+	float						GetTargetPosition() const								{ return mTargetPosition; }
+
+	/// Update the limits of the slider constraint (see SliderConstraintSettings)
+	void						SetLimits(float inLimitsMin, float inLimitsMax);
+	float						GetLimitsMin() const									{ return mLimitsMin; }
+	float						GetLimitsMax() const									{ return mLimitsMax; }
+	bool						HasLimits() const										{ return mHasLimits; }
+
+	/// Update the limits spring settings
+	const SpringSettings &		GetLimitsSpringSettings() const							{ return mLimitsSpringSettings; }
+	SpringSettings &			GetLimitsSpringSettings()								{ return mLimitsSpringSettings; }
+	void						SetLimitsSpringSettings(const SpringSettings &inLimitsSpringSettings) { mLimitsSpringSettings = inLimitsSpringSettings; }
+
+	///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint)
+	inline Vector<2>			GetTotalLambdaPosition() const							{ return mPositionConstraintPart.GetTotalLambda(); }
+	inline float				GetTotalLambdaPositionLimits() const					{ return mPositionLimitsConstraintPart.GetTotalLambda(); }
+	inline Vec3					GetTotalLambdaRotation() const							{ return mRotationConstraintPart.GetTotalLambda(); }
+	inline float				GetTotalLambdaMotor() const								{ return mMotorConstraintPart.GetTotalLambda(); }
+
+private:
+	// Internal helper function to calculate the values below
+	void						CalculateR1R2U(Mat44Arg inRotation1, Mat44Arg inRotation2);
+	void						CalculateSlidingAxisAndPosition(Mat44Arg inRotation1);
+	void						CalculatePositionConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2);
+	void						CalculatePositionLimitsConstraintProperties(float inDeltaTime);
+	void						CalculateMotorConstraintProperties(float inDeltaTime);
+
+	// CONFIGURATION PROPERTIES FOLLOW
+
+	// Local space constraint positions
+	Vec3						mLocalSpacePosition1;
+	Vec3						mLocalSpacePosition2;
+
+	// Local space sliding direction
+	Vec3						mLocalSpaceSliderAxis1;
+
+	// Local space normals to the sliding direction (in body 1 space)
+	Vec3						mLocalSpaceNormal1;
+	Vec3						mLocalSpaceNormal2;
+
+	// Inverse of initial rotation from body 1 to body 2 in body 1 space
+	Quat						mInvInitialOrientation;
+
+	// Slider limits
+	bool						mHasLimits;
+	float						mLimitsMin;
+	float						mLimitsMax;
+
+	// Soft constraint limits
+	SpringSettings				mLimitsSpringSettings;
+
+	// Friction
+	float						mMaxFrictionForce;
+
+	// Motor controls
+	MotorSettings				mMotorSettings;
+	EMotorState					mMotorState = EMotorState::Off;
+	float						mTargetVelocity = 0.0f;
+	float						mTargetPosition = 0.0f;
+
+	// RUN TIME PROPERTIES FOLLOW
+
+	// Positions where the point constraint acts on (middle point between center of masses)
+	Vec3						mR1;
+	Vec3						mR2;
+
+	// X2 + R2 - X1 - R1
+	Vec3						mU;
+
+	// World space sliding direction
+	Vec3						mWorldSpaceSliderAxis;
+
+	// Normals to the slider axis
+	Vec3						mN1;
+	Vec3						mN2;
+
+	// Distance along the slide axis
+	float						mD = 0.0f;
+
+	// The constraint parts
+	DualAxisConstraintPart		mPositionConstraintPart;
+	RotationEulerConstraintPart	mRotationConstraintPart;
+	AxisConstraintPart			mPositionLimitsConstraintPart;
+	AxisConstraintPart			mMotorConstraintPart;
+};
+
+JPH_NAMESPACE_END