Physics3D: Add PivotConstraint3D

include/Nazara/Physics3D.hpp

@@ -31,6 +31,7 @@
 
 #include <Nazara/Physics3D/Collider3D.hpp>
 #include <Nazara/Physics3D/Config.hpp>
+#include <Nazara/Physics3D/Constraint3D.hpp>
 #include <Nazara/Physics3D/Enums.hpp>
 #include <Nazara/Physics3D/Physics3D.hpp>
 #include <Nazara/Physics3D/PhysWorld3D.hpp>

include/Nazara/Physics3D/Constraint3D.hpp

@@ -0,0 +1,74 @@
+// Copyright (C) 2023 Jérôme "Lynix" Leclercq (lynix680@gmail.com)
+// This file is part of the "Nazara Engine - Physics3D module"
+// For conditions of distribution and use, see copyright notice in Config.hpp
+
+#pragma once
+
+#ifndef NAZARA_PHYSICS3D_CONSTRAINT3D_HPP
+#define NAZARA_PHYSICS3D_CONSTRAINT3D_HPP
+
+#include <Nazara/Prerequisites.hpp>
+#include <Nazara/Core/HandledObject.hpp>
+#include <Nazara/Core/ObjectHandle.hpp>
+#include <Nazara/Physics3D/Config.hpp>
+#include <Nazara/Physics3D/PhysWorld3D.hpp>
+#include <Nazara/Physics3D/RigidBody3D.hpp>
+
+class btTypedConstraint;
+
+namespace Nz
+{
+	class Constraint3D;
+
+	using Constraint3DHandle = ObjectHandle<Constraint3D>;
+
+	class NAZARA_PHYSICS3D_API Constraint3D : public HandledObject<Constraint3D>
+	{
+		public:
+			Constraint3D(const Constraint3D&) = delete;
+			Constraint3D(Constraint3D&& constraint) noexcept;
+			virtual ~Constraint3D();
+
+			RigidBody3D& GetBodyA();
+			const RigidBody3D& GetBodyA() const;
+			RigidBody3D& GetBodyB();
+			const RigidBody3D& GetBodyB() const;
+			PhysWorld3D& GetWorld();
+			const PhysWorld3D& GetWorld() const;
+
+			inline bool IsBodyCollisionEnabled() const;
+			bool IsSingleBody() const;
+
+			Constraint3D& operator=(const Constraint3D&) = delete;
+			Constraint3D& operator=(Constraint3D&& constraint) noexcept;
+
+		protected:
+			Constraint3D(std::unique_ptr<btTypedConstraint> constraint, bool disableCollisions = false);
+
+			template<typename T> T* GetConstraint();
+			template<typename T> const T* GetConstraint() const;
+
+		private:
+			std::unique_ptr<btTypedConstraint> m_constraint;
+			bool m_bodyCollisionEnabled;
+	};
+
+	class NAZARA_PHYSICS3D_API PivotConstraint3D : public Constraint3D
+	{
+		public:
+			PivotConstraint3D(RigidBody3D& first, const Vector3f& pivot);
+			PivotConstraint3D(RigidBody3D& first, RigidBody3D& second, const Vector3f& pivot, bool disableCollisions = false);
+			PivotConstraint3D(RigidBody3D& first, RigidBody3D& second, const Vector3f& firstAnchor, const Vector3f& secondAnchor, bool disableCollisions = false);
+			~PivotConstraint3D() = default;
+
+			Vector3f GetFirstAnchor() const;
+			Vector3f GetSecondAnchor() const;
+
+			void SetFirstAnchor(const Vector3f& firstAnchor);
+			void SetSecondAnchor(const Vector3f& secondAnchor);
+	};
+}
+
+#include <Nazara/Physics3D/Constraint3D.inl>
+
+#endif // NAZARA_PHYSICS3D_CONSTRAINT3D_HPP

include/Nazara/Physics3D/Constraint3D.inl

@@ -0,0 +1,29 @@
+// Copyright (C) 2023 Jérôme "Lynix" Leclercq (lynix680@gmail.com)
+// This file is part of the "Nazara Engine - Physics3D module"
+// For conditions of distribution and use, see copyright notice in Config.hpp
+
+#include <Nazara/Physics3D/Constraint3D.hpp>
+#include <Nazara/Physics3D/Debug.hpp>
+#include <Nazara/Physics3D/Debug.hpp>
+
+namespace Nz
+{
+	inline bool Constraint3D::IsBodyCollisionEnabled() const
+	{
+		return m_bodyCollisionEnabled;
+	}
+
+	template<typename T>
+	T* Constraint3D::GetConstraint()
+	{
+		return SafeCast<T*>(m_constraint.get());
+	}
+
+	template<typename T>
+	const T* Constraint3D::GetConstraint() const
+	{
+		return SafeCast<const T*>(m_constraint.get());
+	}
+}
+
+#include <Nazara/Physics3D/DebugOff.hpp>

include/Nazara/Physics3D/RigidBody3D.hpp

@@ -43,16 +43,17 @@ namespace Nz
 			Boxf GetAABB() const;
 			float GetAngularDamping() const;
 			Vector3f GetAngularVelocity() const;
-			const std::shared_ptr<Collider3D>& GetGeom() const;
+			inline const std::shared_ptr<Collider3D>& GetGeom() const;
 			float GetLinearDamping() const;
 			Vector3f GetLinearVelocity() const;
 			float GetMass() const;
 			Vector3f GetMassCenter(CoordSys coordSys = CoordSys::Local) const;
 			Matrix4f GetMatrix() const;
 			Vector3f GetPosition() const;
-			btRigidBody* GetRigidBody() const;
+			inline btRigidBody* GetRigidBody() const;
 			Quaternionf GetRotation() const;
-			PhysWorld3D* GetWorld() const;
+			inline std::size_t GetUniqueIndex() const;
+			inline PhysWorld3D* GetWorld() const;
 
 			bool IsSimulationEnabled() const;
 			bool IsSleeping() const;
@@ -68,6 +69,11 @@ namespace Nz
 			void SetPosition(const Vector3f& position);
 			void SetRotation(const Quaternionf& rotation);
 
+			Quaternionf ToLocal(const Quaternionf& worldRotation);
+			Vector3f ToLocal(const Vector3f& worldPosition);
+			Quaternionf ToWorld(const Quaternionf& localRotation);
+			Vector3f ToWorld(const Vector3f& localPosition);
+
 			void WakeUp();
 
 			RigidBody3D& operator=(const RigidBody3D& object) = delete;

include/Nazara/Physics3D/RigidBody3D.inl

@@ -10,6 +10,25 @@ namespace Nz
 	{
 		return EnableSleeping(false);
 	}
+
+	inline const std::shared_ptr<Collider3D>& RigidBody3D::GetGeom() const
+	{
+		return m_geom;
+	}
+
+	inline btRigidBody* RigidBody3D::GetRigidBody() const
+	{
+		return m_body;
+	}
+	inline std::size_t RigidBody3D::GetUniqueIndex() const
+	{
+		return m_bodyPoolIndex;
+	}
+
+	inline PhysWorld3D* RigidBody3D::GetWorld() const
+	{
+		return m_world;
+	}
 }
 
 #include <Nazara/Physics3D/DebugOff.hpp>

src/Nazara/Physics3D/BulletHelper.inl

@@ -2,7 +2,6 @@
 // This file is part of the "Nazara Engine - Physics3D module"
 // For conditions of distribution and use, see copyright notice in Config.hpp
 
-#include <Nazara/Physics3D/BulletHelper.hpp>
 #include <Nazara/Physics3D/Debug.hpp>
 
 namespace Nz

src/Nazara/Physics3D/Constraint3D.cpp

@@ -0,0 +1,123 @@
+// Copyright (C) 2023 Jérôme "Lynix" Leclercq (lynix680@gmail.com)
+// This file is part of the "Nazara Engine - Physics3D module"
+// For conditions of distribution and use, see copyright notice in Config.hpp
+
+#include <Nazara/Physics3D/Constraint3D.hpp>
+#include <Nazara/Physics3D/BulletHelper.hpp>
+#include <BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h>
+#include <BulletDynamics/Dynamics/btDynamicsWorld.h>
+#include <Nazara/Physics3D/Debug.hpp>
+
+namespace Nz
+{
+	Constraint3D::Constraint3D(std::unique_ptr<btTypedConstraint> constraint, bool disableCollisions) :
+	m_constraint(std::move(constraint)),
+	m_bodyCollisionEnabled(!disableCollisions)
+	{
+		btDynamicsWorld* world = GetWorld().GetDynamicsWorld();
+		world->addConstraint(m_constraint.get(), disableCollisions);
+	}
+
+	Constraint3D::Constraint3D(Constraint3D&& constraint) noexcept :
+	m_constraint(std::move(constraint.m_constraint)),
+	m_bodyCollisionEnabled(constraint.m_bodyCollisionEnabled)
+	{
+		if (m_constraint)
+			m_constraint->setUserConstraintPtr(this);
+	}
+
+	Constraint3D::~Constraint3D()
+	{
+		if (m_constraint)
+		{
+			btDynamicsWorld* world = GetWorld().GetDynamicsWorld();
+			world->removeConstraint(m_constraint.get());
+		}
+	}
+
+	RigidBody3D& Constraint3D::GetBodyA()
+	{
+		return *static_cast<RigidBody3D*>(m_constraint->getRigidBodyA().getUserPointer());
+	}
+
+	const RigidBody3D& Constraint3D::GetBodyA() const
+	{
+		return *static_cast<RigidBody3D*>(m_constraint->getRigidBodyA().getUserPointer());
+	}
+
+	RigidBody3D& Constraint3D::GetBodyB()
+	{
+		NazaraAssert(!IsSingleBody(), "constraint is not attached to a second body");
+		return *static_cast<RigidBody3D*>(m_constraint->getRigidBodyB().getUserPointer());
+	}
+
+	const RigidBody3D& Constraint3D::GetBodyB() const
+	{
+		NazaraAssert(!IsSingleBody(), "constraint is not attached to a second body");
+		return *static_cast<RigidBody3D*>(m_constraint->getRigidBodyB().getUserPointer());
+	}
+
+	PhysWorld3D& Constraint3D::GetWorld()
+	{
+		return *GetBodyA().GetWorld();
+	}
+
+	const PhysWorld3D& Constraint3D::GetWorld() const
+	{
+		return *GetBodyA().GetWorld();
+	}
+
+	bool Constraint3D::IsSingleBody() const
+	{
+		return &m_constraint->getRigidBodyB() == &btTypedConstraint::getFixedBody();
+	}
+
+	Constraint3D& Constraint3D::operator=(Constraint3D&& constraint) noexcept
+	{
+		m_constraint.reset();
+
+		m_constraint = std::move(constraint.m_constraint);
+		m_bodyCollisionEnabled = constraint.m_bodyCollisionEnabled;
+
+		if (m_constraint)
+			m_constraint->setUserConstraintPtr(this);
+
+		return *this;
+	}
+
+
+	PivotConstraint3D::PivotConstraint3D(RigidBody3D& first, const Vector3f& pivot) :
+	Constraint3D(std::make_unique<btPoint2PointConstraint>(*first.GetRigidBody(), ToBullet(first.ToLocal(pivot))))
+	{
+	}
+
+	PivotConstraint3D::PivotConstraint3D(RigidBody3D& first, RigidBody3D& second, const Vector3f& pivot, bool disableCollisions) :
+	Constraint3D(std::make_unique<btPoint2PointConstraint>(*first.GetRigidBody(), *second.GetRigidBody(), ToBullet(first.ToLocal(pivot)), ToBullet(second.ToLocal(pivot))), disableCollisions)
+	{
+	}
+
+	PivotConstraint3D::PivotConstraint3D(RigidBody3D& first, RigidBody3D& second, const Vector3f& firstAnchor, const Vector3f& secondAnchor, bool disableCollisions) :
+	Constraint3D(std::make_unique<btPoint2PointConstraint>(*first.GetRigidBody(), *second.GetRigidBody(), ToBullet(firstAnchor), ToBullet(secondAnchor)), disableCollisions)
+	{
+	}
+
+	Vector3f PivotConstraint3D::GetFirstAnchor() const
+	{
+		return FromBullet(GetConstraint<btPoint2PointConstraint>()->getPivotInA());
+	}
+
+	Vector3f PivotConstraint3D::GetSecondAnchor() const
+	{
+		return FromBullet(GetConstraint<btPoint2PointConstraint>()->getPivotInB());
+	}
+
+	void PivotConstraint3D::SetFirstAnchor(const Vector3f& firstAnchor)
+	{
+		GetConstraint<btPoint2PointConstraint>()->setPivotA(ToBullet(firstAnchor));
+	}
+
+	void PivotConstraint3D::SetSecondAnchor(const Vector3f& secondAnchor)
+	{
+		GetConstraint<btPoint2PointConstraint>()->setPivotB(ToBullet(secondAnchor));
+	}
+}

src/Nazara/Physics3D/RigidBody3D.cpp

@@ -147,11 +147,6 @@ namespace Nz
 		return FromBullet(m_body->getAngularVelocity());
 	}
 
-	const std::shared_ptr<Collider3D>& RigidBody3D::GetGeom() const
-	{
-		return m_geom;
-	}
-
 	float RigidBody3D::GetLinearDamping() const
 	{
 		return m_body->getLinearDamping();
@@ -182,21 +177,11 @@ namespace Nz
 		return FromBullet(m_body->getWorldTransform().getOrigin());
 	}
 
-	btRigidBody* RigidBody3D::GetRigidBody() const
-	{
-		return m_body;
-	}
-
 	Quaternionf RigidBody3D::GetRotation() const
 	{
 		return FromBullet(m_body->getWorldTransform().getRotation());
 	}
 
-	PhysWorld3D* RigidBody3D::GetWorld() const
-	{
-		return m_world;
-	}
-
 	bool RigidBody3D::IsSimulationEnabled() const
 	{
 		return m_body->isActive();
@@ -290,12 +275,30 @@ namespace Nz
 		m_body->setWorldTransform(worldTransform);
 	}
 
+	Quaternionf RigidBody3D::ToLocal(const Quaternionf& worldRotation)
+	{
+		return GetRotation().Conjugate() * worldRotation;
+	}
+
+	Vector3f RigidBody3D::ToLocal(const Vector3f& worldPosition)
+	{
+		btTransform worldTransform = m_body->getWorldTransform();
+		return GetMatrix().InverseTransform() * worldPosition;
+	}
+
+	Quaternionf RigidBody3D::ToWorld(const Quaternionf& localRotation)
+	{
+		return GetRotation() * localRotation;
+	}
+
+	Vector3f RigidBody3D::ToWorld(const Vector3f& localPosition)
+	{
+		return GetMatrix() * localPosition;
+	}
+
 	void RigidBody3D::WakeUp()
 	{
-		m_body->setDeactivationTime(0);
+		m_body->activate();
-
-		if (m_body->getActivationState() == ISLAND_SLEEPING)
-			m_body->setActivationState(ACTIVE_TAG);
 	}
 
 	RigidBody3D& RigidBody3D::operator=(RigidBody3D&& object) noexcept