Physics3D: Use pool for rigid bodies (+ sort them to improve cache)

include/Nazara/Physics3D/PhysWorld3D.hpp

@@ -12,18 +12,26 @@
 #include <Nazara/Math/Box.hpp>
 #include <Nazara/Math/Vector3.hpp>
 #include <Nazara/Physics3D/Config.hpp>
+#include <NazaraUtils/FunctionRef.hpp>
 #include <NazaraUtils/MovablePtr.hpp>
 
 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<void(btRigidBody* body)> constructor);
+			void RemoveRigidBody(btRigidBody* rigidBody, std::size_t rigidBodyIndex);
+
 			struct BulletWorld;
 
 			std::size_t m_maxStepCount;

include/Nazara/Physics3D/RigidBody3D.hpp

@@ -78,7 +78,8 @@ namespace Nz
 
 		private:
 			std::shared_ptr<Collider3D> m_geom;
-			std::unique_ptr<btRigidBody> m_body;
+			std::size_t m_bodyPoolIndex;
+			btRigidBody* m_body;
 			PhysWorld3D* m_world;
 	};
 }

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 <Nazara/Physics3D/RigidBody3D.hpp>
 #include <Nazara/Physics3D/Debug.hpp>
 
 namespace Nz

src/Nazara/Physics3D/PhysWorld3D.cpp

@@ -4,6 +4,7 @@
 
 #include <Nazara/Physics3D/PhysWorld3D.hpp>
 #include <Nazara/Physics3D/BulletHelper.hpp>
+#include <Nazara/Utils/MemoryPool.hpp>
 #include <Nazara/Utils/StackVector.hpp>
 #include <BulletCollision/BroadphaseCollision/btDbvtBroadphase.h>
 #include <BulletCollision/CollisionDispatch/btCollisionDispatcher.h>
@@ -22,10 +23,12 @@ namespace Nz
 		btDbvtBroadphase broadphase;
 		btSequentialImpulseConstraintSolver constraintSolver;
 		btDiscreteDynamicsWorld dynamicWorld;
+		MemoryPool<btRigidBody> 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<BulletWorld>();
 	}
 
-	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<void(btRigidBody* body)> 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);
+	}
 }

src/Nazara/Physics3D/RigidBody3D.cpp

@@ -5,6 +5,7 @@
 #include <Nazara/Physics3D/RigidBody3D.hpp>
 #include <Nazara/Physics3D/BulletHelper.hpp>
 #include <Nazara/Physics3D/PhysWorld3D.hpp>
+#include <Nazara/Utils/MemoryHelper.hpp>
 #include <BulletDynamics/Dynamics/btDynamicsWorld.h>
 #include <BulletDynamics/Dynamics/btRigidBody.h>
 #include <algorithm>
@@ -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<btRigidBody>(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();