From 3fd696385d6504af538ea7f1dc7d7e8b5381c066 Mon Sep 17 00:00:00 2001 From: SirLynix Date: Thu, 7 Dec 2023 16:49:48 +0100 Subject: [PATCH] JoltPhysics3D/JoltPhysWorld3D: Add CollisionQuery for points and shapes --- .../Nazara/JoltPhysics3D/JoltPhysWorld3D.hpp | 20 +++ .../Systems/JoltPhysics3DSystem.hpp | 16 +++ src/Nazara/JoltPhysics3D/JoltPhysWorld3D.cpp | 134 +++++++++++++++++- .../Systems/JoltPhysics3DSystem.cpp | 73 +++++++++- 4 files changed, 233 insertions(+), 10 deletions(-) diff --git a/include/Nazara/JoltPhysics3D/JoltPhysWorld3D.hpp b/include/Nazara/JoltPhysics3D/JoltPhysWorld3D.hpp index 118985eb1..9154fad2a 100644 --- a/include/Nazara/JoltPhysics3D/JoltPhysWorld3D.hpp +++ b/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(const PointCollisionInfo& collisionInfo)>& callback); + bool CollisionQuery(const JoltCollider3D& collider, const Matrix4f& colliderTransform, const FunctionRef(const ShapeCollisionInfo& hitInfo)>& callback); + bool CollisionQuery(const JoltCollider3D& collider, const Matrix4f& colliderTransform, const Vector3f& colliderScale, const FunctionRef(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; diff --git a/include/Nazara/JoltPhysics3D/Systems/JoltPhysics3DSystem.hpp b/include/Nazara/JoltPhysics3D/Systems/JoltPhysics3DSystem.hpp index 900ad1db5..cf918993e 100644 --- a/include/Nazara/JoltPhysics3D/Systems/JoltPhysics3DSystem.hpp +++ b/include/Nazara/JoltPhysics3D/Systems/JoltPhysics3DSystem.hpp @@ -25,13 +25,19 @@ namespace Nz static constexpr Int64 ExecutionOrder = 0; using Components = TypeList; + 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(const PointCollisionInfo& collisionInfo)>& callback); + bool CollisionQuery(const JoltCollider3D& collider, const Matrix4f& colliderTransform, const FunctionRef(const ShapeCollisionInfo& hitInfo)>& callback); + bool CollisionQuery(const JoltCollider3D& collider, const Matrix4f& colliderTransform, const Vector3f& colliderScale, const FunctionRef(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); diff --git a/src/Nazara/JoltPhysics3D/JoltPhysWorld3D.cpp b/src/Nazara/JoltPhysics3D/JoltPhysWorld3D.cpp index 36957df30..dd8006def 100644 --- a/src/Nazara/JoltPhysics3D/JoltPhysWorld3D.cpp +++ b/src/Nazara/JoltPhysics3D/JoltPhysWorld3D.cpp @@ -4,6 +4,7 @@ #include #include +#include #include #include #include @@ -16,6 +17,8 @@ #include #include #include +#include +#include #include #include #include @@ -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(const JoltPhysWorld3D::RaycastHit& hitInfo)>& callback) : + PointCallbackHitResult(const JPH::BodyLockInterface& bodyLockInterface, const FunctionRef(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(static_cast(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(const JoltPhysWorld3D::PointCollisionInfo& hitInfo)>& m_callback; + bool m_didHit; + }; + + class ShapeCallbackHitResult : public JPH::CollideShapeCollector + { + public: + ShapeCallbackHitResult(const JPH::BodyLockInterface& bodyLockInterface, const FunctionRef(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(static_cast(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(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(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(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(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(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(); diff --git a/src/Nazara/JoltPhysics3D/Systems/JoltPhysics3DSystem.cpp b/src/Nazara/JoltPhysics3D/Systems/JoltPhysics3DSystem.cpp index 1508a68cf..868361590 100644 --- a/src/Nazara/JoltPhysics3D/Systems/JoltPhysics3DSystem.cpp +++ b/src/Nazara/JoltPhysics3D/Systems/JoltPhysics3DSystem.cpp @@ -3,6 +3,7 @@ // For conditions of distribution and use, see copyright notice in Config.hpp #include +#include #include #include #include @@ -15,8 +16,9 @@ namespace Nz m_rigidBodyConstructObserver(m_registry, entt::collector.group(entt::exclude)) { m_bodyConstructConnection = registry.on_construct().connect<&JoltPhysics3DSystem::OnBodyConstruct>(this); - m_characterConstructConnection = registry.on_construct().connect<&JoltPhysics3DSystem::OnCharacterConstruct>(this); m_bodyDestructConnection = registry.on_destroy().connect<&JoltPhysics3DSystem::OnBodyDestruct>(this); + m_characterConstructConnection = registry.on_construct().connect<&JoltPhysics3DSystem::OnCharacterConstruct>(this); + m_characterDestructConnection = registry.on_destroy().connect<&JoltPhysics3DSystem::OnCharacterDestruct>(this); } JoltPhysics3DSystem::~JoltPhysics3DSystem() @@ -34,6 +36,47 @@ namespace Nz rigidBodyComponent.Destroy(true); } + bool JoltPhysics3DSystem::CollisionQuery(const Vector3f& point, const FunctionRef(const PointCollisionInfo& collisionInfo)>& callback) + { + return m_physWorld.CollisionQuery(point, [&](const JoltPhysWorld3D::PointCollisionInfo& hitInfo) + { + PointCollisionInfo extendedHitInfo; + static_cast(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(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(const ShapeCollisionInfo& hitInfo)>& callback) + { + return m_physWorld.CollisionQuery(collider, colliderTransform, colliderScale, [&](const JoltPhysWorld3D::ShapeCollisionInfo& hitInfo) + { + ShapeCollisionInfo extendedHitInfo; + static_cast(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(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(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(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(entity); + UInt32 uniqueIndex = character.GetBodyIndex(); + assert(uniqueIndex <= m_bodyIndicesToEntity.size()); + + m_bodyIndicesToEntity[uniqueIndex] = entt::null; + } }