From b3595178a1b83a1c3d31055ddf3a6a31e47088a3 Mon Sep 17 00:00:00 2001 From: SirLynix Date: Sat, 25 Mar 2023 13:53:44 +0100 Subject: [PATCH] JoltPhysics3D: Add RaycastQuery and RaycastQueryFirst --- examples/PhysicsPlayground/main.cpp | 17 ++-- .../Nazara/JoltPhysics3D/JoltPhysWorld3D.hpp | 4 +- .../Systems/JoltPhysics3DSystem.hpp | 3 +- src/Nazara/JoltPhysics3D/JoltPhysWorld3D.cpp | 99 ++++++++++++++++--- src/Nazara/JoltPhysics3D/JoltRigidBody3D.cpp | 4 + .../Systems/JoltPhysics3DSystem.cpp | 41 ++++++-- 6 files changed, 134 insertions(+), 34 deletions(-) diff --git a/examples/PhysicsPlayground/main.cpp b/examples/PhysicsPlayground/main.cpp index d67879509..dd6218403 100644 --- a/examples/PhysicsPlayground/main.cpp +++ b/examples/PhysicsPlayground/main.cpp @@ -299,16 +299,21 @@ int main() Nz::Vector3f from = cameraComponent.Unproject({ float(event.x), float(event.y), 0.f }); Nz::Vector3f to = cameraComponent.Unproject({ float(event.x), float(event.y), 1.f }); - /*/Nz::Physics3DSystem::RaycastHit lastHitInfo; - auto callback = [&](const Nz::Physics3DSystem::RaycastHit& hitInfo) -> std::optional + Nz::JoltPhysics3DSystem::RaycastHit lastHitInfo; + auto callback = [&](const Nz::JoltPhysics3DSystem::RaycastHit& hitInfo) -> std::optional { if (hitInfo.hitEntity == boxEntity) { Nz::Vector3f dirToCenter = Nz::Vector3f::Zero() - hitInfo.hitPosition; dirToCenter.Normalize(); +#ifdef USE_JOLT + if (Nz::Vector3f::DotProduct(dirToCenter, (hitInfo.hitPosition - from).Normalize()) < 0.f) + return std::nullopt; +#else if (Nz::Vector3f::DotProduct(dirToCenter, hitInfo.hitNormal) < 0.f) return std::nullopt; +#endif } lastHitInfo = hitInfo; @@ -320,8 +325,8 @@ int main() { if (lastHitInfo.hitBody && lastHitInfo.hitEntity != boxEntity) { - grabConstraint.emplace(*lastHitInfo.hitBody, lastHitInfo.hitPosition); - grabConstraint->SetImpulseClamp(30.f); + //grabConstraint.emplace(*lastHitInfo.hitBody, lastHitInfo.hitPosition); + //grabConstraint->SetImpulseClamp(30.f); grabbedObjectMove.Connect(eventHandler.OnMouseMoved, [&, body = lastHitInfo.hitBody, distance = Nz::Vector3f::Distance(from, lastHitInfo.hitPosition)](const Nz::WindowEventHandler*, const Nz::WindowEvent::MouseMoveEvent& event) { @@ -329,14 +334,14 @@ int main() Nz::Vector3f to = cameraComponent.Unproject({ float(event.x), float(event.y), 1.f }); Nz::Vector3f newPosition = from + (to - from).Normalize() * distance; - grabConstraint->SetSecondAnchor(newPosition); + //grabConstraint->SetSecondAnchor(newPosition); }); } else cameraMove.Connect(eventHandler.OnMouseMoved, mouseMoveCallback); } else - cameraMove.Connect(eventHandler.OnMouseMoved, mouseMoveCallback);*/ + cameraMove.Connect(eventHandler.OnMouseMoved, mouseMoveCallback); } }); diff --git a/include/Nazara/JoltPhysics3D/JoltPhysWorld3D.hpp b/include/Nazara/JoltPhysics3D/JoltPhysWorld3D.hpp index c47ffa194..6b78a0c67 100644 --- a/include/Nazara/JoltPhysics3D/JoltPhysWorld3D.hpp +++ b/include/Nazara/JoltPhysics3D/JoltPhysWorld3D.hpp @@ -47,7 +47,8 @@ namespace Nz inline bool IsBodyActive(UInt32 bodyIndex) const; - bool RaycastQueryFirst(const Vector3f& from, const Vector3f& to, RaycastHit* hitInfo = nullptr); + bool RaycastQuery(const Vector3f& from, const Vector3f& to, const FunctionRef(const RaycastHit& hitInfo)>& callback); + bool RaycastQueryFirst(const Vector3f& from, const Vector3f& to, const FunctionRef& callback); void SetGravity(const Vector3f& gravity); void SetMaxStepCount(std::size_t maxStepCount); @@ -63,7 +64,6 @@ namespace Nz float fraction; JoltRigidBody3D* hitBody = nullptr; Vector3f hitPosition; - Vector3f hitNormal; }; private: diff --git a/include/Nazara/JoltPhysics3D/Systems/JoltPhysics3DSystem.hpp b/include/Nazara/JoltPhysics3D/Systems/JoltPhysics3DSystem.hpp index 1c70246e4..e40a5fd28 100644 --- a/include/Nazara/JoltPhysics3D/Systems/JoltPhysics3DSystem.hpp +++ b/include/Nazara/JoltPhysics3D/Systems/JoltPhysics3DSystem.hpp @@ -38,7 +38,8 @@ namespace Nz inline JoltPhysWorld3D& GetPhysWorld(); inline const JoltPhysWorld3D& GetPhysWorld() const; - bool RaycastQueryFirst(const Vector3f& from, const Vector3f& to, RaycastHit* hitInfo = nullptr); + bool RaycastQuery(const Vector3f& from, const Vector3f& to, const FunctionRef(const RaycastHit& hitInfo)>& callback); + bool RaycastQueryFirst(const Vector3f& from, const Vector3f& to, const FunctionRef& callback); void Update(Time elapsedTime); diff --git a/src/Nazara/JoltPhysics3D/JoltPhysWorld3D.cpp b/src/Nazara/JoltPhysics3D/JoltPhysWorld3D.cpp index 2f884ba1d..f96d3cd33 100644 --- a/src/Nazara/JoltPhysics3D/JoltPhysWorld3D.cpp +++ b/src/Nazara/JoltPhysics3D/JoltPhysWorld3D.cpp @@ -11,6 +11,9 @@ #include #include #include +#include +#include +#include #include #include #include @@ -157,6 +160,59 @@ namespace DitchMeAsap namespace Nz { + namespace NAZARA_ANONYMOUS_NAMESPACE + { + class CallbackHitResult : public JPH::RayCastBodyCollector + { + public: + CallbackHitResult(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), + m_to(to), + m_didHit(false) + { + } + + void AddHit(const JPH::BroadPhaseCastResult& result) override + { + JoltPhysWorld3D::RaycastHit hitInfo; + hitInfo.fraction = result.mFraction; + hitInfo.hitPosition = Lerp(m_from, m_to, result.mFraction); + + JPH::BodyLockWrite lock(m_bodyLockInterface, result.mBodyID); + if (!lock.Succeeded()) + return; //< body was destroyed + + hitInfo.hitBody = reinterpret_cast(static_cast(lock.GetBody().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::RaycastHit& hitInfo)>& m_callback; + Vector3f m_from; + Vector3f m_to; + bool m_didHit; + }; + } + class JoltPhysWorld3D::BodyActivationListener : public JPH::BodyActivationListener { public: @@ -256,27 +312,40 @@ namespace Nz return m_stepSize; } - bool JoltPhysWorld3D::RaycastQueryFirst(const Vector3f& from, const Vector3f& to, RaycastHit* hitInfo) + bool JoltPhysWorld3D::RaycastQuery(const Vector3f& from, const Vector3f& to, const FunctionRef(const RaycastHit& hitInfo)>& callback) { - return false; - /* - btCollisionWorld::ClosestRayResultCallback callback(ToBullet(from), ToBullet(to)); - m_world->dynamicWorld.rayTest(ToBullet(from), ToBullet(to), callback); + JPH::RayCast rayCast; + rayCast.mDirection = ToJolt(to - from); + rayCast.mOrigin = ToJolt(from); - if (!callback.hasHit()) + CallbackHitResult collector(m_world->physicsSystem.GetBodyLockInterface(), from, to, callback); + m_world->physicsSystem.GetBroadPhaseQuery().CastRay(rayCast, collector); + + return collector.DidHit(); + } + + bool JoltPhysWorld3D::RaycastQueryFirst(const Vector3f& from, const Vector3f& to, const FunctionRef& callback) + { + JPH::RayCast rayCast; + rayCast.mDirection = ToJolt(to - from); + rayCast.mOrigin = ToJolt(from); + + JPH::ClosestHitCollisionCollector collector; + m_world->physicsSystem.GetBroadPhaseQuery().CastRay(rayCast, collector); + + if (!collector.HadHit()) return false; - if (hitInfo) - { - hitInfo->fraction = callback.m_closestHitFraction; - hitInfo->hitNormal = FromBullet(callback.m_hitNormalWorld); - hitInfo->hitPosition = FromBullet(callback.m_hitPointWorld); + JPH::BodyLockWrite lock(m_world->physicsSystem.GetBodyLockInterface(), collector.mHit.mBodyID); + if (!lock.Succeeded()) + return false; //< body was destroyed before lock - if (const btRigidBody* body = btRigidBody::upcast(callback.m_collisionObject)) - hitInfo->hitBody = static_cast(body->getUserPointer()); - } + RaycastHit hitInfo; + hitInfo.fraction = collector.mHit.GetEarlyOutFraction(); + hitInfo.hitPosition = Lerp(from, to, hitInfo.fraction); + hitInfo.hitBody = reinterpret_cast(static_cast(lock.GetBody().GetUserData())); - return true;*/ + return true; } void JoltPhysWorld3D::SetGravity(const Vector3f& gravity) diff --git a/src/Nazara/JoltPhysics3D/JoltRigidBody3D.cpp b/src/Nazara/JoltPhysics3D/JoltRigidBody3D.cpp index 30ee468cd..3c98e8f92 100644 --- a/src/Nazara/JoltPhysics3D/JoltRigidBody3D.cpp +++ b/src/Nazara/JoltPhysics3D/JoltRigidBody3D.cpp @@ -53,6 +53,8 @@ namespace Nz m_bodyIndex(object.m_bodyIndex), m_world(object.m_world) { + m_body->SetUserData(SafeCast(reinterpret_cast(this))); + object.m_body = nullptr; object.m_bodyIndex = std::numeric_limits::max(); } @@ -333,6 +335,8 @@ namespace Nz m_geom = std::move(object.m_geom); m_world = object.m_world; + m_body->SetUserData(SafeCast(reinterpret_cast(this))); + object.m_body = nullptr; object.m_bodyIndex = std::numeric_limits::max(); diff --git a/src/Nazara/JoltPhysics3D/Systems/JoltPhysics3DSystem.cpp b/src/Nazara/JoltPhysics3D/Systems/JoltPhysics3DSystem.cpp index 85f1f0aca..20d3902e4 100644 --- a/src/Nazara/JoltPhysics3D/Systems/JoltPhysics3DSystem.cpp +++ b/src/Nazara/JoltPhysics3D/Systems/JoltPhysics3DSystem.cpp @@ -46,19 +46,40 @@ namespace Nz m_updateTime = Time::Zero(); } - bool JoltPhysics3DSystem::RaycastQueryFirst(const Vector3f& from, const Vector3f& to, RaycastHit* hitInfo) + bool JoltPhysics3DSystem::RaycastQuery(const Vector3f& from, const Vector3f& to, const FunctionRef(const RaycastHit& hitInfo)>& callback) { - if (!m_physWorld.RaycastQueryFirst(from, to, hitInfo)) - return false; - - /*if (hitInfo->hitBody) + return m_physWorld.RaycastQuery(from, to, [&](const JoltPhysWorld3D::RaycastHit& hitInfo) { - std::size_t uniqueIndex = hitInfo->hitBody->GetUniqueIndex(); - if (uniqueIndex < m_physicsEntities.size()) - hitInfo->hitEntity = entt::handle(m_registry, m_physicsEntities[uniqueIndex]); - }*/ + RaycastHit extendedHitInfo; + static_cast(extendedHitInfo) = hitInfo; - return true; + if (extendedHitInfo.hitBody) + { + std::size_t bodyIndex = extendedHitInfo.hitBody->GetBodyIndex(); + if (bodyIndex < m_physicsEntities.size()) + extendedHitInfo.hitEntity = entt::handle(m_registry, m_physicsEntities[bodyIndex]); + } + + return callback(extendedHitInfo); + }); + } + + bool JoltPhysics3DSystem::RaycastQueryFirst(const Vector3f& from, const Vector3f& to, const FunctionRef& callback) + { + return m_physWorld.RaycastQueryFirst(from, to, [&](const JoltPhysWorld3D::RaycastHit& hitInfo) + { + RaycastHit extendedHitInfo; + static_cast(extendedHitInfo) = hitInfo; + + if (extendedHitInfo.hitBody) + { + std::size_t bodyIndex = extendedHitInfo.hitBody->GetBodyIndex(); + if (bodyIndex < m_physicsEntities.size()) + extendedHitInfo.hitEntity = entt::handle(m_registry, m_physicsEntities[bodyIndex]); + } + + callback(extendedHitInfo); + }); } void JoltPhysics3DSystem::Update(Time elapsedTime)