diff --git a/include/Nazara/Physics3D/PhysWorld3D.hpp b/include/Nazara/Physics3D/PhysWorld3D.hpp index 7e37faf2f..c07562c28 100644 --- a/include/Nazara/Physics3D/PhysWorld3D.hpp +++ b/include/Nazara/Physics3D/PhysWorld3D.hpp @@ -12,18 +12,26 @@ #include #include #include +#include #include class btDynamicsWorld; +class btRigidBody; namespace Nz { + class RigidBody3D; + class NAZARA_PHYSICS3D_API PhysWorld3D { + friend RigidBody3D; + public: + struct RaycastHit; + PhysWorld3D(); PhysWorld3D(const PhysWorld3D&) = delete; - PhysWorld3D(PhysWorld3D&& ph) noexcept; + PhysWorld3D(PhysWorld3D&& ph) = delete; ~PhysWorld3D(); btDynamicsWorld* GetDynamicsWorld(); @@ -38,9 +46,20 @@ namespace Nz void Step(Time timestep); PhysWorld3D& operator=(const PhysWorld3D&) = delete; - PhysWorld3D& operator=(PhysWorld3D&&) noexcept; + PhysWorld3D& operator=(PhysWorld3D&&) = delete; + + struct RaycastHit + { + float fraction; + RigidBody3D* hitBody; + Vector3f hitPosition; + Vector3f hitNormal; + }; private: + btRigidBody* AddRigidBody(std::size_t& rigidBodyIndex, FunctionRef constructor); + void RemoveRigidBody(btRigidBody* rigidBody, std::size_t rigidBodyIndex); + struct BulletWorld; std::size_t m_maxStepCount; diff --git a/include/Nazara/Physics3D/RigidBody3D.hpp b/include/Nazara/Physics3D/RigidBody3D.hpp index 7139b4bb1..76911587b 100644 --- a/include/Nazara/Physics3D/RigidBody3D.hpp +++ b/include/Nazara/Physics3D/RigidBody3D.hpp @@ -78,7 +78,8 @@ namespace Nz private: std::shared_ptr m_geom; - std::unique_ptr m_body; + std::size_t m_bodyPoolIndex; + btRigidBody* m_body; PhysWorld3D* m_world; }; } diff --git a/include/Nazara/Physics3D/RigidBody3D.inl b/include/Nazara/Physics3D/RigidBody3D.inl index 5b12ce65c..92b62f12d 100644 --- a/include/Nazara/Physics3D/RigidBody3D.inl +++ b/include/Nazara/Physics3D/RigidBody3D.inl @@ -2,7 +2,6 @@ // This file is part of the "Nazara Engine - Physics3D module" // For conditions of distribution and use, see copyright notice in Config.hpp -#include #include namespace Nz diff --git a/src/Nazara/Physics3D/PhysWorld3D.cpp b/src/Nazara/Physics3D/PhysWorld3D.cpp index b20f1ff80..003d4d60a 100644 --- a/src/Nazara/Physics3D/PhysWorld3D.cpp +++ b/src/Nazara/Physics3D/PhysWorld3D.cpp @@ -4,6 +4,7 @@ #include #include +#include #include #include #include @@ -22,10 +23,12 @@ namespace Nz btDbvtBroadphase broadphase; btSequentialImpulseConstraintSolver constraintSolver; btDiscreteDynamicsWorld dynamicWorld; + MemoryPool rigidBodyPool; BulletWorld() : dispatcher(&collisionConfiguration), - dynamicWorld(&dispatcher, &broadphase, &constraintSolver, &collisionConfiguration) + dynamicWorld(&dispatcher, &broadphase, &constraintSolver, &collisionConfiguration), + rigidBodyPool(256) { } @@ -45,8 +48,6 @@ namespace Nz m_world = std::make_unique(); } - PhysWorld3D::PhysWorld3D(PhysWorld3D&& physWorld) noexcept = default; - PhysWorld3D::~PhysWorld3D() = default; btDynamicsWorld* PhysWorld3D::GetDynamicsWorld() @@ -99,5 +100,37 @@ namespace Nz } } - PhysWorld3D& PhysWorld3D::operator=(PhysWorld3D&& physWorld) noexcept = default; + btRigidBody* PhysWorld3D::AddRigidBody(std::size_t& rigidBodyIndex, FunctionRef constructor) + { + btRigidBody* rigidBody = m_world->rigidBodyPool.Allocate(m_world->rigidBodyPool.DeferConstruct, rigidBodyIndex); + constructor(rigidBody); + + m_world->dynamicWorld.addRigidBody(rigidBody); + + // Small hack to order rigid bodies to make it cache friendly + auto& rigidBodies = m_world->dynamicWorld.getNonStaticRigidBodies(); + if (rigidBodies.size() >= 2 && rigidBodies[rigidBodies.size() - 1] == rigidBody) + { + // Sort rigid bodies + btRigidBody** startPtr = &rigidBodies[0]; + btRigidBody** endPtr = startPtr + rigidBodies.size(); + btRigidBody** lastPtr = endPtr - 1; + + auto it = std::lower_bound(startPtr, endPtr, rigidBody); + if (it != lastPtr) + { + std::move_backward(it, lastPtr, endPtr); + *it = rigidBody; + } + } + + return rigidBody; + } + + void PhysWorld3D::RemoveRigidBody(btRigidBody* rigidBody, std::size_t rigidBodyIndex) + { + // TODO: Improve deletion (since rigid bodies are sorted) + m_world->dynamicWorld.removeRigidBody(rigidBody); //< this does a linear search + m_world->rigidBodyPool.Free(rigidBodyIndex); + } } diff --git a/src/Nazara/Physics3D/RigidBody3D.cpp b/src/Nazara/Physics3D/RigidBody3D.cpp index 4bf13c716..77dca71c1 100644 --- a/src/Nazara/Physics3D/RigidBody3D.cpp +++ b/src/Nazara/Physics3D/RigidBody3D.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -32,13 +33,26 @@ namespace Nz m_geom->ComputeInertia(1.f, &inertia); btRigidBody::btRigidBodyConstructionInfo constructionInfo(1.f, nullptr, m_geom->GetShape(), ToBullet(inertia)); + constructionInfo.m_startWorldTransform = ToBullet(mat); - m_body = std::make_unique(constructionInfo); - - m_world->GetDynamicsWorld()->addRigidBody(m_body.get()); + m_body = m_world->AddRigidBody(m_bodyPoolIndex, [&](btRigidBody* body) + { + PlacementNew(body, constructionInfo); + }); + m_body->setUserPointer(this); } - RigidBody3D::RigidBody3D(RigidBody3D&& object) noexcept = default; + RigidBody3D::RigidBody3D(RigidBody3D&& object) noexcept : + m_geom(std::move(object.m_geom)), + m_bodyPoolIndex(object.m_bodyPoolIndex), + m_body(object.m_body), + m_world(object.m_world) + { + if (m_body) + m_body->setUserPointer(this); + + object.m_body = nullptr; + } RigidBody3D::~RigidBody3D() { @@ -170,7 +184,7 @@ namespace Nz btRigidBody* RigidBody3D::GetRigidBody() const { - return m_body.get(); + return m_body; } Quaternionf RigidBody3D::GetRotation() const @@ -288,9 +302,15 @@ namespace Nz { Destroy(); - m_body = std::move(object.m_body); - m_geom = std::move(object.m_geom); - m_world = object.m_world; + m_body = object.m_body; + m_bodyPoolIndex = object.m_bodyPoolIndex; + m_geom = std::move(object.m_geom); + m_world = object.m_world; + + if (m_body) + m_body->setUserPointer(this); + + object.m_body = nullptr; return *this; } @@ -299,8 +319,8 @@ namespace Nz { if (m_body) { - m_world->GetDynamicsWorld()->removeRigidBody(m_body.get()); - m_body.reset(); + m_world->RemoveRigidBody(m_body, m_bodyPoolIndex); + m_body = nullptr; } m_geom.reset();