From 61724cc4b9664f9bb107de947077d53b28dfc556 Mon Sep 17 00:00:00 2001
From: SirLynix <lynix680@gmail.com>
Date: Wed, 5 Apr 2023 08:31:13 +0200
Subject: [PATCH] JoltPhysics3D: Add distance constraint

---
 .../Nazara/JoltPhysics3D/JoltConstraint3D.hpp | 20 +++++
 src/Nazara/JoltPhysics3D/JoltConstraint3D.cpp | 87 +++++++++++++++++++
 2 files changed, 107 insertions(+)

diff --git a/include/Nazara/JoltPhysics3D/JoltConstraint3D.hpp b/include/Nazara/JoltPhysics3D/JoltConstraint3D.hpp
index 4b2e4bf77..eefddb0f6 100644
--- a/include/Nazara/JoltPhysics3D/JoltConstraint3D.hpp
+++ b/include/Nazara/JoltPhysics3D/JoltConstraint3D.hpp
@@ -58,6 +58,26 @@ namespace Nz
 			std::unique_ptr<JPH::TwoBodyConstraint> m_constraint;
 	};
 
+	class NAZARA_JOLTPHYSICS3D_API JoltDistanceConstraint3D : public JoltConstraint3D
+	{
+		public:
+			JoltDistanceConstraint3D(JoltRigidBody3D& first, const Vector3f& pivot, float maxDist = -1.f, float minDist = -1.f);
+			JoltDistanceConstraint3D(JoltRigidBody3D& first, JoltRigidBody3D& second, const Vector3f& pivot, float maxDist = -1.f, float minDist = -1.f);
+			JoltDistanceConstraint3D(JoltRigidBody3D& first, JoltRigidBody3D& second, const Vector3f& firstAnchor, const Vector3f& secondAnchor, float maxDist = -1.f, float minDist = -1.f);
+			~JoltDistanceConstraint3D() = default;
+
+			float GetDamping() const;
+			float GetFrequency() const;
+			float GetMaxDistance() const;
+			float GetMinDistance() const;
+
+			void SetDamping(float damping);
+			void SetDistance(float minDist, float maxDist);
+			void SetFrequency(float frequency);
+			void SetMaxDistance(float maxDist);
+			void SetMinDistance(float minDist);
+	};
+
 	class NAZARA_JOLTPHYSICS3D_API JoltPivotConstraint3D : public JoltConstraint3D
 	{
 		public:
diff --git a/src/Nazara/JoltPhysics3D/JoltConstraint3D.cpp b/src/Nazara/JoltPhysics3D/JoltConstraint3D.cpp
index b43d972aa..0b5e4cc3e 100644
--- a/src/Nazara/JoltPhysics3D/JoltConstraint3D.cpp
+++ b/src/Nazara/JoltPhysics3D/JoltConstraint3D.cpp
@@ -7,6 +7,7 @@
 #include <Nazara/JoltPhysics3D/JoltHelper.hpp>
 #include <Jolt/Jolt.h>
 #include <Jolt/Physics/PhysicsSystem.h>
+#include <Jolt/Physics/Constraints/DistanceConstraint.h>
 #include <Jolt/Physics/Constraints/PointConstraint.h>
 #include <cassert>
 #include <Nazara/JoltPhysics3D/Debug.hpp>
@@ -99,6 +100,92 @@ namespace Nz
 	}
 
 
+	JoltDistanceConstraint3D::JoltDistanceConstraint3D(JoltRigidBody3D& first, const Vector3f& pivot, float maxDist, float minDist)
+	{
+		JPH::DistanceConstraintSettings settings;
+		settings.mPoint1 = ToJolt(pivot);
+		settings.mPoint2 = ToJolt(pivot);
+		settings.mSpace = JPH::EConstraintSpace::WorldSpace;
+		settings.mMaxDistance = maxDist;
+		settings.mMinDistance = minDist;
+
+		SetupConstraint(std::make_unique<JPH::DistanceConstraint>(*first.GetBody(), JPH::Body::sFixedToWorld, settings));
+	}
+
+	JoltDistanceConstraint3D::JoltDistanceConstraint3D(JoltRigidBody3D& first, JoltRigidBody3D& second, const Vector3f& pivot, float maxDist, float minDist)
+	{
+		JPH::DistanceConstraintSettings settings;
+		settings.mPoint1 = ToJolt(pivot);
+		settings.mPoint2 = ToJolt(pivot);
+		settings.mSpace = JPH::EConstraintSpace::WorldSpace;
+		settings.mMaxDistance = maxDist;
+		settings.mMinDistance = minDist;
+
+		SetupConstraint(std::make_unique<JPH::DistanceConstraint>(*first.GetBody(), *second.GetBody(), settings));
+	}
+
+	JoltDistanceConstraint3D::JoltDistanceConstraint3D(JoltRigidBody3D& first, JoltRigidBody3D& second, const Vector3f& firstAnchor, const Vector3f& secondAnchor, float maxDist, float minDist)
+	{
+		JPH::DistanceConstraintSettings settings;
+		settings.mPoint1 = ToJolt(firstAnchor);
+		settings.mPoint2 = ToJolt(secondAnchor);
+		settings.mSpace = JPH::EConstraintSpace::WorldSpace;
+		settings.mMaxDistance = maxDist;
+		settings.mMinDistance = minDist;
+
+		SetupConstraint(std::make_unique<JPH::DistanceConstraint>(*first.GetBody(), *second.GetBody(), settings));
+	}
+
+	float JoltDistanceConstraint3D::GetDamping() const
+	{
+		return GetConstraint<JPH::DistanceConstraint>()->GetDamping();
+	}
+
+	float JoltDistanceConstraint3D::GetFrequency() const
+	{
+		return GetConstraint<JPH::DistanceConstraint>()->GetFrequency();
+	}
+
+	float JoltDistanceConstraint3D::GetMaxDistance() const
+	{
+		return GetConstraint<JPH::DistanceConstraint>()->GetMaxDistance();
+	}
+
+	float JoltDistanceConstraint3D::GetMinDistance() const
+	{
+		return GetConstraint<JPH::DistanceConstraint>()->GetMinDistance();
+	}
+
+	void JoltDistanceConstraint3D::SetDamping(float damping)
+	{
+		GetConstraint<JPH::DistanceConstraint>()->SetDamping(damping);
+	}
+
+	void JoltDistanceConstraint3D::SetDistance(float minDist, float maxDist)
+	{
+		GetConstraint<JPH::DistanceConstraint>()->SetDistance(minDist, maxDist);
+	}
+
+	void JoltDistanceConstraint3D::SetFrequency(float frequency)
+	{
+		GetConstraint<JPH::DistanceConstraint>()->SetFrequency(frequency);
+	}
+
+	void JoltDistanceConstraint3D::SetMaxDistance(float maxDist)
+	{
+		JPH::DistanceConstraint* constraint = GetConstraint<JPH::DistanceConstraint>();
+
+		constraint->SetDistance(constraint->GetMinDistance(), maxDist);
+	}
+
+	void JoltDistanceConstraint3D::SetMinDistance(float minDist)
+	{
+		JPH::DistanceConstraint* constraint = GetConstraint<JPH::DistanceConstraint>();
+
+		constraint->SetDistance(minDist, constraint->GetMaxDistance());
+	}
+
+
 	JoltPivotConstraint3D::JoltPivotConstraint3D(JoltRigidBody3D& first, const Vector3f& pivot)
 	{
 		JPH::PointConstraintSettings settings;