Physics3D: Add raycast support

include/Nazara/Physics3D/PhysWorld3D.hpp

@@ -39,6 +39,8 @@ namespace Nz
 			std::size_t GetMaxStepCount() const;
 			Time GetStepSize() const;
 
+			bool RaycastQueryFirst(const Vector3f& from, const Vector3f& to, RaycastHit* hitInfo = nullptr);
+
 			void SetGravity(const Vector3f& gravity);
 			void SetMaxStepCount(std::size_t maxStepCount);
 			void SetStepSize(Time stepSize);
@@ -51,7 +53,7 @@ namespace Nz
 			struct RaycastHit
 			{
 				float fraction;
-				RigidBody3D* hitBody;
+				RigidBody3D* hitBody = nullptr;
 				Vector3f hitPosition;
 				Vector3f hitNormal;
 			};

include/Nazara/Physics3D/Systems/Physics3DSystem.hpp

@@ -13,6 +13,7 @@
 #include <Nazara/Physics3D/Components/RigidBody3DComponent.hpp>
 #include <NazaraUtils/TypeList.hpp>
 #include <entt/entt.hpp>
+#include <vector>
 
 namespace Nz
 {
@@ -22,6 +23,8 @@ namespace Nz
 			static constexpr Int64 ExecutionOrder = 0;
 			using Components = TypeList<RigidBody3DComponent, class NodeComponent>;
 
+			struct RaycastHit;
+
 			Physics3DSystem(entt::registry& registry);
 			Physics3DSystem(const Physics3DSystem&) = delete;
 			Physics3DSystem(Physics3DSystem&&) = delete;
@@ -32,16 +35,27 @@ namespace Nz
 			inline PhysWorld3D& GetPhysWorld();
 			inline const PhysWorld3D& GetPhysWorld() const;
 
+			bool RaycastQueryFirst(const Vector3f& from, const Vector3f& to, RaycastHit* hitInfo = nullptr);
+
 			void Update(Time elapsedTime);
 
 			Physics3DSystem& operator=(const Physics3DSystem&) = delete;
 			Physics3DSystem& operator=(Physics3DSystem&&) = delete;
 
-		private:
+			struct RaycastHit : PhysWorld3D::RaycastHit
-			static void OnConstruct(entt::registry& registry, entt::entity entity);
+			{
+				entt::handle hitEntity;
+			};
 
+		private:
+			void OnConstruct(entt::registry& registry, entt::entity entity);
+			void OnDestruct(entt::registry& registry, entt::entity entity);
+
+			std::vector<entt::entity> m_physicsEntities;
 			entt::registry& m_registry;
+			entt::observer m_physicsConstructObserver;
 			entt::scoped_connection m_constructConnection;
+			entt::scoped_connection m_destructConnection;
 			PhysWorld3D m_physWorld;
 	};
 }

src/Nazara/Physics3D/PhysWorld3D.cpp

@@ -70,6 +70,27 @@ namespace Nz
 		return m_stepSize;
 	}
 
+	bool PhysWorld3D::RaycastQueryFirst(const Vector3f& from, const Vector3f& to, RaycastHit* hitInfo)
+	{
+		btCollisionWorld::ClosestRayResultCallback callback(ToBullet(from), ToBullet(to));
+		m_world->dynamicWorld.rayTest(ToBullet(from), ToBullet(to), callback);
+
+		if (!callback.hasHit())
+			return false;
+
+		if (hitInfo)
+		{
+			hitInfo->fraction = callback.m_closestHitFraction;
+			hitInfo->hitNormal = FromBullet(callback.m_hitNormalWorld);
+			hitInfo->hitPosition = FromBullet(callback.m_hitPointWorld);
+
+			if (const btRigidBody* body = btRigidBody::upcast(callback.m_collisionObject))
+				hitInfo->hitBody = static_cast<RigidBody3D*>(body->getUserPointer());
+		}
+
+		return true;
+	}
+
 	void PhysWorld3D::SetGravity(const Vector3f& gravity)
 	{
 		m_world->dynamicWorld.setGravity(ToBullet(gravity));

src/Nazara/Physics3D/Systems/Physics3DSystem.cpp

@@ -9,24 +9,55 @@
 namespace Nz
 {
 	Physics3DSystem::Physics3DSystem(entt::registry& registry) :
-	m_registry(registry)
+	m_registry(registry),
+	m_physicsConstructObserver(m_registry, entt::collector.group<RigidBody3DComponent, NodeComponent>())
 	{
-		m_constructConnection = registry.on_construct<RigidBody3DComponent>().connect<OnConstruct>();
+		m_constructConnection = registry.on_construct<RigidBody3DComponent>().connect<&Physics3DSystem::OnConstruct>(this);
+		m_destructConnection = registry.on_destroy<RigidBody3DComponent>().connect<&Physics3DSystem::OnDestruct>(this);
 	}
 
 	Physics3DSystem::~Physics3DSystem()
 	{
+		m_physicsConstructObserver.disconnect();
+
 		// Ensure every RigidBody3D is destroyed before world is
 		auto rigidBodyView = m_registry.view<RigidBody3DComponent>();
 		for (auto [entity, rigidBodyComponent] : rigidBodyView.each())
 			rigidBodyComponent.Destroy();
 	}
 
+	bool Physics3DSystem::RaycastQueryFirst(const Vector3f& from, const Vector3f& to, RaycastHit* hitInfo)
+	{
+		if (!m_physWorld.RaycastQueryFirst(from, to, hitInfo))
+			return false;
+
+		if (hitInfo->hitBody)
+		{
+			std::size_t uniqueIndex = hitInfo->hitBody->GetUniqueIndex();
+			if (uniqueIndex < m_physicsEntities.size())
+				hitInfo->hitEntity = entt::handle(m_registry, m_physicsEntities[uniqueIndex]);
+		}
+
+		return true;
+	}
+
 	void Physics3DSystem::Update(Time elapsedTime)
 	{
+		// Move newly-created physics entities to their node position/rotation
+		m_physicsConstructObserver.each([&](entt::entity entity)
+		{
+			RigidBody3DComponent& entityPhysics = m_registry.get<RigidBody3DComponent>(entity);
+			NodeComponent& entityNode = m_registry.get<NodeComponent>(entity);
+
+			entityPhysics.SetPosition(entityNode.GetPosition(CoordSys::Global));
+			entityPhysics.SetRotation(entityNode.GetRotation(CoordSys::Global));
+		});
+
+		// Update the physics world
 		m_physWorld.Step(elapsedTime);
 
 		// Replicate rigid body position to their node components
+		// TODO: Only replicate active entities
 		auto view = m_registry.view<NodeComponent, const RigidBody3DComponent>();
 		for (auto [entity, nodeComponent, rigidBodyComponent] : view.each())
 		{
@@ -40,13 +71,22 @@ namespace Nz
 
 	void Physics3DSystem::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
+		// Register rigid body owning entity
-		NodeComponent* node = registry.try_get<NodeComponent>(entity);
-		if (node)
-		{
 		RigidBody3DComponent& rigidBody = registry.get<RigidBody3DComponent>(entity);
-			rigidBody.SetPosition(node->GetPosition(CoordSys::Global));
+		std::size_t uniqueIndex = rigidBody.GetUniqueIndex();
-			rigidBody.SetRotation(node->GetRotation(CoordSys::Global));
+		if (uniqueIndex >= m_physicsEntities.size())
+			m_physicsEntities.resize(uniqueIndex + 1);
+
+		m_physicsEntities[uniqueIndex] = entity;
 	}
+
+	void Physics3DSystem::OnDestruct(entt::registry& registry, entt::entity entity)
+	{
+		// Unregister owning entity
+		RigidBody3DComponent& rigidBody = registry.get<RigidBody3DComponent>(entity);
+		std::size_t uniqueIndex = rigidBody.GetUniqueIndex();
+		assert(uniqueIndex <= m_physicsEntities.size());
+
+		m_physicsEntities[uniqueIndex] = entt::null;
 	}
 }