JoltPhysics3D: Rework RigidBody wrapper

- Add a clear way to setup dynamic/kinematic or static bodies
- Body registration to the world is batched (all bodies created before a physics step are added together, which is what Jolt is optimized for)
- Added support for empty shapes (= rigid bodies without collision) using a very small shape and tagging the body as sensor

src/Nazara/JoltPhysics3D/JoltCharacter.cpp

@@ -24,6 +24,7 @@ namespace Nz
 
 		JPH::CharacterSettings settings;
 		settings.mShape = shapeResult.Get();
+		settings.mLayer = 1;
 
 		m_character = std::make_unique<JPH::Character>(&settings, ToJolt(position), ToJolt(rotation), 0, m_physicsWorld.GetPhysicsSystem());
 		m_character->AddToPhysicsSystem();
@@ -108,7 +109,7 @@ namespace Nz
 		m_character->Activate(false);
 	}
 
-	void JoltCharacter::PreSimulate(float elapsedTime)
+	void JoltCharacter::PreSimulate(float /*elapsedTime*/)
 	{
 	}
 

src/Nazara/JoltPhysics3D/JoltConstraint3D.cpp

@@ -52,12 +52,12 @@ namespace Nz
 
 	JoltPhysWorld3D& JoltConstraint3D::GetWorld()
 	{
-		return *GetBodyA().GetWorld();
+		return GetBodyA().GetWorld();
 	}
 
 	const JoltPhysWorld3D& JoltConstraint3D::GetWorld() const
 	{
-		return *GetBodyA().GetWorld();
+		return GetBodyA().GetWorld();
 	}
 
 	bool JoltConstraint3D::IsSingleBody() const

src/Nazara/JoltPhysics3D/JoltPhysWorld3D.cpp

@@ -15,20 +15,19 @@
 #include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
 #include <Jolt/Physics/Collision/RayCast.h>
 #include <Jolt/Physics/Collision/ObjectLayer.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
 #include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h>
 #include <Jolt/Physics/PhysicsSettings.h>
 #include <Jolt/Physics/PhysicsSystem.h>
 #include <Jolt/Physics/PhysicsStepListener.h>
 #include <Jolt/Physics/Body/BodyActivationListener.h>
+#include <tsl/ordered_set.h>
 #include <cassert>
-#include <iostream>
 #include <Nazara/JoltPhysics3D/Debug.hpp>
 
 namespace DitchMeAsap
 {
 	using namespace JPH;
-	using std::cout;
-	using std::endl;
 
 	// Layer that objects can be in, determines which other objects it can collide with
 	// Typically you at least want to have 1 layer for moving bodies and 1 layer for static bodies, but you can have more
@@ -130,7 +129,7 @@ namespace DitchMeAsap
 	};
 
 	// An example contact listener
-	class MyContactListener : public ContactListener
+	/*class MyContactListener : public ContactListener
 	{
 	public:
 		// See: ContactListener
@@ -156,7 +155,7 @@ namespace DitchMeAsap
 		{
 			cout << "A contact was removed" << endl;
 		}
-	};
+	};*/
 }
 
 namespace Nz
@@ -227,7 +226,7 @@ namespace Nz
 			{
 			}
 
-			void OnBodyActivated(const JPH::BodyID& inBodyID, UInt64 inBodyUserData) override
+			void OnBodyActivated(const JPH::BodyID& inBodyID, UInt64 /*inBodyUserData*/) override
 			{
 				UInt32 bodyIndex = inBodyID.GetIndex();
 				UInt32 blockIndex = bodyIndex / 64;
@@ -236,7 +235,7 @@ namespace Nz
 				m_physWorld.m_activeBodies[blockIndex] |= UInt64(1u) << localIndex;
 			}
 
-			void OnBodyDeactivated(const JPH::BodyID& inBodyID, UInt64 inBodyUserData) override
+			void OnBodyDeactivated(const JPH::BodyID& inBodyID, UInt64 /*inBodyUserData*/) override
 			{
 				UInt32 bodyIndex = inBodyID.GetIndex();
 				UInt32 blockIndex = bodyIndex / 64;
@@ -268,8 +267,15 @@ namespace Nz
 
 	struct JoltPhysWorld3D::JoltWorld
 	{
+		using BodySet = tsl::ordered_set<JPH::BodyID, std::hash<JPH::BodyID>, std::equal_to<JPH::BodyID>, std::allocator<JPH::BodyID>, std::vector<JPH::BodyID>>;
+
 		JPH::TempAllocatorImpl tempAllocator;
 		JPH::PhysicsSystem physicsSystem;
+		BodySet pendingAdditionActivate;
+		BodySet pendingAdditionNoActivate;
+		BodySet pendingDeactivations;
+		std::vector<JPH::BodyID> tempBodyIDVec;
+		std::unique_ptr<JPH::SphereShape> nullShape;
 
 		JoltPhysWorld3D::BodyActivationListener bodyActivationListener;
 		JoltPhysWorld3D::StepListener stepListener;
@@ -305,9 +311,14 @@ namespace Nz
 		m_world->physicsSystem.AddStepListener(&m_world->stepListener);
 
 		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;
+
+		m_registeredBodies = std::make_unique<std::uint64_t[]>(blockCount);
+		for (std::size_t i = 0; i < blockCount; ++i)
+			m_registeredBodies[i] = 0;
 	}
 
 	JoltPhysWorld3D::~JoltPhysWorld3D() = default;
@@ -377,9 +388,57 @@ namespace Nz
 		hitInfo.hitBody = reinterpret_cast<JoltRigidBody3D*>(static_cast<std::uintptr_t>(body.GetUserData()));
 		hitInfo.hitNormal = FromJolt(body.GetWorldSpaceSurfaceNormal(collector.mHit.mSubShapeID2, rayCast.GetPointOnRay(collector.mHit.GetEarlyOutFraction())));
 
+		callback(hitInfo);
 		return true;
 	}
 
+	void JoltPhysWorld3D::RefreshBodies()
+	{
+		// Batch add bodies (keeps the broadphase efficient)
+		JPH::BodyInterface& bodyInterface = m_world->physicsSystem.GetBodyInterfaceNoLock();
+		auto AddBodies = [&](const JoltWorld::BodySet& bodies, JPH::EActivation activation)
+		{
+			for (const JPH::BodyID& bodyId : bodies)
+			{
+				UInt32 bodyIndex = bodyId.GetIndex();
+				UInt32 blockIndex = bodyIndex / 64;
+				UInt32 localIndex = bodyIndex % 64;
+
+				m_registeredBodies[blockIndex] |= UInt64(1u) << localIndex;
+			}
+
+			if (bodies.size() == 1)
+				bodyInterface.AddBody(bodies.front(), activation);
+			else
+			{
+				m_world->tempBodyIDVec.resize(bodies.size());
+				std::memcpy(&m_world->tempBodyIDVec[0], bodies.data(), bodies.size() * sizeof(JPH::BodyID));
+
+				JPH::BodyInterface::AddState addState = bodyInterface.AddBodiesPrepare(m_world->tempBodyIDVec.data(), SafeCast<int>(m_world->tempBodyIDVec.size()));
+				bodyInterface.AddBodiesFinalize(m_world->tempBodyIDVec.data(), SafeCast<int>(m_world->tempBodyIDVec.size()), addState, activation);
+			}
+		};
+
+		// Handle pending register/unregister bodies
+		if (!m_world->pendingAdditionActivate.empty())
+		{
+			AddBodies(m_world->pendingAdditionActivate, JPH::EActivation::Activate);
+			m_world->pendingAdditionActivate.clear();
+		}
+
+		if (!m_world->pendingAdditionNoActivate.empty())
+		{
+			AddBodies(m_world->pendingAdditionNoActivate, JPH::EActivation::DontActivate);
+			m_world->pendingAdditionNoActivate.clear();
+		}
+
+		if (!m_world->pendingDeactivations.empty())
+		{
+			bodyInterface.DeactivateBodies(m_world->pendingDeactivations.data(), SafeCast<int>(m_world->pendingDeactivations.size()));
+			m_world->pendingDeactivations.clear();
+		}
+	}
+
 	void JoltPhysWorld3D::SetGravity(const Vector3f& gravity)
 	{
 		m_world->physicsSystem.SetGravity(ToJolt(gravity));
@@ -397,18 +456,12 @@ namespace Nz
 
 	void JoltPhysWorld3D::Step(Time timestep)
 	{
+		RefreshBodies();
+
 		JPH::JobSystem& jobSystem = JoltPhysics3D::Instance()->GetThreadPool();
-
-		m_timestepAccumulator += timestep;
-
 		float stepSize = m_stepSize.AsSeconds<float>();
 
-		static bool firstStep = true;
-		if (firstStep)
-		{
-			m_world->physicsSystem.OptimizeBroadPhase();
-			firstStep = false;
-		}
+		m_timestepAccumulator += timestep;
 
 		std::size_t stepCount = 0;
 		while (m_timestepAccumulator >= m_stepSize && stepCount < m_maxStepCount)
@@ -423,6 +476,63 @@ namespace Nz
 		}
 	}
 
+	void JoltPhysWorld3D::RegisterBody(const JPH::BodyID& bodyID, bool activate, bool removeFromDeactivationList)
+	{
+		assert(removeFromDeactivationList || !m_world->pendingDeactivations.contains(bodyID));
+
+		auto& activationSet = (activate) ? m_world->pendingAdditionActivate : m_world->pendingAdditionNoActivate;
+		activationSet.insert(bodyID);
+
+		if (removeFromDeactivationList)
+		{
+			auto& otherActivationSet = (activate) ? m_world->pendingAdditionNoActivate : m_world->pendingAdditionActivate;
+			otherActivationSet.erase(bodyID);
+
+			m_world->pendingDeactivations.erase(bodyID);
+		}
+	}
+
+	void JoltPhysWorld3D::UnregisterBody(const JPH::BodyID& bodyID, bool destroy, bool removeFromActivationList)
+	{
+		if (destroy)
+		{
+			auto& bodyInterface = m_world->physicsSystem.GetBodyInterface();
+
+			UInt32 bodyIndex = bodyID.GetIndex();
+			if (IsBodyRegistered(bodyIndex))
+			{
+				UInt32 blockIndex = bodyIndex / 64;
+				UInt32 localIndex = bodyIndex % 64;
+
+				m_registeredBodies[blockIndex] &= ~(UInt64(1u) << localIndex);
+
+				bodyInterface.RemoveBody(bodyID);
+			}
+
+			bodyInterface.DestroyBody(bodyID);
+
+		}
+		else
+			m_world->pendingDeactivations.insert(bodyID);
+
+		if (removeFromActivationList)
+		{
+			m_world->pendingAdditionActivate.erase(bodyID);
+			m_world->pendingAdditionNoActivate.erase(bodyID);
+		}
+	}
+
+	const JPH::Shape* JoltPhysWorld3D::GetNullShape() const
+	{
+		if (!m_world->nullShape)
+		{
+			m_world->nullShape = std::make_unique<JPH::SphereShape>(std::numeric_limits<float>::epsilon());
+			m_world->nullShape->SetEmbedded();
+		}
+
+		return m_world->nullShape.get();
+	}
+
 	void JoltPhysWorld3D::OnPreStep(float deltatime)
 	{
 		for (JoltCharacter* character : m_characters)

src/Nazara/JoltPhysics3D/JoltRigidBody3D.cpp

@@ -17,46 +17,57 @@
 
 namespace Nz
 {
-	JoltRigidBody3D::JoltRigidBody3D(JoltPhysWorld3D* world, const Matrix4f& mat) :
-	JoltRigidBody3D(world, nullptr, mat)
+	JoltRigidBody3D::JoltRigidBody3D(JoltPhysWorld3D& world, const DynamicSettings& settings) :
+	m_geom(settings.geom),
+	m_world(&world),
+	m_isSimulationEnabled(false),
+	m_isTrigger(settings.isTrigger)
 	{
-	}
-
-	JoltRigidBody3D::JoltRigidBody3D(JoltPhysWorld3D* world, std::shared_ptr<JoltCollider3D> geom, const Matrix4f& mat) :
-	m_geom(std::move(geom)),
-	m_world(world)
-	{
-		NazaraAssert(m_world, "Invalid world");
-
-		if (!m_geom)
-			m_geom = std::make_shared<JoltSphereCollider3D>(std::numeric_limits<float>::epsilon());
-
 		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);
-
-		m_body = bodyInterface.CreateBody(settings);
-		m_body->SetUserData(SafeCast<UInt64>(reinterpret_cast<std::uintptr_t>(this)));
+		JPH::BodyCreationSettings creationSettings;
+		BuildSettings(settings, creationSettings);
 
+		m_body = bodyInterface.CreateBody(creationSettings);
 		JPH::BodyID bodyId = m_body->GetID();
-
-		bodyInterface.AddBody(bodyId, JPH::EActivation::Activate);
 		m_bodyIndex = bodyId.GetIndex();
+
+		if (settings.isSimulationEnabled)
+			m_world->RegisterBody(bodyId, !settings.initiallySleeping, false);
 	}
 
-	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)
+	JoltRigidBody3D::JoltRigidBody3D(JoltPhysWorld3D& world, const StaticSettings& settings) :
+	m_geom(settings.geom),
+	m_world(&world),
+	m_isSimulationEnabled(false),
+	m_isTrigger(settings.isTrigger)
 	{
-		m_body->SetUserData(SafeCast<UInt64>(reinterpret_cast<std::uintptr_t>(this)));
+		JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterface();
 
-		object.m_body = nullptr;
-		object.m_bodyIndex = std::numeric_limits<UInt32>::max();
+		JPH::BodyCreationSettings creationSettings;
+		BuildSettings(settings, creationSettings);
+
+		m_body = bodyInterface.CreateBody(creationSettings);
+		JPH::BodyID bodyId = m_body->GetID();
+		m_bodyIndex = bodyId.GetIndex();
+
+		if (settings.isSimulationEnabled)
+			m_world->RegisterBody(bodyId, false, false); //< static bodies cannot be activated
+	}
+
+	JoltRigidBody3D::JoltRigidBody3D(JoltRigidBody3D&& body) noexcept :
+	m_geom(std::move(body.m_geom)),
+	m_body(body.m_body),
+	m_world(body.m_world),
+	m_bodyIndex(body.m_bodyIndex),
+	m_isSimulationEnabled(body.m_isSimulationEnabled),
+	m_isTrigger(body.m_isTrigger)
+	{
+		body.m_body = nullptr;
+		body.m_bodyIndex = std::numeric_limits<UInt32>::max();
+
+		if (m_body)
+			m_body->SetUserData(SafeCast<UInt64>(reinterpret_cast<std::uintptr_t>(this)));
 	}
 
 	JoltRigidBody3D::~JoltRigidBody3D()
@@ -69,10 +80,8 @@ namespace Nz
 		switch (coordSys)
 		{
 			case CoordSys::Global:
-			{
 				m_body->AddForce(ToJolt(force));
 				break;
-			}
 
 			case CoordSys::Local:
 				m_body->AddForce(m_body->GetRotation() * ToJolt(force));
@@ -80,21 +89,17 @@ namespace Nz
 		}
 	}
 
-#if 0
 	void JoltRigidBody3D::AddForce(const Vector3f& force, const Vector3f& point, CoordSys coordSys)
 	{
 		switch (coordSys)
 		{
 			case CoordSys::Global:
-				WakeUp();
-				m_body->applyForce(ToJolt(force), ToJolt(point));
+				m_body->AddForce(ToJolt(force), ToJolt(point));
 				break;
 
 			case CoordSys::Local:
-			{
-				Matrix4f transformMatrix = GetMatrix();
-				return AddForce(transformMatrix.Transform(force, 0.f), point, CoordSys::Global);
-			}
+				m_body->AddForce(m_body->GetRotation() * ToJolt(force), ToJolt(ToWorld(point)));
+				break;
 		}
 	}
 
@@ -103,74 +108,83 @@ namespace Nz
 		switch (coordSys)
 		{
 			case CoordSys::Global:
-				WakeUp();
-				m_body->applyTorque(ToJolt(torque));
+				m_body->AddTorque(ToJolt(torque));
 				break;
 
 			case CoordSys::Local:
-				Matrix4f transformMatrix = GetMatrix();
-				WakeUp();
-				m_body->applyTorque(ToJolt(transformMatrix.Transform(torque, 0.f)));
+				m_body->AddTorque(m_body->GetRotation() * ToJolt(torque));
 				break;
 		}
 	}
-#endif
+
+	void JoltRigidBody3D::EnableSimulation(bool enable)
+	{
+		if (m_isSimulationEnabled == enable)
+			return;
+
+		if (enable)
+			m_world->RegisterBody(m_body->GetID(), true, true);
+		else
+			m_world->UnregisterBody(m_body->GetID(), false, true);
+
+		m_isSimulationEnabled = enable;
+	}
+
 	void JoltRigidBody3D::EnableSleeping(bool enable)
 	{
 		m_body->SetAllowSleeping(enable);
 	}
 
-#if 0
 	void JoltRigidBody3D::FallAsleep()
 	{
-		if (m_body->getActivationState() != DISABLE_DEACTIVATION)
-			m_body->setActivationState(ISLAND_SLEEPING);
+		JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterface();
+		bodyInterface.DeactivateBody(m_body->GetID());
 	}
 
 	Boxf JoltRigidBody3D::GetAABB() const
 	{
-		btVector3 min, max;
-		m_body->getAabb(min, max);
-
-		return Boxf(FromJolt(min), FromJolt(max));
+		const JPH::AABox& aabb = m_body->GetWorldSpaceBounds();
+		return Boxf(FromJolt(aabb.mMin), FromJolt(aabb.GetSize()));
 	}
 
 	float JoltRigidBody3D::GetAngularDamping() const
 	{
-		return m_body->getAngularDamping();
+		if NAZARA_UNLIKELY(IsStatic())
+			return 0.f;
+
+		return m_body->GetMotionProperties()->GetAngularDamping();
 	}
 
 	Vector3f JoltRigidBody3D::GetAngularVelocity() const
 	{
-		return FromJolt(m_body->getAngularVelocity());
+		return FromJolt(m_body->GetAngularVelocity());
 	}
 
 	float JoltRigidBody3D::GetLinearDamping() const
 	{
-		return m_body->getLinearDamping();
+		if NAZARA_UNLIKELY(IsStatic())
+			return 0.f;
+
+		return m_body->GetMotionProperties()->GetLinearDamping();
 	}
 
 	Vector3f JoltRigidBody3D::GetLinearVelocity() const
 	{
-		return FromJolt(m_body->getLinearVelocity());
+		return FromJolt(m_body->GetLinearVelocity());
 	}
-#endif
 
 	float JoltRigidBody3D::GetMass() const
 	{
+		if NAZARA_UNLIKELY(IsStatic())
+			return 0.f;
+
 		return 1.f / m_body->GetMotionProperties()->GetInverseMass();
 	}
-#if 0
-	Vector3f JoltRigidBody3D::GetMassCenter(CoordSys coordSys) const
-	{
-		return FromJolt(m_body->getCenterOfMassPosition());
-	}
 
 	Matrix4f JoltRigidBody3D::GetMatrix() const
 	{
-		return FromJolt(m_body->getWorldTransform());
+		return FromJolt(m_body->GetCenterOfMassTransform());
 	}
-#endif
 
 	Vector3f JoltRigidBody3D::GetPosition() const
 	{
@@ -190,13 +204,6 @@ namespace Nz
 		return FromJolt(m_body->GetRotation());
 	}
 
-#if 0
-	bool JoltRigidBody3D::IsSimulationEnabled() const
-	{
-		return m_body->isActive();
-	}
-#endif
-
 	bool JoltRigidBody3D::IsSleeping() const
 	{
 		return m_body->IsActive();
@@ -207,30 +214,42 @@ namespace Nz
 		return m_body->GetAllowSleeping();
 	}
 
-#if 0
+	bool JoltRigidBody3D::IsStatic() const
+	{
+		return m_body->GetMotionType() == JPH::EMotionType::Static;
+	}
+
 	void JoltRigidBody3D::SetAngularDamping(float angularDamping)
 	{
-		m_body->setDamping(m_body->getLinearDamping(), angularDamping);
+		if NAZARA_UNLIKELY(IsStatic())
+			return;
+
+		m_body->GetMotionProperties()->SetAngularDamping(angularDamping);
 	}
 
 	void JoltRigidBody3D::SetAngularVelocity(const Vector3f& angularVelocity)
 	{
-		m_body->setAngularVelocity(ToJolt(angularVelocity));
+		m_body->SetAngularVelocity(ToJolt(angularVelocity));
 	}
-#endif
+
 	void JoltRigidBody3D::SetGeom(std::shared_ptr<JoltCollider3D> geom, bool recomputeInertia)
 	{
 		if (m_geom != geom)
 		{
 			float mass = GetMass();
 
-			if (geom)
-				m_geom = std::move(geom);
+			const JPH::Shape* shape;
+			m_geom = std::move(geom);
+			if (m_geom)
+				shape = m_geom->GetShapeSettings()->Create().Get();
 			else
-				m_geom = std::make_shared<JoltSphereCollider3D>(std::numeric_limits<float>::epsilon());
+			{
+				m_body->SetIsSensor(true);
+				shape = m_world->GetNullShape();
+			}
 
 			JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterface();
-			bodyInterface.SetShape(m_body->GetID(), m_geom->GetShapeSettings()->Create().Get(), false, JPH::EActivation::Activate);
+			bodyInterface.SetShape(m_body->GetID(), m_geom->GetShapeSettings()->Create().Get(), false, (ShouldActivate()) ? JPH::EActivation::Activate : JPH::EActivation::DontActivate);
 			if (recomputeInertia)
 			{
 				JPH::MassProperties massProperties = m_body->GetShape()->GetMassProperties();
@@ -242,6 +261,9 @@ namespace Nz
 
 	void JoltRigidBody3D::SetLinearDamping(float damping)
 	{
+		if NAZARA_UNLIKELY(IsStatic())
+			return;
+
 		m_body->GetMotionProperties()->SetLinearDamping(damping);
 	}
 
@@ -270,34 +292,28 @@ namespace Nz
 			JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterfaceNoLock();
 			bodyInterface.DeactivateBody(m_body->GetID());
 
-			m_body->SetMotionType(JPH::EMotionType::Static);
+			m_body->SetMotionType(JPH::EMotionType::Kinematic);
 		}
 	}
 
-	void JoltRigidBody3D::SetMassCenter(const Vector3f& center)
-	{
-		//m_body->GetMotionProperties()->set
-	}
-
 	void JoltRigidBody3D::SetPosition(const Vector3f& position)
 	{
 		JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterfaceNoLock();
-		bodyInterface.SetPosition(m_body->GetID(), ToJolt(position), JPH::EActivation::Activate);
+		bodyInterface.SetPosition(m_body->GetID(), ToJolt(position), (ShouldActivate()) ? JPH::EActivation::Activate : JPH::EActivation::DontActivate);
 	}
 
 	void JoltRigidBody3D::SetRotation(const Quaternionf& rotation)
 	{
-		JPH::BodyInterface& body_interface = m_world->GetPhysicsSystem()->GetBodyInterfaceNoLock();
-		body_interface.SetRotation(m_body->GetID(), ToJolt(rotation), JPH::EActivation::Activate);
+		JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterfaceNoLock();
+		bodyInterface.SetRotation(m_body->GetID(), ToJolt(rotation), (ShouldActivate()) ? JPH::EActivation::Activate : JPH::EActivation::DontActivate);
 	}
 
 	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);
+		bodyInterface.SetPositionAndRotation(m_body->GetID(), ToJolt(position), ToJolt(rotation), (ShouldActivate()) ? JPH::EActivation::Activate : JPH::EActivation::DontActivate);
 	}
 
-#if 0
 	Quaternionf JoltRigidBody3D::ToLocal(const Quaternionf& worldRotation)
 	{
 		return GetRotation().Conjugate() * worldRotation;
@@ -305,8 +321,7 @@ namespace Nz
 
 	Vector3f JoltRigidBody3D::ToLocal(const Vector3f& worldPosition)
 	{
-		btTransform worldTransform = m_body->getWorldTransform();
-		return GetMatrix().InverseTransform() * worldPosition;
+		return FromJolt(m_body->GetInverseCenterOfMassTransform() * ToJolt(worldPosition));
 	}
 
 	Quaternionf JoltRigidBody3D::ToWorld(const Quaternionf& localRotation)
@@ -316,45 +331,100 @@ namespace Nz
 
 	Vector3f JoltRigidBody3D::ToWorld(const Vector3f& localPosition)
 	{
-		return GetMatrix() * localPosition;
+		return FromJolt(m_body->GetCenterOfMassTransform() * ToJolt(localPosition));
 	}
-#endif
 
 	void JoltRigidBody3D::WakeUp()
 	{
-		JPH::BodyInterface& body_interface = m_world->GetPhysicsSystem()->GetBodyInterface();
-		body_interface.ActivateBody(m_body->GetID()); 
+		JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterface();
+		bodyInterface.ActivateBody(m_body->GetID()); 
 	}
 
-	JoltRigidBody3D& JoltRigidBody3D::operator=(JoltRigidBody3D&& object) noexcept
+	JoltRigidBody3D& JoltRigidBody3D::operator=(JoltRigidBody3D&& body) noexcept
 	{
 		Destroy();
 
-		m_body          = object.m_body;
-		m_bodyIndex     = object.m_bodyIndex;
-		m_geom          = std::move(object.m_geom);
-		m_world         = object.m_world;
+		m_body                = body.m_body;
+		m_bodyIndex           = body.m_bodyIndex;
+		m_geom                = std::move(body.m_geom);
+		m_world               = body.m_world;
+		m_isSimulationEnabled = body.m_isSimulationEnabled;
+		m_isTrigger           = body.m_isTrigger;
 
-		m_body->SetUserData(SafeCast<UInt64>(reinterpret_cast<std::uintptr_t>(this)));
+		body.m_body = nullptr;
+		body.m_bodyIndex = std::numeric_limits<UInt32>::max();
 
-		object.m_body = nullptr;
-		object.m_bodyIndex = std::numeric_limits<UInt32>::max();
+		if (m_body)
+			m_body->SetUserData(SafeCast<UInt64>(reinterpret_cast<std::uintptr_t>(this)));
 
 		return *this;
 	}
 
-	void JoltRigidBody3D::Destroy()
+	void JoltRigidBody3D::Destroy(bool worldDestruction)
 	{
 		if (m_body)
 		{
-			JPH::BodyID bodyId = m_body->GetID();
-
 			JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterface();
-			bodyInterface.RemoveBody(bodyId);
-			bodyInterface.DestroyBody(bodyId);
+			m_world->UnregisterBody(m_body->GetID(), true, !worldDestruction);
 			m_body = nullptr;
 		}
 
 		m_geom.reset();
 	}
+
+	void JoltRigidBody3D::BuildSettings(const DynamicSettings& settings, JPH::BodyCreationSettings& creationSettings)
+	{
+		BuildSettings(static_cast<const CommonSettings&>(settings), creationSettings);
+
+		creationSettings.mAngularDamping = settings.angularDamping;
+		creationSettings.mAngularVelocity = ToJolt(settings.angularVelocity);
+		creationSettings.mLinearDamping = settings.linearDamping;
+		creationSettings.mLinearVelocity = ToJolt(settings.linearVelocity);
+		creationSettings.mFriction = settings.friction;
+		creationSettings.mGravityFactor = settings.gravityFactor;
+		creationSettings.mMaxAngularVelocity = settings.maxAngularVelocity;
+		creationSettings.mMaxLinearVelocity = settings.maxLinearVelocity;
+		creationSettings.mObjectLayer = 1;
+		creationSettings.mRestitution = settings.restitution;
+		creationSettings.mUserData = SafeCast<UInt64>(reinterpret_cast<std::uintptr_t>(this));
+
+		creationSettings.mMassPropertiesOverride = creationSettings.GetShape()->GetMassProperties();
+		creationSettings.mMassPropertiesOverride.ScaleToMass(settings.mass);
+
+		creationSettings.mMotionType = (settings.mass > 0.f) ? JPH::EMotionType::Dynamic : JPH::EMotionType::Kinematic;
+
+		switch (settings.motionQuality)
+		{
+			case JoltMotionQuality::Discrete:   creationSettings.mMotionQuality = JPH::EMotionQuality::Discrete;   break;
+			case JoltMotionQuality::LinearCast: creationSettings.mMotionQuality = JPH::EMotionQuality::LinearCast; break;
+		}
+	}
+
+	void JoltRigidBody3D::BuildSettings(const StaticSettings& settings, JPH::BodyCreationSettings& creationSettings)
+	{
+		BuildSettings(static_cast<const CommonSettings&>(settings), creationSettings);
+
+		creationSettings.mMotionType = JPH::EMotionType::Static;
+	}
+
+	void JoltRigidBody3D::BuildSettings(const CommonSettings& settings, JPH::BodyCreationSettings& creationSettings)
+	{
+		if (settings.geom)
+		{
+			creationSettings.SetShapeSettings(settings.geom->GetShapeSettings());
+			creationSettings.mIsSensor = settings.isTrigger;
+		}
+		else
+		{
+			creationSettings.SetShape(m_world->GetNullShape());
+			creationSettings.mIsSensor = true; //< not the best solution but enough for now
+			creationSettings.mPosition = ToJolt(settings.position);
+			creationSettings.mRotation = ToJolt(settings.rotation);
+		}
+	}
+
+	bool JoltRigidBody3D::ShouldActivate() const
+	{
+		return m_isSimulationEnabled && m_world->IsBodyRegistered(m_bodyIndex);
+	}
 }

src/Nazara/JoltPhysics3D/Systems/JoltPhysics3DSystem.cpp

@@ -28,7 +28,7 @@ namespace Nz
 		// Ensure every RigidBody3D is destroyed before world is
 		auto rigidBodyView = m_registry.view<JoltRigidBody3DComponent>();
 		for (auto [entity, rigidBodyComponent] : rigidBodyView.each())
-			rigidBodyComponent.Destroy();
+			rigidBodyComponent.Destroy(true);
 	}
 
 	void JoltPhysics3DSystem::Dump()