ChipmunkPhysics2D/Constraint2D: Fix leak and implement single body constraints

Also move anchors to world space

include/Nazara/ChipmunkPhysics2D/ChipmunkConstraint2D.hpp

@@ -15,7 +15,6 @@
 #include <Nazara/Core/ObjectHandle.hpp>
 #include <Nazara/Math/Angle.hpp>
 #include <NazaraUtils/MovablePtr.hpp>
-#include <vector>
 
 struct cpConstraint;
 
@@ -29,7 +28,7 @@ namespace Nz
 	{
 		public:
 			ChipmunkConstraint2D(const ChipmunkConstraint2D&) = delete;
-			ChipmunkConstraint2D(ChipmunkConstraint2D&& rhs);
+			ChipmunkConstraint2D(ChipmunkConstraint2D&& constraint) noexcept;
 			virtual ~ChipmunkConstraint2D();
 
 			void EnableBodyCollision(bool enable);
@@ -46,18 +45,22 @@ namespace Nz
 			const ChipmunkPhysWorld2D& GetWorld() const;
 
 			bool IsBodyCollisionEnabled() const;
+			bool IsSingleBody() const;
 
 			void SetErrorBias(float bias);
 			void SetMaxBias(float bias);
 			void SetMaxForce(float force);
 
 			ChipmunkConstraint2D& operator=(const ChipmunkConstraint2D&) = delete;
-			ChipmunkConstraint2D& operator=(ChipmunkConstraint2D&& rhs);
+			ChipmunkConstraint2D& operator=(ChipmunkConstraint2D&& constraint) noexcept;
 
 		protected:
 			ChipmunkConstraint2D(ChipmunkPhysWorld2D* world, cpConstraint* constraint);
 
 			MovablePtr<cpConstraint> m_constraint;
+
+		private:
+			void Destroy();
 	};
 
 	class ChipmunkDampedSpringConstraint2D;
@@ -140,6 +143,7 @@ namespace Nz
 	class NAZARA_CHIPMUNKPHYSICS2D_API ChipmunkPinConstraint2D : public ChipmunkConstraint2D
 	{
 		public:
+			ChipmunkPinConstraint2D(ChipmunkRigidBody2D& body, const Vector2f& anchor);
 			ChipmunkPinConstraint2D(ChipmunkRigidBody2D& first, ChipmunkRigidBody2D& second, const Vector2f& firstAnchor, const Vector2f& secondAnchor);
 			~ChipmunkPinConstraint2D() = default;
 
@@ -159,6 +163,7 @@ namespace Nz
 	class NAZARA_CHIPMUNKPHYSICS2D_API ChipmunkPivotConstraint2D : public ChipmunkConstraint2D
 	{
 		public:
+			ChipmunkPivotConstraint2D(ChipmunkRigidBody2D& body, const Vector2f& anchor);
 			ChipmunkPivotConstraint2D(ChipmunkRigidBody2D& first, ChipmunkRigidBody2D& second, const Vector2f& anchor);
 			ChipmunkPivotConstraint2D(ChipmunkRigidBody2D& first, ChipmunkRigidBody2D& second, const Vector2f& firstAnchor, const Vector2f& secondAnchor);
 			~ChipmunkPivotConstraint2D() = default;

src/Nazara/ChipmunkPhysics2D/ChipmunkConstraint2D.cpp

@@ -3,6 +3,7 @@
 // For conditions of distribution and use, see copyright notice in Config.hpp
 
 #include <Nazara/ChipmunkPhysics2D/ChipmunkConstraint2D.hpp>
+#include <Nazara/ChipmunkPhysics2D/ChipmunkHelper.hpp>
 #include <chipmunk/chipmunk.h>
 #include <Nazara/ChipmunkPhysics2D/Debug.hpp>
 
@@ -15,15 +16,16 @@ namespace Nz
 		cpSpaceAddConstraint(world->GetHandle(), m_constraint);
 	}
 
-	ChipmunkConstraint2D::ChipmunkConstraint2D(ChipmunkConstraint2D&& rhs) :
+	ChipmunkConstraint2D::ChipmunkConstraint2D(ChipmunkConstraint2D&& constraint) noexcept :
-	m_constraint(std::move(rhs.m_constraint))
+	m_constraint(std::move(constraint.m_constraint))
 	{
-		cpConstraintSetUserData(m_constraint, this);
+		if (m_constraint)
+			cpConstraintSetUserData(m_constraint, this);
 	}
 
 	ChipmunkConstraint2D::~ChipmunkConstraint2D()
 	{
-		cpSpaceRemoveConstraint(cpConstraintGetSpace(m_constraint), m_constraint);
+		Destroy();
 	}
 
 	void ChipmunkConstraint2D::EnableBodyCollision(bool enable)
@@ -43,11 +45,13 @@ namespace Nz
 
 	ChipmunkRigidBody2D& ChipmunkConstraint2D::GetBodyB()
 	{
+		NazaraAssert(!IsSingleBody(), "constraint is not attached to a second body");
 		return *static_cast<ChipmunkRigidBody2D*>(cpBodyGetUserData(cpConstraintGetBodyB(m_constraint)));
 	}
 
 	const ChipmunkRigidBody2D& ChipmunkConstraint2D::GetBodyB() const
 	{
+		NazaraAssert(!IsSingleBody(), "constraint is not attached to a second body");
 		return *static_cast<ChipmunkRigidBody2D*>(cpBodyGetUserData(cpConstraintGetBodyB(m_constraint)));
 	}
 
@@ -86,6 +90,11 @@ namespace Nz
 		return cpConstraintGetCollideBodies(m_constraint) == cpTrue;
 	}
 
+	bool ChipmunkConstraint2D::IsSingleBody() const
+	{
+		return cpConstraintGetBodyB(m_constraint) == cpSpaceGetStaticBody(cpConstraintGetSpace(m_constraint));
+	}
+
 	void ChipmunkConstraint2D::SetErrorBias(float bias)
 	{
 		cpConstraintSetErrorBias(m_constraint, bias);
@@ -101,17 +110,31 @@ namespace Nz
 		cpConstraintSetMaxForce(m_constraint, force);
 	}
 
-	ChipmunkConstraint2D& ChipmunkConstraint2D::operator=(ChipmunkConstraint2D && rhs)
+	ChipmunkConstraint2D& ChipmunkConstraint2D::operator=(ChipmunkConstraint2D&& rhs) noexcept
 	{
+		Destroy();
+
 		m_constraint = std::move(rhs.m_constraint);
-		cpConstraintSetUserData(m_constraint, this);
+		if (m_constraint)
+			cpConstraintSetUserData(m_constraint, this);
 
 		return *this;
 	}
 
+	void ChipmunkConstraint2D::Destroy()
+	{
+		if (m_constraint)
+		{
+			cpSpaceRemoveConstraint(cpConstraintGetSpace(m_constraint), m_constraint);
+			cpConstraintDestroy(m_constraint);
+
+			m_constraint = nullptr;
+		}
+	}
+
 
 	ChipmunkDampedSpringConstraint2D::ChipmunkDampedSpringConstraint2D(ChipmunkRigidBody2D& first, ChipmunkRigidBody2D& second, const Vector2f& firstAnchor, const Vector2f& secondAnchor, float restLength, float stiffness, float damping) :
-	ChipmunkConstraint2D(first.GetWorld(), cpDampedSpringNew(first.GetHandle(), second.GetHandle(), cpv(firstAnchor.x, firstAnchor.y), cpv(secondAnchor.x, secondAnchor.y), restLength, stiffness, damping))
+	ChipmunkConstraint2D(first.GetWorld(), cpDampedSpringNew(first.GetHandle(), second.GetHandle(), ToChipmunk(firstAnchor), ToChipmunk(secondAnchor), restLength, stiffness, damping))
 	{
 	}
 
@@ -149,7 +172,7 @@ namespace Nz
 
 	void ChipmunkDampedSpringConstraint2D::SetFirstAnchor(const Vector2f& firstAnchor)
 	{
-		cpDampedSpringSetAnchorA(m_constraint, cpv(firstAnchor.x, firstAnchor.y));
+		cpDampedSpringSetAnchorA(m_constraint, ToChipmunk(firstAnchor));
 	}
 
 	void ChipmunkDampedSpringConstraint2D::SetRestLength(float newLength)
@@ -159,7 +182,7 @@ namespace Nz
 
 	void ChipmunkDampedSpringConstraint2D::SetSecondAnchor(const Vector2f& firstAnchor)
 	{
-		cpDampedSpringSetAnchorB(m_constraint, cpv(firstAnchor.x, firstAnchor.y));
+		cpDampedSpringSetAnchorB(m_constraint, ToChipmunk(firstAnchor));
 	}
 
 	void ChipmunkDampedSpringConstraint2D::SetStiffness(float newStiffness)
@@ -246,8 +269,13 @@ namespace Nz
 	}
 
 
+	ChipmunkPinConstraint2D::ChipmunkPinConstraint2D(ChipmunkRigidBody2D& body, const Vector2f& anchor) :
+	ChipmunkConstraint2D(body.GetWorld(), cpPinJointNew(body.GetHandle(), cpSpaceGetStaticBody(body.GetWorld()->GetHandle()), ToChipmunk(body.ToLocal(anchor)), ToChipmunk(body.ToLocal(anchor))))
+	{
+	}
+
 	ChipmunkPinConstraint2D::ChipmunkPinConstraint2D(ChipmunkRigidBody2D& first, ChipmunkRigidBody2D& second, const Vector2f& firstAnchor, const Vector2f& secondAnchor) :
-	ChipmunkConstraint2D(first.GetWorld(), cpPinJointNew(first.GetHandle(), second.GetHandle(), cpv(firstAnchor.x, firstAnchor.y), cpv(secondAnchor.x, secondAnchor.y)))
+	ChipmunkConstraint2D(first.GetWorld(), cpPinJointNew(first.GetHandle(), second.GetHandle(), ToChipmunk(firstAnchor), ToChipmunk(secondAnchor)))
 	{
 	}
 
@@ -258,14 +286,12 @@ namespace Nz
 
 	Vector2f ChipmunkPinConstraint2D::GetFirstAnchor() const
 	{
-		cpVect anchor = cpPinJointGetAnchorA(m_constraint);
+		return FromChipmunk(cpBodyLocalToWorld(cpConstraintGetBodyA(m_constraint), cpPinJointGetAnchorA(m_constraint)));
-		return Vector2f(static_cast<float>(anchor.x), static_cast<float>(anchor.y));
 	}
 
 	Vector2f ChipmunkPinConstraint2D::GetSecondAnchor() const
 	{
-		cpVect anchor = cpPinJointGetAnchorB(m_constraint);
+		return FromChipmunk(cpBodyLocalToWorld(cpConstraintGetBodyB(m_constraint), cpPinJointGetAnchorB(m_constraint)));
-		return Vector2f(static_cast<float>(anchor.x), static_cast<float>(anchor.y));
 	}
 
 	void ChipmunkPinConstraint2D::SetDistance(float newDistance)
@@ -275,45 +301,48 @@ namespace Nz
 
 	void ChipmunkPinConstraint2D::SetFirstAnchor(const Vector2f& firstAnchor)
 	{
-		cpPinJointSetAnchorA(m_constraint, cpv(firstAnchor.x, firstAnchor.y));
+		cpPinJointSetAnchorA(m_constraint, cpBodyWorldToLocal(cpConstraintGetBodyA(m_constraint), ToChipmunk(firstAnchor)));
 	}
 
 	void ChipmunkPinConstraint2D::SetSecondAnchor(const Vector2f& firstAnchor)
 	{
-		cpPinJointSetAnchorB(m_constraint, cpv(firstAnchor.x, firstAnchor.y));
+		cpPinJointSetAnchorB(m_constraint, cpBodyWorldToLocal(cpConstraintGetBodyB(m_constraint), ToChipmunk(firstAnchor)));
 	}
 
 
+	ChipmunkPivotConstraint2D::ChipmunkPivotConstraint2D(ChipmunkRigidBody2D& body, const Vector2f& anchor) :
+	ChipmunkConstraint2D(body.GetWorld(), cpPivotJointNew(cpSpaceGetStaticBody(body.GetWorld()->GetHandle()), body.GetHandle(), ToChipmunk(anchor)))
+	{
+	}
+
 	ChipmunkPivotConstraint2D::ChipmunkPivotConstraint2D(ChipmunkRigidBody2D& first, ChipmunkRigidBody2D& second, const Vector2f& anchor) :
-	ChipmunkConstraint2D(first.GetWorld(), cpPivotJointNew(first.GetHandle(), second.GetHandle(), cpv(anchor.x, anchor.y)))
+	ChipmunkConstraint2D(first.GetWorld(), cpPivotJointNew(first.GetHandle(), second.GetHandle(), ToChipmunk(anchor)))
 	{
 	}
 
 	ChipmunkPivotConstraint2D::ChipmunkPivotConstraint2D(ChipmunkRigidBody2D& first, ChipmunkRigidBody2D& second, const Vector2f& firstAnchor, const Vector2f& secondAnchor) :
-	ChipmunkConstraint2D(first.GetWorld(), cpPivotJointNew2(first.GetHandle(), second.GetHandle(), cpv(firstAnchor.x, firstAnchor.y), cpv(secondAnchor.x, secondAnchor.y)))
+	ChipmunkConstraint2D(first.GetWorld(), cpPivotJointNew2(first.GetHandle(), second.GetHandle(), ToChipmunk(firstAnchor), ToChipmunk(secondAnchor)))
 	{
 	}
 
 	Vector2f ChipmunkPivotConstraint2D::GetFirstAnchor() const
 	{
-		cpVect anchor = cpPivotJointGetAnchorA(m_constraint);
+		return FromChipmunk(cpBodyLocalToWorld(cpConstraintGetBodyA(m_constraint), cpPivotJointGetAnchorA(m_constraint)));
-		return Vector2f(static_cast<float>(anchor.x), static_cast<float>(anchor.y));
 	}
 
 	Vector2f ChipmunkPivotConstraint2D::GetSecondAnchor() const
 	{
-		cpVect anchor = cpPivotJointGetAnchorB(m_constraint);
+		return FromChipmunk(cpBodyLocalToWorld(cpConstraintGetBodyB(m_constraint), cpPivotJointGetAnchorB(m_constraint)));
-		return Vector2f(static_cast<float>(anchor.x), static_cast<float>(anchor.y));
 	}
 
 	void ChipmunkPivotConstraint2D::SetFirstAnchor(const Vector2f& firstAnchor)
 	{
-		cpPivotJointSetAnchorA(m_constraint, cpv(firstAnchor.x, firstAnchor.y));
+		cpPivotJointSetAnchorA(m_constraint, cpBodyWorldToLocal(cpConstraintGetBodyA(m_constraint), ToChipmunk(firstAnchor)));
 	}
 
 	void ChipmunkPivotConstraint2D::SetSecondAnchor(const Vector2f& firstAnchor)
 	{
-		cpPivotJointSetAnchorB(m_constraint, cpv(firstAnchor.x, firstAnchor.y));
+		cpPivotJointSetAnchorB(m_constraint, cpBodyWorldToLocal(cpConstraintGetBodyB(m_constraint), ToChipmunk(firstAnchor)));
 	}
 
 
@@ -380,7 +409,7 @@ namespace Nz
 
 
 	ChipmunkSlideConstraint2D::ChipmunkSlideConstraint2D(ChipmunkRigidBody2D& first, ChipmunkRigidBody2D& second, const Vector2f& firstAnchor, const Vector2f& secondAnchor, float min, float max) :
-	ChipmunkConstraint2D(first.GetWorld(), cpSlideJointNew(first.GetHandle(), second.GetHandle(), cpv(firstAnchor.x, firstAnchor.y), cpv(secondAnchor.x, secondAnchor.y), min, max))
+	ChipmunkConstraint2D(first.GetWorld(), cpSlideJointNew(first.GetHandle(), second.GetHandle(), ToChipmunk(firstAnchor), ToChipmunk(secondAnchor), min, max))
 	{
 	}
 
@@ -408,7 +437,7 @@ namespace Nz
 
 	void ChipmunkSlideConstraint2D::SetFirstAnchor(const Vector2f& firstAnchor)
 	{
-		cpSlideJointSetAnchorA(m_constraint, cpv(firstAnchor.x, firstAnchor.y));
+		cpSlideJointSetAnchorA(m_constraint, ToChipmunk(firstAnchor));
 	}
 
 	void ChipmunkSlideConstraint2D::SetMaxDistance(float newMaxDistance)
@@ -423,6 +452,6 @@ namespace Nz
 
 	void ChipmunkSlideConstraint2D::SetSecondAnchor(const Vector2f& firstAnchor)
 	{
-		cpSlideJointSetAnchorB(m_constraint, cpv(firstAnchor.x, firstAnchor.y));
+		cpSlideJointSetAnchorB(m_constraint, ToChipmunk(firstAnchor));
 	}
 }