From 63d75e8904357784b3a8f9c188e936fc87195b2e Mon Sep 17 00:00:00 2001 From: SirLynix Date: Thu, 23 Mar 2023 13:16:06 +0100 Subject: [PATCH] Physics3D/PhysWorld3D: Add generic RaycastQuery --- .../BulletPhysics3D/BulletPhysWorld3D.hpp | 2 + .../Systems/BulletPhysics3DSystem.hpp | 1 + .../BulletPhysics3D/BulletPhysWorld3D.cpp | 54 ++++++++++++++++--- .../Systems/BulletPhysics3DSystem.cpp | 18 +++++++ 4 files changed, 68 insertions(+), 7 deletions(-) diff --git a/include/Nazara/BulletPhysics3D/BulletPhysWorld3D.hpp b/include/Nazara/BulletPhysics3D/BulletPhysWorld3D.hpp index 6ae12f812..43e553065 100644 --- a/include/Nazara/BulletPhysics3D/BulletPhysWorld3D.hpp +++ b/include/Nazara/BulletPhysics3D/BulletPhysWorld3D.hpp @@ -14,6 +14,7 @@ #include #include #include +#include class btDynamicsWorld; class btRigidBody; @@ -39,6 +40,7 @@ namespace Nz std::size_t GetMaxStepCount() const; Time GetStepSize() const; + bool RaycastQuery(const Vector3f& from, const Vector3f& to, const FunctionRef(const RaycastHit& hitInfo)>& callback); bool RaycastQueryFirst(const Vector3f& from, const Vector3f& to, RaycastHit* hitInfo = nullptr); void SetGravity(const Vector3f& gravity); diff --git a/include/Nazara/BulletPhysics3D/Systems/BulletPhysics3DSystem.hpp b/include/Nazara/BulletPhysics3D/Systems/BulletPhysics3DSystem.hpp index 0b58a4caf..f7deabd58 100644 --- a/include/Nazara/BulletPhysics3D/Systems/BulletPhysics3DSystem.hpp +++ b/include/Nazara/BulletPhysics3D/Systems/BulletPhysics3DSystem.hpp @@ -38,6 +38,7 @@ namespace Nz inline BulletPhysWorld3D& GetPhysWorld(); inline const BulletPhysWorld3D& GetPhysWorld() const; + bool RaycastQuery(const Vector3f& from, const Vector3f& to, const FunctionRef(const RaycastHit& hitInfo)>& callback); bool RaycastQueryFirst(const Vector3f& from, const Vector3f& to, RaycastHit* hitInfo = nullptr); void Update(Time elapsedTime); diff --git a/src/Nazara/BulletPhysics3D/BulletPhysWorld3D.cpp b/src/Nazara/BulletPhysics3D/BulletPhysWorld3D.cpp index 7e86f85fe..ef48ba357 100644 --- a/src/Nazara/BulletPhysics3D/BulletPhysWorld3D.cpp +++ b/src/Nazara/BulletPhysics3D/BulletPhysWorld3D.cpp @@ -16,6 +16,38 @@ namespace Nz { + class CallbackHitResult : public btCollisionWorld::RayResultCallback + { + public: + CallbackHitResult(const Vector3f& from, const Vector3f& to, const FunctionRef(const BulletPhysWorld3D::RaycastHit& hitInfo)>& callback) : + m_callback(callback), + m_from(from), + m_to(to) + { + } + + btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace) override + { + m_collisionObject = rayResult.m_collisionObject; + + BulletPhysWorld3D::RaycastHit hitInfo; + hitInfo.fraction = rayResult.m_hitFraction; + hitInfo.hitPosition = Lerp(m_from, m_to, rayResult.m_hitFraction); + hitInfo.hitNormal = FromBullet((normalInWorldSpace) ? rayResult.m_hitNormalLocal : m_collisionObject->getWorldTransform().getBasis() * rayResult.m_hitNormalLocal); + + if (const btRigidBody* body = btRigidBody::upcast(m_collisionObject)) + hitInfo.hitBody = static_cast(body->getUserPointer()); + + m_closestHitFraction = std::max(m_callback(hitInfo).value_or(m_closestHitFraction), 0.f); + return m_closestHitFraction; + } + + private: + const FunctionRef(const BulletPhysWorld3D::RaycastHit& hitInfo)>& m_callback; + Vector3f m_from; + Vector3f m_to; + }; + struct BulletPhysWorld3D::BulletWorld { btDefaultCollisionConfiguration collisionConfiguration; @@ -70,21 +102,29 @@ namespace Nz return m_stepSize; } + bool BulletPhysWorld3D::RaycastQuery(const Vector3f& from, const Vector3f& to, const FunctionRef(const RaycastHit& hitInfo)>& callback) + { + CallbackHitResult resultHandler(from, to, callback); + m_world->dynamicWorld.rayTest(ToBullet(from), ToBullet(to), resultHandler); + + return resultHandler.hasHit(); + } + bool BulletPhysWorld3D::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); + btCollisionWorld::ClosestRayResultCallback resultHandler(ToBullet(from), ToBullet(to)); + m_world->dynamicWorld.rayTest(ToBullet(from), ToBullet(to), resultHandler); - if (!callback.hasHit()) + if (!resultHandler.hasHit()) return false; if (hitInfo) { - hitInfo->fraction = callback.m_closestHitFraction; - hitInfo->hitNormal = FromBullet(callback.m_hitNormalWorld); - hitInfo->hitPosition = FromBullet(callback.m_hitPointWorld); + hitInfo->fraction = resultHandler.m_closestHitFraction; + hitInfo->hitNormal = FromBullet(resultHandler.m_hitNormalWorld); + hitInfo->hitPosition = FromBullet(resultHandler.m_hitPointWorld); - if (const btRigidBody* body = btRigidBody::upcast(callback.m_collisionObject)) + if (const btRigidBody* body = btRigidBody::upcast(resultHandler.m_collisionObject)) hitInfo->hitBody = static_cast(body->getUserPointer()); } diff --git a/src/Nazara/BulletPhysics3D/Systems/BulletPhysics3DSystem.cpp b/src/Nazara/BulletPhysics3D/Systems/BulletPhysics3DSystem.cpp index 767c74fce..d373d79b1 100644 --- a/src/Nazara/BulletPhysics3D/Systems/BulletPhysics3DSystem.cpp +++ b/src/Nazara/BulletPhysics3D/Systems/BulletPhysics3DSystem.cpp @@ -46,6 +46,24 @@ namespace Nz m_updateTime = Time::Zero(); } + bool BulletPhysics3DSystem::RaycastQuery(const Vector3f& from, const Vector3f& to, const FunctionRef(const RaycastHit& hitInfo)>& callback) + { + return m_physWorld.RaycastQuery(from, to, [&](const BulletPhysWorld3D::RaycastHit& hitInfo) + { + RaycastHit hitWithEntity; + static_cast(hitWithEntity) = hitInfo; + + if (hitWithEntity.hitBody) + { + std::size_t uniqueIndex = hitWithEntity.hitBody->GetUniqueIndex(); + if (uniqueIndex < m_physicsEntities.size()) + hitWithEntity.hitEntity = entt::handle(m_registry, m_physicsEntities[uniqueIndex]); + } + + return callback(hitWithEntity); + }); + } + bool BulletPhysics3DSystem::RaycastQueryFirst(const Vector3f& from, const Vector3f& to, RaycastHit* hitInfo) { if (!m_physWorld.RaycastQueryFirst(from, to, hitInfo))