JoltPhysics3D/JoltPhysWorld3D: Add CollisionQuery for points and shapes

include/Nazara/JoltPhysics3D/JoltPhysWorld3D.hpp

@@ -40,13 +40,19 @@ namespace Nz
 		friend JoltRigidBody3D;
 
 		public:
+			struct PointCollisionInfo;
 			struct RaycastHit;
+			struct ShapeCollisionInfo;
 
 			JoltPhysWorld3D();
 			JoltPhysWorld3D(const JoltPhysWorld3D&) = delete;
 			JoltPhysWorld3D(JoltPhysWorld3D&& ph) = delete;
 			~JoltPhysWorld3D();
 
+			bool CollisionQuery(const Vector3f& point, const FunctionRef<std::optional<float>(const PointCollisionInfo& collisionInfo)>& callback);
+			bool CollisionQuery(const JoltCollider3D& collider, const Matrix4f& colliderTransform, const FunctionRef<std::optional<float>(const ShapeCollisionInfo& hitInfo)>& callback);
+			bool CollisionQuery(const JoltCollider3D& collider, const Matrix4f& colliderTransform, const Vector3f& colliderScale, const FunctionRef<std::optional<float>(const ShapeCollisionInfo& hitInfo)>& callback);
+
 			UInt32 GetActiveBodyCount() const;
 			Vector3f GetGravity() const;
 			std::size_t GetMaxStepCount() const;
@@ -74,6 +80,11 @@ namespace Nz
 			JoltPhysWorld3D& operator=(const JoltPhysWorld3D&) = delete;
 			JoltPhysWorld3D& operator=(JoltPhysWorld3D&&) = delete;
 
+			struct PointCollisionInfo
+			{
+				JoltAbstractBody* hitBody = nullptr;
+			};
+
 			struct RaycastHit
 			{
 				float fraction;
@@ -82,6 +93,15 @@ namespace Nz
 				Vector3f hitPosition;
 			};
 
+			struct ShapeCollisionInfo
+			{
+				JoltAbstractBody* hitBody = nullptr;
+				Vector3f collisionPosition1;
+				Vector3f collisionPosition2;
+				Vector3f penetrationAxis;
+				float penetrationDepth;
+			};
+
 		private:
 			class BodyActivationListener;
 			friend BodyActivationListener;

include/Nazara/JoltPhysics3D/Systems/JoltPhysics3DSystem.hpp

@@ -25,13 +25,19 @@ namespace Nz
 			static constexpr Int64 ExecutionOrder = 0;
 			using Components = TypeList<JoltCharacterComponent, JoltRigidBody3DComponent, class NodeComponent>;
 
+			struct PointCollisionInfo;
 			struct RaycastHit;
+			struct ShapeCollisionInfo;
 
 			JoltPhysics3DSystem(entt::registry& registry);
 			JoltPhysics3DSystem(const JoltPhysics3DSystem&) = delete;
 			JoltPhysics3DSystem(JoltPhysics3DSystem&&) = delete;
 			~JoltPhysics3DSystem();
 
+			bool CollisionQuery(const Vector3f& point, const FunctionRef<std::optional<float>(const PointCollisionInfo& collisionInfo)>& callback);
+			bool CollisionQuery(const JoltCollider3D& collider, const Matrix4f& colliderTransform, const FunctionRef<std::optional<float>(const ShapeCollisionInfo& hitInfo)>& callback);
+			bool CollisionQuery(const JoltCollider3D& collider, const Matrix4f& colliderTransform, const Vector3f& colliderScale, const FunctionRef<std::optional<float>(const ShapeCollisionInfo& hitInfo)>& callback);
+
 			inline JoltPhysWorld3D& GetPhysWorld();
 			inline const JoltPhysWorld3D& GetPhysWorld() const;
 			inline entt::handle GetRigidBodyEntity(UInt32 bodyIndex) const;
@@ -44,11 +50,21 @@ namespace Nz
 			JoltPhysics3DSystem& operator=(const JoltPhysics3DSystem&) = delete;
 			JoltPhysics3DSystem& operator=(JoltPhysics3DSystem&&) = delete;
 
+			struct PointCollisionInfo : JoltPhysWorld3D::PointCollisionInfo
+			{
+				entt::handle hitEntity;
+			};
+
 			struct RaycastHit : JoltPhysWorld3D::RaycastHit
 			{
 				entt::handle hitEntity;
 			};
 
+			struct ShapeCollisionInfo : JoltPhysWorld3D::ShapeCollisionInfo
+			{
+				entt::handle hitEntity;
+			};
+
 		private:
 			void OnBodyConstruct(entt::registry& registry, entt::entity entity);
 			void OnBodyDestruct(entt::registry& registry, entt::entity entity);

src/Nazara/JoltPhysics3D/JoltPhysWorld3D.cpp

@@ -4,6 +4,7 @@
 
 #include <Nazara/JoltPhysics3D/JoltPhysWorld3D.hpp>
 #include <Nazara/JoltPhysics3D/JoltCharacter.hpp>
+#include <Nazara/JoltPhysics3D/JoltCollider3D.hpp>
 #include <Nazara/JoltPhysics3D/JoltHelper.hpp>
 #include <Nazara/JoltPhysics3D/JoltPhysics3D.hpp>
 #include <Nazara/JoltPhysics3D/JoltPhysicsStepListener.hpp>
@@ -16,6 +17,8 @@
 #include <Jolt/Physics/PhysicsSystem.h>
 #include <Jolt/Physics/Body/BodyActivationListener.h>
 #include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/CollidePointResult.h>
+#include <Jolt/Physics/Collision/CollideShape.h>
 #include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
 #include <Jolt/Physics/Collision/ObjectLayer.h>
 #include <Jolt/Physics/Collision/RayCast.h>
@@ -162,10 +165,106 @@ namespace Nz
 {
 	namespace NAZARA_ANONYMOUS_NAMESPACE
 	{
-		class CallbackHitResult : public JPH::CastRayCollector
+		class PointCallbackHitResult : public JPH::CollidePointCollector
 		{
 			public:
-				CallbackHitResult(const JPH::BodyLockInterface& bodyLockInterface, const Vector3f& from, const Vector3f& to, const FunctionRef<std::optional<float>(const JoltPhysWorld3D::RaycastHit& hitInfo)>& callback) :
+				PointCallbackHitResult(const JPH::BodyLockInterface& bodyLockInterface, const FunctionRef<std::optional<float>(const JoltPhysWorld3D::PointCollisionInfo& hitInfo)>& callback) :
+				m_bodyLockInterface(bodyLockInterface),
+				m_callback(callback),
+				m_didHit(false)
+				{
+				}
+
+				void AddHit(const JPH::CollidePointResult& result)
+				{
+					JoltPhysWorld3D::PointCollisionInfo hitInfo;
+
+					JPH::BodyLockWrite lock(m_bodyLockInterface, result.mBodyID);
+					if (!lock.Succeeded())
+						return; //< body was destroyed
+
+					JPH::Body& body = lock.GetBody();
+
+					hitInfo.hitBody = reinterpret_cast<JoltAbstractBody*>(static_cast<std::uintptr_t>(body.GetUserData()));
+
+					if (auto fractionOpt = m_callback(hitInfo))
+					{
+						float fraction = fractionOpt.value();
+						if (fraction > 0.f)
+						{
+							m_didHit = true;
+							UpdateEarlyOutFraction(fraction);
+						}
+						else
+							ForceEarlyOut();
+					}
+				}
+
+				bool DidHit() const
+				{
+					return m_didHit;
+				}
+
+			private:
+				const JPH::BodyLockInterface& m_bodyLockInterface;
+				const FunctionRef<std::optional<float>(const JoltPhysWorld3D::PointCollisionInfo& hitInfo)>& m_callback;
+				bool m_didHit;
+		};
+		
+		class ShapeCallbackHitResult : public JPH::CollideShapeCollector
+		{
+			public:
+				ShapeCallbackHitResult(const JPH::BodyLockInterface& bodyLockInterface, const FunctionRef<std::optional<float>(const JoltPhysWorld3D::ShapeCollisionInfo& hitInfo)>& callback) :
+				m_bodyLockInterface(bodyLockInterface),
+				m_callback(callback),
+				m_didHit(false)
+				{
+				}
+
+				void AddHit(const JPH::CollideShapeResult& result)
+				{
+					JoltPhysWorld3D::ShapeCollisionInfo hitInfo;
+					hitInfo.collisionPosition1 = FromJolt(result.mContactPointOn1);
+					hitInfo.collisionPosition2 = FromJolt(result.mContactPointOn2);
+					hitInfo.penetrationAxis = FromJolt(result.mPenetrationAxis);
+					hitInfo.penetrationDepth = result.mPenetrationDepth;
+
+					JPH::BodyLockWrite lock(m_bodyLockInterface, result.mBodyID2);
+					if (!lock.Succeeded())
+						return; //< body was destroyed
+
+					JPH::Body& body = lock.GetBody();
+
+					hitInfo.hitBody = reinterpret_cast<JoltAbstractBody*>(static_cast<std::uintptr_t>(body.GetUserData()));
+
+					if (auto fractionOpt = m_callback(hitInfo))
+					{
+						float fraction = fractionOpt.value();
+						if (fraction > 0.f)
+						{
+							m_didHit = true;
+							UpdateEarlyOutFraction(fraction);
+						}
+						else
+							ForceEarlyOut();
+					}
+				}
+
+				bool DidHit() const
+				{
+					return m_didHit;
+				}
+
+			private:
+				const JPH::BodyLockInterface& m_bodyLockInterface;
+				const FunctionRef<std::optional<float>(const JoltPhysWorld3D::ShapeCollisionInfo& hitInfo)>& m_callback;
+				bool m_didHit;
+		};
+
+		class RaycastCallbackHitResult : public JPH::CastRayCollector
+		{
+			public:
+				RaycastCallbackHitResult(const JPH::BodyLockInterface& bodyLockInterface, const Vector3f& from, const Vector3f& to, const FunctionRef<std::optional<float>(const JoltPhysWorld3D::RaycastHit& hitInfo)>& callback) :
 				m_bodyLockInterface(bodyLockInterface),
 				m_callback(callback),
 				m_from(from),
@@ -323,6 +422,35 @@ namespace Nz
 
 	JoltPhysWorld3D::~JoltPhysWorld3D() = default;
 
+	bool JoltPhysWorld3D::CollisionQuery(const Vector3f& point, const FunctionRef<std::optional<float>(const PointCollisionInfo& collisionInfo)>& callback)
+	{
+		NAZARA_USE_ANONYMOUS_NAMESPACE
+
+		PointCallbackHitResult collector(m_world->physicsSystem.GetBodyLockInterface(), callback);
+		m_world->physicsSystem.GetNarrowPhaseQuery().CollidePoint(ToJolt(point), collector);
+
+		return collector.DidHit();
+	}
+
+	bool JoltPhysWorld3D::CollisionQuery(const JoltCollider3D& collider, const Matrix4f& colliderTransform, const FunctionRef<std::optional<float>(const ShapeCollisionInfo& hitInfo)>& callback)
+	{
+		return CollisionQuery(collider, colliderTransform, Vector3f::Unit(), callback);
+	}
+
+	bool JoltPhysWorld3D::CollisionQuery(const JoltCollider3D& collider, const Matrix4f& colliderTransform, const Vector3f& colliderScale, const FunctionRef<std::optional<float>(const ShapeCollisionInfo& hitInfo)>& callback)
+	{
+		NAZARA_USE_ANONYMOUS_NAMESPACE
+
+		JPH::Shape* shape = collider.GetShapeSettings()->Create().Get();
+
+		JPH::CollideShapeSettings collideShapeSettings;
+
+		ShapeCallbackHitResult collector(m_world->physicsSystem.GetBodyLockInterface(), callback);
+		m_world->physicsSystem.GetNarrowPhaseQuery().CollideShape(shape, ToJolt(colliderScale), ToJolt(colliderTransform), collideShapeSettings, JPH::Vec3::sZero(), collector);
+
+		return collector.DidHit();
+	}
+
 	UInt32 JoltPhysWorld3D::GetActiveBodyCount() const
 	{
 		return m_world->physicsSystem.GetNumActiveBodies(JPH::EBodyType::RigidBody);
@@ -358,7 +486,7 @@ namespace Nz
 
 		JPH::RayCastSettings rayCastSettings;
 
-		CallbackHitResult collector(m_world->physicsSystem.GetBodyLockInterface(), from, to, callback);
+		RaycastCallbackHitResult collector(m_world->physicsSystem.GetBodyLockInterface(), from, to, callback);
 		m_world->physicsSystem.GetNarrowPhaseQuery().CastRay(rayCast, rayCastSettings, collector);
 
 		return collector.DidHit();

src/Nazara/JoltPhysics3D/Systems/JoltPhysics3DSystem.cpp

@@ -3,6 +3,7 @@
 // For conditions of distribution and use, see copyright notice in Config.hpp
 
 #include <Nazara/JoltPhysics3D/Systems/JoltPhysics3DSystem.hpp>
+#include <Nazara/JoltPhysics3D/JoltAbstractBody.hpp>
 #include <Nazara/Core/Components/DisabledComponent.hpp>
 #include <Nazara/Utility/Components/NodeComponent.hpp>
 #include <Nazara/JoltPhysics3D/Debug.hpp>
@@ -15,8 +16,9 @@ namespace Nz
 	m_rigidBodyConstructObserver(m_registry, entt::collector.group<JoltRigidBody3DComponent, NodeComponent>(entt::exclude<DisabledComponent, JoltCharacterComponent>))
 	{
 		m_bodyConstructConnection = registry.on_construct<JoltRigidBody3DComponent>().connect<&JoltPhysics3DSystem::OnBodyConstruct>(this);
-		m_characterConstructConnection = registry.on_construct<JoltCharacterComponent>().connect<&JoltPhysics3DSystem::OnCharacterConstruct>(this);
 		m_bodyDestructConnection = registry.on_destroy<JoltRigidBody3DComponent>().connect<&JoltPhysics3DSystem::OnBodyDestruct>(this);
+		m_characterConstructConnection = registry.on_construct<JoltCharacterComponent>().connect<&JoltPhysics3DSystem::OnCharacterConstruct>(this);
+		m_characterDestructConnection = registry.on_destroy<JoltCharacterComponent>().connect<&JoltPhysics3DSystem::OnCharacterDestruct>(this);
 	}
 
 	JoltPhysics3DSystem::~JoltPhysics3DSystem()
@@ -34,6 +36,47 @@ namespace Nz
 			rigidBodyComponent.Destroy(true);
 	}
 
+	bool JoltPhysics3DSystem::CollisionQuery(const Vector3f& point, const FunctionRef<std::optional<float>(const PointCollisionInfo& collisionInfo)>& callback)
+	{
+		return m_physWorld.CollisionQuery(point, [&](const JoltPhysWorld3D::PointCollisionInfo& hitInfo)
+		{
+			PointCollisionInfo extendedHitInfo;
+			static_cast<JoltPhysWorld3D::PointCollisionInfo&>(extendedHitInfo) = hitInfo;
+
+			if (extendedHitInfo.hitBody)
+			{
+				std::size_t bodyIndex = extendedHitInfo.hitBody->GetBodyIndex();
+				if (bodyIndex < m_bodyIndicesToEntity.size())
+					extendedHitInfo.hitEntity = entt::handle(m_registry, m_bodyIndicesToEntity[bodyIndex]);
+			}
+
+			return callback(extendedHitInfo);
+		});
+	}
+
+	bool JoltPhysics3DSystem::CollisionQuery(const JoltCollider3D& collider, const Matrix4f& shapeTransform, const FunctionRef<std::optional<float>(const ShapeCollisionInfo& hitInfo)>& callback)
+	{
+		return CollisionQuery(collider, shapeTransform, Vector3f::Unit(), callback);
+	}
+
+	bool JoltPhysics3DSystem::CollisionQuery(const JoltCollider3D& collider, const Matrix4f& colliderTransform, const Vector3f& colliderScale, const FunctionRef<std::optional<float>(const ShapeCollisionInfo& hitInfo)>& callback)
+	{
+		return m_physWorld.CollisionQuery(collider, colliderTransform, colliderScale, [&](const JoltPhysWorld3D::ShapeCollisionInfo& hitInfo)
+		{
+			ShapeCollisionInfo extendedHitInfo;
+			static_cast<JoltPhysWorld3D::ShapeCollisionInfo&>(extendedHitInfo) = hitInfo;
+
+			if (extendedHitInfo.hitBody)
+			{
+				std::size_t bodyIndex = extendedHitInfo.hitBody->GetBodyIndex();
+				if (bodyIndex < m_bodyIndicesToEntity.size())
+					extendedHitInfo.hitEntity = entt::handle(m_registry, m_bodyIndicesToEntity[bodyIndex]);
+			}
+
+			return callback(extendedHitInfo);
+		});
+	}
+
 	bool JoltPhysics3DSystem::RaycastQuery(const Vector3f& from, const Vector3f& to, const FunctionRef<std::optional<float>(const RaycastHit& hitInfo)>& callback)
 	{
 		return m_physWorld.RaycastQuery(from, to, [&](const JoltPhysWorld3D::RaycastHit& hitInfo)
@@ -139,12 +182,6 @@ namespace Nz
 		m_bodyIndicesToEntity[uniqueIndex] = entity;
 	}
 
-	void JoltPhysics3DSystem::OnCharacterConstruct(entt::registry& registry, entt::entity entity)
-	{
-		JoltCharacterComponent& character = registry.get<JoltCharacterComponent>(entity);
-		character.Construct(m_physWorld);
-	}
-
 	void JoltPhysics3DSystem::OnBodyDestruct(entt::registry& registry, entt::entity entity)
 	{
 		// Unregister owning entity
@@ -154,4 +191,26 @@ namespace Nz
 
 		m_bodyIndicesToEntity[uniqueIndex] = entt::null;
 	}
+
+	void JoltPhysics3DSystem::OnCharacterConstruct(entt::registry& registry, entt::entity entity)
+	{
+		JoltCharacterComponent& character = registry.get<JoltCharacterComponent>(entity);
+		character.Construct(m_physWorld);
+
+		UInt32 uniqueIndex = character.GetBodyIndex();
+		if (uniqueIndex >= m_bodyIndicesToEntity.size())
+			m_bodyIndicesToEntity.resize(uniqueIndex + 1);
+
+		m_bodyIndicesToEntity[uniqueIndex] = entity;
+	}
+
+	void JoltPhysics3DSystem::OnCharacterDestruct(entt::registry& registry, entt::entity entity)
+	{
+		// Unregister owning entity
+		JoltCharacterComponent& character = registry.get<JoltCharacterComponent>(entity);
+		UInt32 uniqueIndex = character.GetBodyIndex();
+		assert(uniqueIndex <= m_bodyIndicesToEntity.size());
+
+		m_bodyIndicesToEntity[uniqueIndex] = entt::null;
+	}
 }