@bloomengine/engine 0.3.1

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

@@ -0,0 +1,266 @@
+// 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/DistanceConstraint.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(DistanceConstraintSettings)
+{
+	JPH_ADD_BASE_CLASS(DistanceConstraintSettings, TwoBodyConstraintSettings)
+
+	JPH_ADD_ENUM_ATTRIBUTE(DistanceConstraintSettings, mSpace)
+	JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mPoint1)
+	JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mPoint2)
+	JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mMinDistance)
+	JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mMaxDistance)
+	JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(DistanceConstraintSettings, mLimitsSpringSettings.mMode, "mSpringMode")
+	JPH_ADD_ATTRIBUTE_WITH_ALIAS(DistanceConstraintSettings, mLimitsSpringSettings.mFrequency, "mFrequency") // Renaming attributes to stay compatible with old versions of the library
+	JPH_ADD_ATTRIBUTE_WITH_ALIAS(DistanceConstraintSettings, mLimitsSpringSettings.mDamping, "mDamping")
+}
+
+void DistanceConstraintSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	ConstraintSettings::SaveBinaryState(inStream);
+
+	inStream.Write(mSpace);
+	inStream.Write(mPoint1);
+	inStream.Write(mPoint2);
+	inStream.Write(mMinDistance);
+	inStream.Write(mMaxDistance);
+	mLimitsSpringSettings.SaveBinaryState(inStream);
+}
+
+void DistanceConstraintSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	ConstraintSettings::RestoreBinaryState(inStream);
+
+	inStream.Read(mSpace);
+	inStream.Read(mPoint1);
+	inStream.Read(mPoint2);
+	inStream.Read(mMinDistance);
+	inStream.Read(mMaxDistance);
+	mLimitsSpringSettings.RestoreBinaryState(inStream);
+}
+
+TwoBodyConstraint *DistanceConstraintSettings::Create(Body &inBody1, Body &inBody2) const
+{
+	return new DistanceConstraint(inBody1, inBody2, *this);
+}
+
+DistanceConstraint::DistanceConstraint(Body &inBody1, Body &inBody2, const DistanceConstraintSettings &inSettings) :
+	TwoBodyConstraint(inBody1, inBody2, inSettings),
+	mMinDistance(inSettings.mMinDistance),
+	mMaxDistance(inSettings.mMaxDistance)
+{
+	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
+	{
+		// If all properties were specified in world space, take them to local space now
+		mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mPoint1);
+		mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mPoint2);
+		mWorldSpacePosition1 = inSettings.mPoint1;
+		mWorldSpacePosition2 = inSettings.mPoint2;
+	}
+	else
+	{
+		// If properties were specified in local space, we need to calculate world space positions
+		mLocalSpacePosition1 = Vec3(inSettings.mPoint1);
+		mLocalSpacePosition2 = Vec3(inSettings.mPoint2);
+		mWorldSpacePosition1 = inBody1.GetCenterOfMassTransform() * inSettings.mPoint1;
+		mWorldSpacePosition2 = inBody2.GetCenterOfMassTransform() * inSettings.mPoint2;
+	}
+
+	// Store distance we want to keep between the world space points
+	float distance = Vec3(mWorldSpacePosition2 - mWorldSpacePosition1).Length();
+	float min_distance, max_distance;
+	if (mMinDistance < 0.0f && mMaxDistance < 0.0f)
+	{
+		min_distance = max_distance = distance;
+	}
+	else
+	{
+		min_distance = mMinDistance < 0.0f? min(distance, mMaxDistance) : mMinDistance;
+		max_distance = mMaxDistance < 0.0f? max(distance, mMinDistance) : mMaxDistance;
+	}
+	SetDistance(min_distance, max_distance);
+
+	// Most likely gravity is going to tear us apart (this is only used when the distance between the points = 0)
+	mWorldSpaceNormal = Vec3::sAxisY();
+
+	// Store spring settings
+	SetLimitsSpringSettings(inSettings.mLimitsSpringSettings);
+}
+
+void DistanceConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
+{
+	if (mBody1->GetID() == inBodyID)
+		mLocalSpacePosition1 -= inDeltaCOM;
+	else if (mBody2->GetID() == inBodyID)
+		mLocalSpacePosition2 -= inDeltaCOM;
+}
+
+void DistanceConstraint::CalculateConstraintProperties(float inDeltaTime)
+{
+	// Update world space positions (the bodies may have moved)
+	mWorldSpacePosition1 = mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1;
+	mWorldSpacePosition2 = mBody2->GetCenterOfMassTransform() * mLocalSpacePosition2;
+
+	// Calculate world space normal
+	Vec3 delta = Vec3(mWorldSpacePosition2 - mWorldSpacePosition1);
+	float delta_len = delta.Length();
+	if (delta_len > 0.0f)
+		mWorldSpaceNormal = delta / delta_len;
+
+	// Calculate points relative to body
+	// r1 + u = (p1 - x1) + (p2 - p1) = p2 - x1
+	Vec3 r1_plus_u = Vec3(mWorldSpacePosition2 - mBody1->GetCenterOfMassPosition());
+	Vec3 r2 = Vec3(mWorldSpacePosition2 - mBody2->GetCenterOfMassPosition());
+
+	if (mMinDistance == mMaxDistance)
+	{
+		mAxisConstraint.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, mWorldSpaceNormal, 0.0f, delta_len - mMinDistance, mLimitsSpringSettings);
+
+		// Single distance, allow constraint forces in both directions
+		mMinLambda = -FLT_MAX;
+		mMaxLambda = FLT_MAX;
+	}
+	else if (delta_len <= mMinDistance)
+	{
+		mAxisConstraint.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, mWorldSpaceNormal, 0.0f, delta_len - mMinDistance, mLimitsSpringSettings);
+
+		// Allow constraint forces to make distance bigger only
+		mMinLambda = 0;
+		mMaxLambda = FLT_MAX;
+	}
+	else if (delta_len >= mMaxDistance)
+	{
+		mAxisConstraint.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, mWorldSpaceNormal, 0.0f, delta_len - mMaxDistance, mLimitsSpringSettings);
+
+		// Allow constraint forces to make distance smaller only
+		mMinLambda = -FLT_MAX;
+		mMaxLambda = 0;
+	}
+	else
+		mAxisConstraint.Deactivate();
+}
+
+void DistanceConstraint::SetupVelocityConstraint(float inDeltaTime)
+{
+	CalculateConstraintProperties(inDeltaTime);
+}
+
+void DistanceConstraint::ResetWarmStart()
+{
+	mAxisConstraint.Deactivate();
+}
+
+void DistanceConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
+{
+	mAxisConstraint.WarmStart(*mBody1, *mBody2, mWorldSpaceNormal, inWarmStartImpulseRatio);
+}
+
+bool DistanceConstraint::SolveVelocityConstraint(float inDeltaTime)
+{
+	if (mAxisConstraint.IsActive())
+		return mAxisConstraint.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceNormal, mMinLambda, mMaxLambda);
+	else
+		return false;
+}
+
+bool DistanceConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
+{
+	if (mLimitsSpringSettings.mFrequency <= 0.0f) // When the spring is active, we don't need to solve the position constraint
+	{
+		float distance = Vec3(mWorldSpacePosition2 - mWorldSpacePosition1).Dot(mWorldSpaceNormal);
+
+		// Calculate position error
+		float position_error = 0.0f;
+		if (distance < mMinDistance)
+			position_error = distance - mMinDistance;
+		else if (distance > mMaxDistance)
+			position_error = distance - mMaxDistance;
+
+		if (position_error != 0.0f)
+		{
+			// Update constraint properties (bodies may have moved)
+			CalculateConstraintProperties(inDeltaTime);
+
+			return mAxisConstraint.SolvePositionConstraint(*mBody1, *mBody2, mWorldSpaceNormal, position_error, inBaumgarte);
+		}
+	}
+
+	return false;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void DistanceConstraint::DrawConstraint(DebugRenderer *inRenderer) const
+{
+	// Draw constraint
+	Vec3 delta = Vec3(mWorldSpacePosition2 - mWorldSpacePosition1);
+	float len = delta.Length();
+	if (len < mMinDistance)
+	{
+		RVec3 real_end_pos = mWorldSpacePosition1 + (len > 0.0f? delta * mMinDistance / len : Vec3(0, len, 0));
+		inRenderer->DrawLine(mWorldSpacePosition1, mWorldSpacePosition2, Color::sGreen);
+		inRenderer->DrawLine(mWorldSpacePosition2, real_end_pos, Color::sYellow);
+	}
+	else if (len > mMaxDistance)
+	{
+		RVec3 real_end_pos = mWorldSpacePosition1 + (len > 0.0f? delta * mMaxDistance / len : Vec3(0, len, 0));
+		inRenderer->DrawLine(mWorldSpacePosition1, real_end_pos, Color::sGreen);
+		inRenderer->DrawLine(real_end_pos, mWorldSpacePosition2, Color::sRed);
+	}
+	else
+		inRenderer->DrawLine(mWorldSpacePosition1, mWorldSpacePosition2, Color::sGreen);
+
+	// Draw constraint end points
+	inRenderer->DrawMarker(mWorldSpacePosition1, Color::sWhite, 0.1f);
+	inRenderer->DrawMarker(mWorldSpacePosition2, Color::sWhite, 0.1f);
+
+	// Draw current length
+	inRenderer->DrawText3D(0.5_r * (mWorldSpacePosition1 + mWorldSpacePosition2), StringFormat("%.2f", (double)len));
+}
+#endif // JPH_DEBUG_RENDERER
+
+void DistanceConstraint::SaveState(StateRecorder &inStream) const
+{
+	TwoBodyConstraint::SaveState(inStream);
+
+	mAxisConstraint.SaveState(inStream);
+	inStream.Write(mWorldSpaceNormal); // When distance = 0, the normal is used from last frame so we need to store it
+}
+
+void DistanceConstraint::RestoreState(StateRecorder &inStream)
+{
+	TwoBodyConstraint::RestoreState(inStream);
+
+	mAxisConstraint.RestoreState(inStream);
+	inStream.Read(mWorldSpaceNormal);
+}
+
+Ref<ConstraintSettings> DistanceConstraint::GetConstraintSettings() const
+{
+	DistanceConstraintSettings *settings = new DistanceConstraintSettings;
+	ToConstraintSettings(*settings);
+	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
+	settings->mPoint1 = RVec3(mLocalSpacePosition1);
+	settings->mPoint2 = RVec3(mLocalSpacePosition2);
+	settings->mMinDistance = mMinDistance;
+	settings->mMaxDistance = mMaxDistance;
+	settings->mLimitsSpringSettings = mLimitsSpringSettings;
+	return settings;
+}
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,120 @@
+// 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/ConstraintPart/AxisConstraintPart.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Distance constraint settings, used to create a distance constraint
+class JPH_EXPORT DistanceConstraintSettings final : public TwoBodyConstraintSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, DistanceConstraintSettings)
+
+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, all properties below should be in the specified space
+	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
+
+	/// Body 1 constraint reference frame (space determined by mSpace).
+	/// Constraint will keep mPoint1 (a point on body 1) and mPoint2 (a point on body 2) at the same distance.
+	/// Note that this constraint can be used as a cheap PointConstraint by setting mPoint1 = mPoint2 (but this removes only 1 degree of freedom instead of 3).
+	RVec3						mPoint1 = RVec3::sZero();
+
+	/// Body 2 constraint reference frame (space determined by mSpace)
+	RVec3						mPoint2 = RVec3::sZero();
+
+	/// Ability to override the distance range at which the two points are kept apart. If the value is negative, it will be replaced by the distance between mPoint1 and mPoint2 (works only if mSpace is world space).
+	float						mMinDistance = -1.0f;
+	float						mMaxDistance = -1.0f;
+
+	/// When enabled, this makes the limits soft. When the constraint exceeds the limits, a spring force will pull it back.
+	SpringSettings				mLimitsSpringSettings;
+
+protected:
+	// See: ConstraintSettings::RestoreBinaryState
+	virtual void				RestoreBinaryState(StreamIn &inStream) override;
+};
+
+/// This constraint is a stiff spring that holds 2 points at a fixed distance from each other
+class JPH_EXPORT DistanceConstraint final : public TwoBodyConstraint
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Construct distance constraint
+								DistanceConstraint(Body &inBody1, Body &inBody2, const DistanceConstraintSettings &inSettings);
+
+	// Generic interface of a constraint
+	virtual EConstraintSubType	GetSubType() const override									{ return EConstraintSubType::Distance; }
+	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 distance for the constraint
+	void						SetDistance(float inMinDistance, float inMaxDistance)		{ JPH_ASSERT(inMinDistance <= inMaxDistance); mMinDistance = inMinDistance; mMaxDistance = inMaxDistance; }
+	float						GetMinDistance() const										{ return mMinDistance; }
+	float						GetMaxDistance() const										{ return mMaxDistance; }
+
+	/// 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 impulse applied to satisfy the constraint)
+	inline float				GetTotalLambdaPosition() const								{ return mAxisConstraint.GetTotalLambda(); }
+
+private:
+	// Internal helper function to calculate the values below
+	void						CalculateConstraintProperties(float inDeltaTime);
+
+	// CONFIGURATION PROPERTIES FOLLOW
+
+	// Local space constraint positions
+	Vec3						mLocalSpacePosition1;
+	Vec3						mLocalSpacePosition2;
+
+	// Min/max distance that must be kept between the world space points
+	float						mMinDistance;
+	float						mMaxDistance;
+
+	// Soft constraint limits
+	SpringSettings				mLimitsSpringSettings;
+
+	// RUN TIME PROPERTIES FOLLOW
+
+	// World space positions and normal
+	RVec3						mWorldSpacePosition1;
+	RVec3						mWorldSpacePosition2;
+	Vec3						mWorldSpaceNormal;
+
+	// Depending on if the distance < min or distance > max we can apply forces to prevent further violations
+	float						mMinLambda;
+	float						mMaxLambda;
+
+	// The constraint part
+	AxisConstraintPart			mAxisConstraint;
+};
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,215 @@
+// 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/FixedConstraint.h>
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+using namespace literals;
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(FixedConstraintSettings)
+{
+	JPH_ADD_BASE_CLASS(FixedConstraintSettings, TwoBodyConstraintSettings)
+
+	JPH_ADD_ENUM_ATTRIBUTE(FixedConstraintSettings, mSpace)
+	JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mAutoDetectPoint)
+	JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mPoint1)
+	JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mAxisX1)
+	JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mAxisY1)
+	JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mPoint2)
+	JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mAxisX2)
+	JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mAxisY2)
+}
+
+void FixedConstraintSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	ConstraintSettings::SaveBinaryState(inStream);
+
+	inStream.Write(mSpace);
+	inStream.Write(mAutoDetectPoint);
+	inStream.Write(mPoint1);
+	inStream.Write(mAxisX1);
+	inStream.Write(mAxisY1);
+	inStream.Write(mPoint2);
+	inStream.Write(mAxisX2);
+	inStream.Write(mAxisY2);
+}
+
+void FixedConstraintSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	ConstraintSettings::RestoreBinaryState(inStream);
+
+	inStream.Read(mSpace);
+	inStream.Read(mAutoDetectPoint);
+	inStream.Read(mPoint1);
+	inStream.Read(mAxisX1);
+	inStream.Read(mAxisY1);
+	inStream.Read(mPoint2);
+	inStream.Read(mAxisX2);
+	inStream.Read(mAxisY2);
+}
+
+TwoBodyConstraint *FixedConstraintSettings::Create(Body &inBody1, Body &inBody2) const
+{
+	return new FixedConstraint(inBody1, inBody2, *this);
+}
+
+FixedConstraint::FixedConstraint(Body &inBody1, Body &inBody2, const FixedConstraintSettings &inSettings) :
+	TwoBodyConstraint(inBody1, inBody2, inSettings)
+{
+	// Store inverse of initial rotation from body 1 to body 2 in body 1 space
+	mInvInitialOrientation = RotationEulerConstraintPart::sGetInvInitialOrientationXY(inSettings.mAxisX1, inSettings.mAxisY1, inSettings.mAxisX2, inSettings.mAxisY2);
+
+	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
+	{
+		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()) / (inv_m1 + inv_m2);
+				else
+					anchor = inBody1.GetCenterOfMassPosition();
+			}
+
+			// Store local positions
+			mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * anchor);
+			mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * anchor);
+		}
+		else
+		{
+			// Store local positions
+			mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mPoint1);
+			mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mPoint2);
+		}
+
+		// 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);
+	}
+}
+
+void FixedConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
+{
+	if (mBody1->GetID() == inBodyID)
+		mLocalSpacePosition1 -= inDeltaCOM;
+	else if (mBody2->GetID() == inBodyID)
+		mLocalSpacePosition2 -= inDeltaCOM;
+}
+
+void FixedConstraint::SetupVelocityConstraint(float inDeltaTime)
+{
+	// Calculate constraint values that don't change when the bodies don't change position
+	Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation());
+	Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
+	mRotationConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, *mBody2, rotation2);
+	mPointConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, mLocalSpacePosition1, *mBody2, rotation2, mLocalSpacePosition2);
+}
+
+void FixedConstraint::ResetWarmStart()
+{
+	mRotationConstraintPart.Deactivate();
+	mPointConstraintPart.Deactivate();
+}
+
+void FixedConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
+{
+	// Warm starting: Apply previous frame impulse
+	mRotationConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+	mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+}
+
+bool FixedConstraint::SolveVelocityConstraint(float inDeltaTime)
+{
+	// Solve rotation constraint
+	bool rot = mRotationConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+
+	// Solve position constraint
+	bool pos = mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+
+	return rot || pos;
+}
+
+bool FixedConstraint::SolvePositionConstraint(float inDeltaTime, float 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 position constraint
+	mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), mLocalSpacePosition1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2);
+	bool pos = mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte);
+
+	return rot || pos;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void FixedConstraint::DrawConstraint(DebugRenderer *inRenderer) const
+{
+	RMat44 com1 = mBody1->GetCenterOfMassTransform();
+	RMat44 com2 = mBody2->GetCenterOfMassTransform();
+
+	RVec3 anchor1 = com1 * mLocalSpacePosition1;
+	RVec3 anchor2 = com2 * mLocalSpacePosition2;
+
+	// Draw constraint
+	inRenderer->DrawLine(com1.GetTranslation(), anchor1, Color::sGreen);
+	inRenderer->DrawLine(com2.GetTranslation(), anchor2, Color::sBlue);
+}
+#endif // JPH_DEBUG_RENDERER
+
+void FixedConstraint::SaveState(StateRecorder &inStream) const
+{
+	TwoBodyConstraint::SaveState(inStream);
+
+	mRotationConstraintPart.SaveState(inStream);
+	mPointConstraintPart.SaveState(inStream);
+}
+
+void FixedConstraint::RestoreState(StateRecorder &inStream)
+{
+	TwoBodyConstraint::RestoreState(inStream);
+
+	mRotationConstraintPart.RestoreState(inStream);
+	mPointConstraintPart.RestoreState(inStream);
+}
+
+Ref<ConstraintSettings> FixedConstraint::GetConstraintSettings() const
+{
+	FixedConstraintSettings *settings = new FixedConstraintSettings;
+	ToConstraintSettings(*settings);
+	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
+	settings->mPoint1 = RVec3(mLocalSpacePosition1);
+	settings->mAxisX1 = Vec3::sAxisX();
+	settings->mAxisY1 = Vec3::sAxisY();
+	settings->mPoint2 = RVec3(mLocalSpacePosition2);
+	settings->mAxisX2 = mInvInitialOrientation.RotateAxisX();
+	settings->mAxisY2 = mInvInitialOrientation.RotateAxisY();
+	return settings;
+}
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,96 @@
+// 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/ConstraintPart/RotationEulerConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/PointConstraintPart.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Fixed constraint settings, used to create a fixed constraint
+class JPH_EXPORT FixedConstraintSettings final : public TwoBodyConstraintSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, FixedConstraintSettings)
+
+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, 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 (they will be fixated in their current relative position/orientation). 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)
+	RVec3						mPoint1 = RVec3::sZero();
+	Vec3						mAxisX1 = Vec3::sAxisX();
+	Vec3						mAxisY1 = Vec3::sAxisY();
+
+	/// Body 2 constraint reference frame (space determined by mSpace)
+	RVec3						mPoint2 = RVec3::sZero();
+	Vec3						mAxisX2 = Vec3::sAxisX();
+	Vec3						mAxisY2 = Vec3::sAxisY();
+
+protected:
+	// See: ConstraintSettings::RestoreBinaryState
+	virtual void				RestoreBinaryState(StreamIn &inStream) override;
+};
+
+/// A fixed constraint welds two bodies together removing all degrees of freedom between them.
+/// This variant uses Euler angles for the rotation constraint.
+class JPH_EXPORT FixedConstraint final : public TwoBodyConstraint
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+								FixedConstraint(Body &inBody1, Body &inBody2, const FixedConstraintSettings &inSettings);
+
+	// Generic interface of a constraint
+	virtual EConstraintSubType	GetSubType() const override									{ return EConstraintSubType::Fixed; }
+	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::sRotationTranslation(mInvInitialOrientation, mLocalSpacePosition2); }
+
+	///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint)
+	inline Vec3					GetTotalLambdaPosition() const								{ return mPointConstraintPart.GetTotalLambda(); }
+	inline Vec3					GetTotalLambdaRotation() const								{ return mRotationConstraintPart.GetTotalLambda(); }
+
+private:
+	// CONFIGURATION PROPERTIES FOLLOW
+
+	// Local space constraint positions
+	Vec3						mLocalSpacePosition1;
+	Vec3						mLocalSpacePosition2;
+
+	// Inverse of initial rotation from body 1 to body 2 in body 1 space
+	Quat						mInvInitialOrientation;
+
+	// RUN TIME PROPERTIES FOLLOW
+
+	// The constraint parts
+	RotationEulerConstraintPart	mRotationConstraintPart;
+	PointConstraintPart			mPointConstraintPart;
+};
+
+JPH_NAMESPACE_END