Add Physics2D components and systems (WIP)

src/Nazara/Physics2D/Components/RigidBody2DComponent.cpp

@@ -0,0 +1,10 @@
+// Copyright (C) 2022 Jérôme "Lynix" Leclercq (lynix680@gmail.com)
+// This file is part of the "Nazara Engine - Physics2D module"
+// For conditions of distribution and use, see copyright notice in Config.hpp
+
+#include <Nazara/Physics2D/Components/RigidBody2DComponent.hpp>
+#include <Nazara/Physics2D/Debug.hpp>
+
+namespace Nz
+{
+}

src/Nazara/Physics2D/RigidBody2D.cpp

@@ -620,6 +620,22 @@ namespace Nz
 		return *this;
 	}
 
+	void RigidBody2D::Destroy()
+	{
+		UnregisterFromSpace();
+
+		for (cpShape* shape : m_shapes)
+			cpShapeFree(shape);
+
+		if (m_handle)
+		{
+			cpBodyFree(m_handle);
+			m_handle = nullptr;
+		}
+
+		m_shapes.clear();
+	}
+
 	cpBody* RigidBody2D::Create(float mass, float moment)
 	{
 		cpBody* handle;
@@ -638,19 +654,6 @@ namespace Nz
 		return handle;
 	}
 
-	void RigidBody2D::Destroy()
-	{
-		UnregisterFromSpace();
-
-		for (cpShape* shape : m_shapes)
-			cpShapeFree(shape);
-
-		if (m_handle)
-			cpBodyFree(m_handle);
-
-		m_shapes.clear();
-	}
-
 	void RigidBody2D::RegisterToSpace()
 	{
 		if (!m_isRegistered)

src/Nazara/Physics2D/Systems/Physics2DSystem.cpp

@@ -0,0 +1,65 @@
+// Copyright (C) 2022 Jérôme "Lynix" Leclercq (lynix680@gmail.com)
+// This file is part of the "Nazara Engine - Physics2D module"
+// For conditions of distribution and use, see copyright notice in Config.hpp
+
+#include <Nazara/Physics2D/Systems/Physics2DSystem.hpp>
+#include <Nazara/Utility/Components/NodeComponent.hpp>
+#include <Nazara/Physics2D/Debug.hpp>
+
+namespace Nz
+{
+	namespace
+	{
+		inline Nz::RadianAnglef AngleFromQuaternion(const Nz::Quaternionf& quat)
+		{
+			float siny_cosp = 2.f * (quat.w * quat.z + quat.x * quat.y);
+			float cosy_cosp = 1.f - 2.f * (quat.y * quat.y + quat.z * quat.z);
+
+			return std::atan2(siny_cosp, cosy_cosp); //<FIXME: not very efficient
+		}
+	}
+
+	Physics2DSystem::Physics2DSystem(entt::registry& registry) :
+	m_registry(registry)
+	{
+		m_constructConnection = registry.on_construct<RigidBody2DComponent>().connect<OnConstruct>();
+	}
+
+	Physics2DSystem::~Physics2DSystem()
+	{
+		// Ensure every NewtonBody is destroyed before world is
+		auto rigidBodyView = m_registry.view<RigidBody2DComponent>();
+		for (auto [entity, rigidBodyComponent] : rigidBodyView.each())
+			rigidBodyComponent.Destroy();
+
+		m_constructConnection.release();
+	}
+
+	void Physics2DSystem::Update(entt::registry& registry, float elapsedTime)
+	{
+		m_physWorld.Step(elapsedTime);
+
+		// Replicate rigid body position to their node components
+		auto view = registry.view<NodeComponent, const RigidBody2DComponent>();
+		for (auto [entity, nodeComponent, rigidBodyComponent] : view.each())
+		{
+			if (rigidBodyComponent.IsSleeping())
+				continue;
+
+			nodeComponent.SetPosition(rigidBodyComponent.GetPosition(), CoordSys::Global);
+			nodeComponent.SetRotation(rigidBodyComponent.GetRotation(), CoordSys::Global);
+		}
+	}
+
+	void Physics2DSystem::OnConstruct(entt::registry& registry, entt::entity entity)
+	{
+		// If our entity already has a node component when adding a rigid body, initialize it with its position/rotation
+		NodeComponent* node = registry.try_get<NodeComponent>(entity);
+		if (node)
+		{
+			RigidBody2DComponent& rigidBody = registry.get<RigidBody2DComponent>(entity);
+			rigidBody.SetPosition(Vector2f(node->GetPosition()));
+			rigidBody.SetRotation(AngleFromQuaternion(node->GetRotation()));
+		}
+	}
+}