Continue Jolt integration

src/Nazara/BulletPhysics3D/BulletCollider3D.cpp

@@ -10,7 +10,7 @@
 #include <Nazara/Utility/SoftwareBuffer.hpp>
 #include <Nazara/Utility/StaticMesh.hpp>
 #include <Nazara/Utility/VertexBuffer.hpp>
-#include <Nazara/Utils/MemoryHelper.hpp>
+#include <NazaraUtils/MemoryHelper.hpp>
 #include <tsl/ordered_map.h>
 #include <BulletCollision/CollisionShapes/btBoxShape.h>
 #include <BulletCollision/CollisionShapes/btCapsuleShape.h>

src/Nazara/BulletPhysics3D/BulletHelper.hpp

@@ -7,7 +7,7 @@
 #ifndef NAZARA_BULLETPHYSICS3D_BULLETHELPER_HPP
 #define NAZARA_BULLETPHYSICS3D_BULLETHELPER_HPP
 
-#include <Nazara/Prerequisites.hpp>
+#include <NazaraUtils/Prerequisites.hpp>
 #include <Nazara/BulletPhysics3D/Config.hpp>
 #include <Nazara/Math/Matrix4.hpp>
 #include <Nazara/Math/Quaternion.hpp>

src/Nazara/BulletPhysics3D/BulletPhysWorld3D.cpp

@@ -4,8 +4,8 @@
 
 #include <Nazara/BulletPhysics3D/BulletPhysWorld3D.hpp>
 #include <Nazara/BulletPhysics3D/BulletHelper.hpp>
-#include <Nazara/Utils/MemoryPool.hpp>
-#include <Nazara/Utils/StackVector.hpp>
+#include <NazaraUtils/MemoryPool.hpp>
+#include <NazaraUtils/StackVector.hpp>
 #include <BulletCollision/BroadphaseCollision/btDbvtBroadphase.h>
 #include <BulletCollision/CollisionDispatch/btCollisionDispatcher.h>
 #include <BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h>

src/Nazara/BulletPhysics3D/BulletRigidBody3D.cpp

@@ -5,7 +5,7 @@
 #include <Nazara/BulletPhysics3D/BulletRigidBody3D.hpp>
 #include <Nazara/BulletPhysics3D/BulletHelper.hpp>
 #include <Nazara/BulletPhysics3D/BulletPhysWorld3D.hpp>
-#include <Nazara/Utils/MemoryHelper.hpp>
+#include <NazaraUtils/MemoryHelper.hpp>
 #include <BulletDynamics/Dynamics/btDynamicsWorld.h>
 #include <BulletDynamics/Dynamics/btRigidBody.h>
 #include <algorithm>

src/Nazara/BulletPhysics3D/Systems/BulletPhysics3DSystem.cpp

@@ -38,6 +38,7 @@ namespace Nz
 
 		std::cout << "Physics time: " << (m_physicsTime / Time::Nanoseconds(m_stepCount)) << std::endl;
 		std::cout << "Update time: " << (m_updateTime / Time::Nanoseconds(m_stepCount)) << std::endl;
+		std::cout << "Active body count: " << m_activeObjectCount << std::endl;
 		std::cout << "--" << std::endl;
 
 		m_stepCount = 0;
@@ -81,14 +82,18 @@ namespace Nz
 
 		// Replicate rigid body position to their node components
 		// TODO: Only replicate active entities
+		m_activeObjectCount = 0;
+
 		auto view = m_registry.view<NodeComponent, const BulletRigidBody3DComponent>();
-		for (auto [entity, nodeComponent, rigidBodyComponent] : view.each())
+		for (auto entity : view)
 		{
+			auto& rigidBodyComponent = view.get<const BulletRigidBody3DComponent>(entity);
 			if (rigidBodyComponent.IsSleeping())
 				continue;
 
-			nodeComponent.SetPosition(rigidBodyComponent.GetPosition(), CoordSys::Global);
-			nodeComponent.SetRotation(rigidBodyComponent.GetRotation(), CoordSys::Global);
+			auto& nodeComponent = view.get<NodeComponent>(entity);
+			nodeComponent.SetTransform(rigidBodyComponent.GetPosition(), rigidBodyComponent.GetRotation());
+			m_activeObjectCount++;
 		}
 
 		Time t3 = GetElapsedNanoseconds();

src/Nazara/JoltPhysics3D/JoltCharacter.cpp

@@ -0,0 +1,78 @@
+// Copyright (C) 2023 Jérôme "Lynix" Leclercq (lynix680@gmail.com)
+// This file is part of the "Nazara Engine - JoltPhysics3D module"
+// For conditions of distribution and use, see copyright notice in Config.hpp
+
+#include <Nazara/JoltPhysics3D/JoltCharacter.hpp>
+#include <Nazara/JoltPhysics3D/JoltCollider3D.hpp>
+#include <Nazara/JoltPhysics3D/JoltHelper.hpp>
+#include <Nazara/JoltPhysics3D/JoltPhysWorld3D.hpp>
+#include <Jolt/Jolt.h>
+#include <Jolt/Physics/Collision/ObjectLayer.h>
+#include <Jolt/Physics/Character/Character.h>
+#include <Nazara/JoltPhysics3D/Debug.hpp>
+
+namespace Nz
+{
+	JoltCharacter::JoltCharacter(JoltPhysWorld3D& physWorld, std::shared_ptr<JoltCollider3D> collider, const Vector3f& position, const Quaternionf& rotation) :
+	m_collider(std::move(collider)),
+	m_physicsWorld(physWorld)
+	{
+		auto shapeResult = m_collider->GetShapeSettings()->Create();
+		if (!shapeResult.IsValid())
+			throw std::runtime_error("invalid shape");
+
+		JPH::CharacterSettings settings;
+		settings.mShape = shapeResult.Get();
+
+		m_character = std::make_unique<JPH::Character>(&settings, ToJolt(position), ToJolt(rotation), 0, m_physicsWorld.GetPhysicsSystem());
+		m_character->AddToPhysicsSystem();
+
+		m_physicsWorld.RegisterCharacter(this);
+	}
+
+	JoltCharacter::~JoltCharacter()
+	{
+		m_character->RemoveFromPhysicsSystem();
+
+		m_physicsWorld.UnregisterCharacter(this);
+	}
+
+	Vector3f JoltCharacter::GetLinearVelocity() const
+	{
+		return FromJolt(m_character->GetLinearVelocity());
+	}
+
+	Quaternionf JoltCharacter::GetRotation() const
+	{
+		return FromJolt(m_character->GetRotation());
+	}
+
+	Vector3f JoltCharacter::GetPosition() const
+	{
+		return FromJolt(m_character->GetPosition());
+	}
+
+	std::pair<Vector3f, Quaternionf> JoltCharacter::GetPositionAndRotation() const
+	{
+		JPH::Vec3 position;
+		JPH::Quat rotation;
+		m_character->GetPositionAndRotation(position, rotation);
+
+		return { FromJolt(position), FromJolt(rotation) };
+	}
+
+	bool JoltCharacter::IsOnGround() const
+	{
+		return m_character->IsSupported();
+	}
+
+	void JoltCharacter::SetLinearVelocity(const Vector3f& linearVel)
+	{
+		m_character->SetLinearVelocity(ToJolt(linearVel));
+	}
+
+	void JoltCharacter::PostSimulate()
+	{
+		m_character->PostSimulation(0.05f);
+	}
+}

src/Nazara/JoltPhysics3D/JoltCollider3D.cpp

@@ -10,15 +10,17 @@
 #include <Nazara/Utility/SoftwareBuffer.hpp>
 #include <Nazara/Utility/StaticMesh.hpp>
 #include <Nazara/Utility/VertexBuffer.hpp>
-#include <Nazara/Utils/MemoryHelper.hpp>
+#include <NazaraUtils/MemoryHelper.hpp>
 #include <Jolt/Core/Core.h>
 #include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
 #include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
 #include <Jolt/Physics/Collision/Shape/SphereShape.h>
 #include <Nazara/JoltPhysics3D/Debug.hpp>
 
 namespace Nz
 {
+	JoltCollider3D::JoltCollider3D() = default;
 	JoltCollider3D::~JoltCollider3D() = default;
 
 	std::shared_ptr<StaticMesh> JoltCollider3D::GenerateDebugMesh() const
@@ -85,14 +87,11 @@ namespace Nz
 
 	/********************************** BoxCollider3D **********************************/
 
-	JoltBoxCollider3D::JoltBoxCollider3D(const Vector3f& lengths, float convexRadius) :
-	m_lengths(lengths)
+	JoltBoxCollider3D::JoltBoxCollider3D(const Vector3f& lengths, float convexRadius)
 	{
-		m_shapeSettings = std::make_unique<JPH::BoxShapeSettings>(ToJolt(m_lengths * 0.5f), convexRadius);
+		SetupShapeSettings(std::make_unique<JPH::BoxShapeSettings>(ToJolt(lengths * 0.5f), convexRadius));
 	}
 
-	JoltBoxCollider3D::~JoltBoxCollider3D() = default;
-
 	void JoltBoxCollider3D::BuildDebugMesh(std::vector<Vector3f>& vertices, std::vector<UInt16>& indices, const Matrix4f& offsetMatrix) const
 	{
 		auto InsertVertex = [&](float x, float y, float z) -> UInt16
@@ -102,7 +101,7 @@ namespace Nz
 			return index;
 		};
 
-		Vector3f halfLengths = m_lengths * 0.5f;
+		Vector3f halfLengths = FromJolt(GetShapeSettingsAs<JPH::BoxShapeSettings>()->mHalfExtent);
 
 		UInt16 v0 = InsertVertex(-halfLengths.x, -halfLengths.y, -halfLengths.z);
 		UInt16 v1 = InsertVertex(halfLengths.x, -halfLengths.y, -halfLengths.z);
@@ -135,14 +134,9 @@ namespace Nz
 		InsertEdge(v3, v7);
 	}
 
-	JPH::ShapeSettings* JoltBoxCollider3D::GetShapeSettings() const
-	{
-		return m_shapeSettings.get();
-	}
-
 	Vector3f JoltBoxCollider3D::GetLengths() const
 	{
-		return m_lengths;
+		return FromJolt(GetShapeSettingsAs<JPH::BoxShapeSettings>()->mHalfExtent) * 2.f;
 	}
 
 	JoltColliderType3D JoltBoxCollider3D::GetType() const
@@ -155,12 +149,12 @@ namespace Nz
 	JoltCompoundCollider3D::JoltCompoundCollider3D(std::vector<ChildCollider> childs) :
 	m_childs(std::move(childs))
 	{
-		m_shapeSettings = std::make_unique<JPH::StaticCompoundShapeSettings>();
+		auto shapeSettings = std::make_unique<JPH::StaticCompoundShapeSettings>();
 		for (const auto& child : m_childs)
-			m_shapeSettings->AddShape(ToJolt(child.offset), ToJolt(child.rotation), child.collider->GetShapeSettings());
-	}
+			shapeSettings->AddShape(ToJolt(child.offset), ToJolt(child.rotation), child.collider->GetShapeSettings());
 
-	JoltCompoundCollider3D::~JoltCompoundCollider3D() = default;
+		SetupShapeSettings(std::move(shapeSettings));
+	}
 
 	void JoltCompoundCollider3D::BuildDebugMesh(std::vector<Vector3f>& vertices, std::vector<UInt16>& indices, const Matrix4f& offsetMatrix) const
 	{
@@ -173,11 +167,6 @@ namespace Nz
 		return m_childs;
 	}
 
-	JPH::ShapeSettings* JoltCompoundCollider3D::GetShapeSettings() const
-	{
-		return m_shapeSettings.get();
-	}
-
 	JoltColliderType3D JoltCompoundCollider3D::GetType() const
 	{
 		return JoltColliderType3D::Compound;
@@ -185,30 +174,40 @@ namespace Nz
 
 	/******************************** JoltSphereCollider3D *********************************/
 
-	JoltSphereCollider3D::JoltSphereCollider3D(float radius) :
-	m_radius(radius)
+	JoltSphereCollider3D::JoltSphereCollider3D(float radius)
 	{
-		m_shapeSettings = std::make_unique<JPH::SphereShapeSettings>(radius);
+		SetupShapeSettings(std::make_unique<JPH::SphereShapeSettings>(radius));
 	}
 
-	JoltSphereCollider3D::~JoltSphereCollider3D() = default;
-
 	void JoltSphereCollider3D::BuildDebugMesh(std::vector<Vector3f>& vertices, std::vector<UInt16>& indices, const Matrix4f& offsetMatrix) const
 	{
 	}
 
 	float JoltSphereCollider3D::GetRadius() const
 	{
-		return m_radius;
-	}
-
-	JPH::ShapeSettings* JoltSphereCollider3D::GetShapeSettings() const
-	{
-		return m_shapeSettings.get();
+		return GetShapeSettingsAs<JPH::SphereShapeSettings>()->mRadius;
 	}
 
 	JoltColliderType3D JoltSphereCollider3D::GetType() const
 	{
 		return JoltColliderType3D::Sphere;
 	}
+
+	JoltTranslatedRotatedCollider3D::JoltTranslatedRotatedCollider3D(std::shared_ptr<JoltCollider3D> collider, const Vector3f& translation, const Quaternionf& rotation) :
+	m_collider(std::move(collider))
+	{
+		SetupShapeSettings(std::make_unique<JPH::RotatedTranslatedShapeSettings>(ToJolt(translation), ToJolt(rotation), m_collider->GetShapeSettings()));
+	}
+
+	void JoltTranslatedRotatedCollider3D::BuildDebugMesh(std::vector<Vector3f>& vertices, std::vector<UInt16>& indices, const Matrix4f& offsetMatrix) const
+	{
+		const JPH::RotatedTranslatedShapeSettings* settings = GetShapeSettingsAs<JPH::RotatedTranslatedShapeSettings>();
+
+		m_collider->BuildDebugMesh(vertices, indices, offsetMatrix * Matrix4f::Transform(FromJolt(settings->mPosition), FromJolt(settings->mRotation)));
+	}
+
+	JoltColliderType3D JoltTranslatedRotatedCollider3D::GetType() const
+	{
+		return JoltColliderType3D::TranslatedRotatedDecoration;
+	}
 }

src/Nazara/JoltPhysics3D/JoltHelper.hpp

@@ -7,7 +7,7 @@
 #ifndef NAZARA_JOLTPHYSICS3D_JOLTHELPER_HPP
 #define NAZARA_JOLTPHYSICS3D_JOLTHELPER_HPP
 
-#include <Nazara/Prerequisites.hpp>
+#include <NazaraUtils/Prerequisites.hpp>
 #include <Nazara/BulletPhysics3D/Config.hpp>
 #include <Nazara/Math/Matrix4.hpp>
 #include <Nazara/Math/Quaternion.hpp>

src/Nazara/JoltPhysics3D/JoltPhysWorld3D.cpp

@@ -3,11 +3,12 @@
 // For conditions of distribution and use, see copyright notice in Config.hpp
 
 #include <Nazara/JoltPhysics3D/JoltPhysWorld3D.hpp>
+#include <Nazara/JoltPhysics3D/JoltCharacter.hpp>
 #include <Nazara/JoltPhysics3D/JoltHelper.hpp>
 #include <Nazara/JoltPhysics3D/JoltPhysics3D.hpp>
 #include <Nazara/JoltPhysics3D/JoltPhysWorld3D.hpp>
-#include <Nazara/Utils/MemoryPool.hpp>
-#include <Nazara/Utils/StackVector.hpp>
+#include <NazaraUtils/MemoryPool.hpp>
+#include <NazaraUtils/StackVector.hpp>
 #include <Jolt/Jolt.h>
 #include <Jolt/Core/TempAllocator.h>
 #include <Jolt/Physics/Collision/ObjectLayer.h>
@@ -152,37 +153,59 @@ namespace DitchMeAsap
 			cout << "A contact was removed" << endl;
 		}
 	};
-
-	// An example activation listener
-	class MyBodyActivationListener : public BodyActivationListener
-	{
-	public:
-		virtual void		OnBodyActivated(const BodyID& inBodyID, uint64 inBodyUserData) override
-		{
-			cout << "A body got activated" << endl;
-		}
-
-		virtual void		OnBodyDeactivated(const BodyID& inBodyID, uint64 inBodyUserData) override
-		{
-			cout << "A body went to sleep" << endl;
-		}
-	};
 }
 
 namespace Nz
 {
+	class JoltPhysWorld3D::BodyActivationListener : public JPH::BodyActivationListener
+	{
+		public:
+			static constexpr UInt32 BodyPerBlock = 64;
+
+			BodyActivationListener(JoltPhysWorld3D& physWorld) :
+			m_physWorld(physWorld)
+			{
+			}
+
+			void OnBodyActivated(const JPH::BodyID& inBodyID, UInt64 inBodyUserData) override
+			{
+				UInt32 bodyIndex = inBodyID.GetIndex();
+				UInt32 blockIndex = bodyIndex / 64;
+				UInt32 localIndex = bodyIndex % 64;
+
+				m_physWorld.m_activeBodies[blockIndex] |= UInt64(1u) << localIndex;
+			}
+
+			void OnBodyDeactivated(const JPH::BodyID& inBodyID, UInt64 inBodyUserData) override
+			{
+				UInt32 bodyIndex = inBodyID.GetIndex();
+				UInt32 blockIndex = bodyIndex / 64;
+				UInt32 localIndex = bodyIndex % 64;
+
+				m_physWorld.m_activeBodies[blockIndex] &= ~(UInt64(1u) << localIndex);
+			}
+
+		private:
+			JoltPhysWorld3D& m_physWorld;
+	};
+
 	struct JoltPhysWorld3D::JoltWorld
 	{
-		JPH::TempAllocatorMalloc tempAllocation;
+		JPH::TempAllocatorImpl tempAllocator;
 		JPH::PhysicsSystem physicsSystem;
 
+		JoltPhysWorld3D::BodyActivationListener bodyActivationListener;
+
 		DitchMeAsap::BPLayerInterfaceImpl layerInterface;
 		DitchMeAsap::ObjectLayerPairFilterImpl objectLayerFilter;
 		DitchMeAsap::ObjectVsBroadPhaseLayerFilterImpl objectBroadphaseLayerFilter;
-		DitchMeAsap::MyBodyActivationListener bodyActivationListener;
-		DitchMeAsap::MyContactListener contactListener;
 
-		JoltWorld() = default;
+		JoltWorld(JoltPhysWorld3D& world, JPH::uint tempAllocatorSize) :
+		tempAllocator(tempAllocatorSize),
+		bodyActivationListener(world)
+		{
+		}
+
 		JoltWorld(const JoltWorld&) = delete;
 		JoltWorld(JoltWorld&&) = delete;
 
@@ -196,14 +219,23 @@ namespace Nz
 	m_stepSize(Time::TickDuration(120)),
 	m_timestepAccumulator(Time::Zero())
 	{
-		m_world = std::make_unique<JoltWorld>();
-		m_world->physicsSystem.Init(0xFFFF, 0, 0xFFFF, 0xFFFF, m_world->layerInterface, m_world->objectBroadphaseLayerFilter, m_world->objectLayerFilter);
+		m_world = std::make_unique<JoltWorld>(*this, 10 * 1024 * 1024);
+		m_world->physicsSystem.Init(0xFFFF, 0, 0xFFFF, 10 * 1024, m_world->layerInterface, m_world->objectBroadphaseLayerFilter, m_world->objectLayerFilter);
 		m_world->physicsSystem.SetBodyActivationListener(&m_world->bodyActivationListener);
-		//m_world->physicsSystem.SetContactListener(&m_world->contactListener);
+
+		std::size_t blockCount = (m_world->physicsSystem.GetMaxBodies() - 1) / 64 + 1;
+		m_activeBodies = std::make_unique<std::atomic_uint64_t[]>(blockCount);
+		for (std::size_t i = 0; i < blockCount; ++i)
+			m_activeBodies[i] = 0;
 	}
 
 	JoltPhysWorld3D::~JoltPhysWorld3D() = default;
 
+	UInt32 JoltPhysWorld3D::GetActiveBodyCount() const
+	{
+		return m_world->physicsSystem.GetNumActiveBodies();
+	}
+
 	Vector3f JoltPhysWorld3D::GetGravity() const
 	{
 		return FromJolt(m_world->physicsSystem.GetGravity());
@@ -280,7 +312,11 @@ namespace Nz
 		std::size_t stepCount = 0;
 		while (m_timestepAccumulator >= m_stepSize && stepCount < m_maxStepCount)
 		{
-			m_world->physicsSystem.Update(stepSize, 1, 1, &m_world->tempAllocation, &jobSystem);
+			m_world->physicsSystem.Update(stepSize, 1, 1, &m_world->tempAllocator, &jobSystem);
+
+			for (JoltCharacter* character : m_characters)
+				character->PostSimulate();
+
 			m_timestepAccumulator -= m_stepSize;
 			stepCount++;
 		}

src/Nazara/JoltPhysics3D/JoltPhysics3D.cpp

@@ -40,14 +40,18 @@ namespace Nz
 		JPH::Factory::sInstance = new JPH::Factory;
 		JPH::RegisterTypes();
 
-		m_threadPool = std::make_unique<JPH::JobSystemThreadPool>(JPH::cMaxPhysicsJobs, JPH::cMaxPhysicsBarriers, /* Core::Instance()->GetHardwareInfo().GetCpuThreadCount()*/0);
+		int threadCount = -1; //< system CPU core count
+#ifdef NAZARA_PLATFORM_WEB
+		threadCount = 0; // no thread on web for now
+#endif
+
+		m_threadPool = std::make_unique<JPH::JobSystemThreadPool>(JPH::cMaxPhysicsJobs, JPH::cMaxPhysicsBarriers, threadCount);
 	}
 
 	JoltPhysics3D::~JoltPhysics3D()
 	{
 		m_threadPool.reset();
-		// FIXME: Uncomment when next version of Jolt gets released
-		//JPH::UnregisterTypes();
+		JPH::UnregisterTypes();
 
 		delete JPH::Factory::sInstance;
 		JPH::Factory::sInstance = nullptr;

src/Nazara/JoltPhysics3D/JoltRigidBody3D.cpp

@@ -6,7 +6,7 @@
 #include <Nazara/JoltPhysics3D/JoltHelper.hpp>
 #include <Nazara/JoltPhysics3D/JoltPhysWorld3D.hpp>
 #include <Nazara/JoltPhysics3D/JoltRigidBody3D.hpp>
-#include <Nazara/Utils/MemoryHelper.hpp>
+#include <NazaraUtils/MemoryHelper.hpp>
 #include <Jolt/Jolt.h>
 #include <Jolt/Physics/PhysicsSystem.h>
 #include <Jolt/Physics/Body/BodyCreationSettings.h>
@@ -29,28 +29,32 @@ namespace Nz
 		NazaraAssert(m_world, "Invalid world");
 
 		if (!m_geom)
-			m_geom = std::make_shared<JoltSphereCollider3D>(1.f);
+			m_geom = std::make_shared<JoltSphereCollider3D>(std::numeric_limits<float>::epsilon());
 
-		JPH::BodyInterface& body_interface = m_world->GetPhysicsSystem()->GetBodyInterface();
+		JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterface();
 
 		JPH::Vec3 position = ToJolt(mat.GetTranslation());
 		JPH::Quat rotation = ToJolt(mat.GetRotation().Normalize());
 
 		JPH::BodyCreationSettings settings(m_geom->GetShapeSettings(), position, rotation, JPH::EMotionType::Dynamic, 1);
 
-		JPH::Body* body = body_interface.CreateBody(settings);
-		body->SetUserData(SafeCast<UInt64>(reinterpret_cast<std::uintptr_t>(this)));
+		m_body = bodyInterface.CreateBody(settings);
+		m_body->SetUserData(SafeCast<UInt64>(reinterpret_cast<std::uintptr_t>(this)));
 
-		body_interface.AddBody(body->GetID(), JPH::EActivation::Activate);
-		m_bodyIndex = body->GetID().GetIndexAndSequenceNumber();
+		JPH::BodyID bodyId = m_body->GetID();
+
+		bodyInterface.AddBody(bodyId, JPH::EActivation::Activate);
+		m_bodyIndex = bodyId.GetIndex();
 	}
 
 	JoltRigidBody3D::JoltRigidBody3D(JoltRigidBody3D&& object) noexcept :
 	m_geom(std::move(object.m_geom)),
+	m_body(object.m_body),
 	m_bodyIndex(object.m_bodyIndex),
 	m_world(object.m_world)
 	{
-		object.m_bodyIndex = JPH::BodyID::cInvalidBodyID;
+		object.m_body = nullptr;
+		object.m_bodyIndex = std::numeric_limits<UInt32>::max();
 	}
 
 	JoltRigidBody3D::~JoltRigidBody3D()
@@ -64,16 +68,12 @@ namespace Nz
 		{
 			case CoordSys::Global:
 			{
-				JPH::BodyInterface& body_interface = m_world->GetPhysicsSystem()->GetBodyInterface();
-				body_interface.AddForce(JPH::BodyID(m_bodyIndex), ToJolt(force));
-				//WakeUp();
-				//m_body->applyCentralForce(ToJolt(force));
+				m_body->AddForce(ToJolt(force));
 				break;
 			}
 
 			case CoordSys::Local:
-				//WakeUp();
-				//m_body->applyCentralForce(ToJolt(GetRotation() * force));
+				m_body->AddForce(m_body->GetRotation() * ToJolt(force));
 				break;
 		}
 	}
@@ -115,13 +115,7 @@ namespace Nz
 #endif
 	void JoltRigidBody3D::EnableSleeping(bool enable)
 	{
-		auto& body_interface = m_world->GetPhysicsSystem()->GetBodyLockInterface();
-		JPH::BodyLockWrite lock(body_interface, JPH::BodyID(m_bodyIndex));
-		if (lock.Succeeded())
-		{
-			JPH::Body& body = lock.GetBody();
-			body.SetAllowSleeping(enable);
-		}
+		m_body->SetAllowSleeping(enable);
 	}
 
 #if 0
@@ -162,13 +156,7 @@ namespace Nz
 
 	float JoltRigidBody3D::GetMass() const
 	{
-		auto& body_interface = m_world->GetPhysicsSystem()->GetBodyLockInterface();
-		JPH::BodyLockRead lock(body_interface, JPH::BodyID(m_bodyIndex));
-		if (!lock.Succeeded())
-			return 0.f;
-
-		const JPH::Body& body = lock.GetBody();
-		return 1.f / body.GetMotionProperties()->GetInverseMass();
+		return 1.f / m_body->GetMotionProperties()->GetInverseMass();
 	}
 #if 0
 	Vector3f JoltRigidBody3D::GetMassCenter(CoordSys coordSys) const
@@ -184,14 +172,20 @@ namespace Nz
 
 	Vector3f JoltRigidBody3D::GetPosition() const
 	{
-		JPH::BodyInterface& body_interface = m_world->GetPhysicsSystem()->GetBodyInterface();
-		return FromJolt(body_interface.GetPosition(JPH::BodyID(m_bodyIndex)));
+		return FromJolt(m_body->GetPosition());
+	}
+
+	std::pair<Vector3f, Quaternionf> JoltRigidBody3D::GetPositionAndRotation() const
+	{
+		JPH::Vec3 position = m_body->GetPosition();
+		JPH::Quat rotation = m_body->GetRotation();
+
+		return { FromJolt(position), FromJolt(rotation) };
 	}
 
 	Quaternionf JoltRigidBody3D::GetRotation() const
 	{
-		JPH::BodyInterface& body_interface = m_world->GetPhysicsSystem()->GetBodyInterface();
-		return FromJolt(body_interface.GetRotation(JPH::BodyID(m_bodyIndex)));
+		return FromJolt(m_body->GetRotation());
 	}
 
 #if 0
@@ -203,19 +197,12 @@ namespace Nz
 
 	bool JoltRigidBody3D::IsSleeping() const
 	{
-		JPH::BodyInterface& body_interface = m_world->GetPhysicsSystem()->GetBodyInterface();
-		return !body_interface.IsActive(JPH::BodyID(m_bodyIndex));
+		return m_body->IsActive();
 	}
 
 	bool JoltRigidBody3D::IsSleepingEnabled() const
 	{
-		auto& body_interface = m_world->GetPhysicsSystem()->GetBodyLockInterface();
-		JPH::BodyLockRead lock(body_interface, JPH::BodyID(m_bodyIndex));
-		if (!lock.Succeeded())
-			return true;
-
-		const JPH::Body& body = lock.GetBody();
-		return body.GetAllowSleeping();
+		return m_body->GetAllowSleeping();
 	}
 
 #if 0
@@ -233,72 +220,79 @@ namespace Nz
 	{
 		if (m_geom != geom)
 		{
+			float mass = GetMass();
+
 			if (geom)
 				m_geom = std::move(geom);
 			else
-				m_geom = std::make_shared<JoltSphereCollider3D>(1.f);
+				m_geom = std::make_shared<JoltSphereCollider3D>(std::numeric_limits<float>::epsilon());
 
-			JPH::BodyInterface& body_interface = m_world->GetPhysicsSystem()->GetBodyInterface();
-			body_interface.SetShape(JPH::BodyID(m_bodyIndex), m_geom->GetShapeSettings()->Create().Get(), true, JPH::EActivation::Activate);
+			JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterface();
+			bodyInterface.SetShape(m_body->GetID(), m_geom->GetShapeSettings()->Create().Get(), false, JPH::EActivation::Activate);
+			if (recomputeInertia)
+			{
+				JPH::MassProperties massProperties = m_body->GetShape()->GetMassProperties();
+				massProperties.ScaleToMass(mass);
+				m_body->GetMotionProperties()->SetMassProperties(massProperties);
+			}
 		}
 	}
-#if 0
+
 	void JoltRigidBody3D::SetLinearDamping(float damping)
 	{
-		m_body->setDamping(damping, m_body->getAngularDamping());
+		m_body->GetMotionProperties()->SetLinearDamping(damping);
 	}
 
 	void JoltRigidBody3D::SetLinearVelocity(const Vector3f& velocity)
 	{
-		m_body->setLinearVelocity(ToJolt(velocity));
+		m_body->SetLinearVelocity(ToJolt(velocity));
 	}
-#endif 0
-	void JoltRigidBody3D::SetMass(float mass)
+
+	void JoltRigidBody3D::SetMass(float mass, bool recomputeInertia)
 	{
 		NazaraAssert(mass >= 0.f, "Mass must be positive and finite");
 		NazaraAssert(std::isfinite(mass), "Mass must be positive and finite");
 
-		auto& bodyLock = m_world->GetPhysicsSystem()->GetBodyLockInterface();
-		JPH::BodyLockWrite lock(bodyLock, JPH::BodyID(m_bodyIndex));
-		if (!lock.Succeeded())
-			return;
-
-		JPH::Body& body = lock.GetBody();
-
 		if (mass > 0.f)
 		{
-			body.SetMotionType(JPH::EMotionType::Dynamic);
-			body.GetMotionProperties()->SetInverseMass(1.f / mass);
+			m_body->SetMotionType(JPH::EMotionType::Dynamic);
+			if (recomputeInertia)
+			{
+				JPH::MassProperties massProperties = m_body->GetShape()->GetMassProperties();
+				massProperties.ScaleToMass(mass);
+				m_body->GetMotionProperties()->SetMassProperties(massProperties);
+			}
 		}
 		else
 		{
-			JPH::BodyInterface& body_interface = m_world->GetPhysicsSystem()->GetBodyInterfaceNoLock();
+			JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterfaceNoLock();
+			bodyInterface.DeactivateBody(m_body->GetID());
 
-			body_interface.DeactivateBody(body.GetID());
-			body.SetMotionType(JPH::EMotionType::Static);
+			m_body->SetMotionType(JPH::EMotionType::Static);
 		}
 	}
 
-#if 0
 	void JoltRigidBody3D::SetMassCenter(const Vector3f& center)
 	{
-		btTransform centerTransform;
-		centerTransform.setIdentity();
-		centerTransform.setOrigin(ToJolt(center));
-
-		m_body->setCenterOfMassTransform(centerTransform);
+		//m_body->GetMotionProperties()->set
 	}
-#endif 0
+
 	void JoltRigidBody3D::SetPosition(const Vector3f& position)
 	{
-		JPH::BodyInterface& body_interface = m_world->GetPhysicsSystem()->GetBodyInterface();
-		body_interface.SetPosition(JPH::BodyID(m_bodyIndex), ToJolt(position), JPH::EActivation::Activate);
+		JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterfaceNoLock();
+		bodyInterface.SetPosition(m_body->GetID(), ToJolt(position), JPH::EActivation::Activate);
 	}
 
 	void JoltRigidBody3D::SetRotation(const Quaternionf& rotation)
 	{
-		JPH::BodyInterface& body_interface = m_world->GetPhysicsSystem()->GetBodyInterface();
-		body_interface.SetRotation(JPH::BodyID(m_bodyIndex), ToJolt(rotation), JPH::EActivation::Activate);
+		JPH::BodyInterface& body_interface = m_world->GetPhysicsSystem()->GetBodyInterfaceNoLock();
+		body_interface.SetRotation(m_body->GetID(), ToJolt(rotation), JPH::EActivation::Activate);
+	}
+
+	void JoltRigidBody3D::TeleportTo(const Vector3f& position, const Quaternionf& rotation)
+	{
+		JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterfaceNoLock();
+		bodyInterface.SetPositionAndRotation(m_body->GetID(), ToJolt(position), ToJolt(rotation), JPH::EActivation::Activate);
 	}
 
 #if 0
@@ -327,30 +321,34 @@ namespace Nz
 	void JoltRigidBody3D::WakeUp()
 	{
 		JPH::BodyInterface& body_interface = m_world->GetPhysicsSystem()->GetBodyInterface();
-		body_interface.ActivateBody(JPH::BodyID(m_bodyIndex)); 
+		body_interface.ActivateBody(m_body->GetID()); 
 	}
 
 	JoltRigidBody3D& JoltRigidBody3D::operator=(JoltRigidBody3D&& object) noexcept
 	{
 		Destroy();
 
+		m_body          = object.m_body;
 		m_bodyIndex     = object.m_bodyIndex;
 		m_geom          = std::move(object.m_geom);
 		m_world         = object.m_world;
 
-		object.m_bodyIndex = JPH::BodyID::cInvalidBodyID;
+		object.m_body = nullptr;
+		object.m_bodyIndex = std::numeric_limits<UInt32>::max();
 
 		return *this;
 	}
 
 	void JoltRigidBody3D::Destroy()
 	{
-		if (m_bodyIndex != JPH::BodyID::cInvalidBodyID)
+		if (m_body)
 		{
-			JPH::BodyInterface& body_interface = m_world->GetPhysicsSystem()->GetBodyInterface();
-			body_interface.RemoveBody(JPH::BodyID(m_bodyIndex));
-			body_interface.DestroyBody(JPH::BodyID(m_bodyIndex));
-			m_bodyIndex = JPH::BodyID::cInvalidBodyID;
+			JPH::BodyID bodyId = m_body->GetID();
+
+			JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterface();
+			bodyInterface.RemoveBody(bodyId);
+			bodyInterface.DestroyBody(bodyId);
+			m_body = nullptr;
 		}
 
 		m_geom.reset();

src/Nazara/JoltPhysics3D/Systems/JoltPhysics3DSystem.cpp

@@ -37,7 +37,8 @@ namespace Nz
 			m_stepCount = 1;
 
 		std::cout << "Physics time: " << (m_physicsTime / Time::Nanoseconds(m_stepCount)) << std::endl;
-		std::cout << "Update time: " << (m_updateTime / Time::Nanoseconds(m_stepCount)) << std::endl;
+		std::cout << "Replication time: " << (m_updateTime / Time::Nanoseconds(m_stepCount)) << std::endl;
+		std::cout << "Active entity count: " << m_physWorld.GetActiveBodyCount() << std::endl;
 		std::cout << "--" << std::endl;
 
 		m_stepCount = 0;
@@ -68,8 +69,7 @@ namespace Nz
 			JoltRigidBody3DComponent& entityPhysics = m_registry.get<JoltRigidBody3DComponent>(entity);
 			NodeComponent& entityNode = m_registry.get<NodeComponent>(entity);
 
-			entityPhysics.SetPosition(entityNode.GetPosition(CoordSys::Global));
-			entityPhysics.SetRotation(entityNode.GetRotation(CoordSys::Global));
+			entityPhysics.TeleportTo(entityNode.GetPosition(), entityNode.GetRotation());
 		});
 
 		Time t1 = GetElapsedNanoseconds();
@@ -80,16 +80,18 @@ namespace Nz
 
 		Time t2 = GetElapsedNanoseconds();
 
-		// Replicate rigid body position to their node components
-		// TODO: Only replicate active entities
+		// Replicate active rigid body position to their node components
 		auto view = m_registry.view<NodeComponent, const JoltRigidBody3DComponent>();
-		for (auto [entity, nodeComponent, rigidBodyComponent] : view.each())
+		for (auto entity : view)
 		{
-			if (rigidBodyComponent.IsSleeping())
+			auto& rigidBodyComponent = view.get<const JoltRigidBody3DComponent>(entity);
+			if (!m_physWorld.IsBodyActive(rigidBodyComponent.GetBodyIndex()))
 				continue;
 
-			nodeComponent.SetPosition(rigidBodyComponent.GetPosition(), CoordSys::Global);
-			nodeComponent.SetRotation(rigidBodyComponent.GetRotation(), CoordSys::Global);
+			auto& nodeComponent = view.get<NodeComponent>(entity);
+
+			auto [position, rotation] = rigidBodyComponent.GetPositionAndRotation();
+			nodeComponent.SetTransform(position, rotation);
 		}
 
 		Time t3 = GetElapsedNanoseconds();
@@ -100,9 +102,22 @@ namespace Nz
 
 	void JoltPhysics3DSystem::OnConstruct(entt::registry& registry, entt::entity entity)
 	{
+		// Register rigid body owning entity
+		JoltRigidBody3DComponent& rigidBody = registry.get<JoltRigidBody3DComponent>(entity);
+		std::size_t uniqueIndex = rigidBody.GetBodyIndex();
+		if (uniqueIndex >= m_physicsEntities.size())
+			m_physicsEntities.resize(uniqueIndex + 1);
+
+		m_physicsEntities[uniqueIndex] = entity;
 	}
 
 	void JoltPhysics3DSystem::OnDestruct(entt::registry& registry, entt::entity entity)
 	{
+		// Unregister owning entity
+		JoltRigidBody3DComponent& rigidBody = registry.get<JoltRigidBody3DComponent>(entity);
+		std::size_t uniqueIndex = rigidBody.GetBodyIndex();
+		assert(uniqueIndex <= m_physicsEntities.size());
+
+		m_physicsEntities[uniqueIndex] = entt::null;
 	}
 }

src/Nazara/Utility/Node.cpp

@@ -262,6 +262,44 @@ namespace Nz
 		Invalidate(invalidation);
 	}
 
+	void Node::SetTransform(const Vector3f& position, const Quaternionf& rotation, CoordSys coordSys, Invalidation invalidation)
+	{
+		switch (coordSys)
+		{
+			case CoordSys::Global:
+			{
+				// Position
+				if (m_parent && m_inheritPosition)
+				{
+					m_parent->EnsureDerivedUpdate();
+
+					m_position = (m_parent->m_derivedRotation.GetConjugate() * (position - m_parent->m_derivedPosition)) / m_parent->m_derivedScale - m_initialPosition;
+				}
+				else
+					m_position = position - m_initialPosition;
+
+				// Rotation
+				if (m_parent && m_inheritRotation)
+				{
+					Quaternionf rot(m_parent->GetRotation() * m_initialRotation);
+
+					m_rotation = rot.GetConjugate() * rotation;
+				}
+				else
+					m_rotation = rotation;
+
+				break;
+			}
+
+			case CoordSys::Local:
+				m_position = position;
+				m_rotation = rotation;
+				break;
+		}
+
+		Invalidate(invalidation);
+	}
+	
 	void Node::SetTransform(const Vector3f& position, const Quaternionf& rotation, const Vector3f& scale, CoordSys coordSys, Invalidation invalidation)
 	{
 		switch (coordSys)