Add more JOLT code

AnKi/Physics2/Common.h

@@ -12,45 +12,101 @@
 #include <AnKi/Math.h>
 
 #include <Jolt/Jolt.h>
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Jolt/Physics/Body/BodyActivationListener.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaledShape.h>
+#include <Jolt/Physics/PhysicsSystem.h>
+#include <Jolt/RegisterTypes.h>
+#include <Jolt/Core/JobSystemThreadPool.h>
 
 namespace anki {
+namespace v2 {
 
 #define ANKI_PHYS_LOGI(...) ANKI_LOG("PHYS", kNormal, __VA_ARGS__)
 #define ANKI_PHYS_LOGE(...) ANKI_LOG("PHYS", kError, __VA_ARGS__)
 #define ANKI_PHYS_LOGW(...) ANKI_LOG("PHYS", kWarning, __VA_ARGS__)
 #define ANKI_PHYS_LOGF(...) ANKI_LOG("PHYS", kFatal, __VA_ARGS__)
 
-class Physics2MemoryPool : public HeapMemoryPool, public MakeSingleton<Physics2MemoryPool>
+#define ANKI_PHYSICS_COMMON_FRIENDS \
+	friend class PhysicsWorld; \
+	template<typename, typename> \
+	friend class IntrusivePtr; \
+	template<typename, typename, typename> \
+	friend class BlockArray;
+
+class PhysicsMemoryPool : public HeapMemoryPool, public MakeSingleton<PhysicsMemoryPool>
 {
 	template<typename>
 	friend class MakeSingleton;
 
 private:
-	Physics2MemoryPool(AllocAlignedCallback allocCb, void* allocCbUserData)
+	PhysicsMemoryPool(AllocAlignedCallback allocCb, void* allocCbUserData)
 		: HeapMemoryPool(allocCb, allocCbUserData, "PhysicsMemPool")
 	{
 	}
 
-	~Physics2MemoryPool() = default;
+	~PhysicsMemoryPool() = default;
 };
 
-ANKI_DEFINE_SUBMODULE_UTIL_CONTAINERS(Physics2, Physics2MemoryPool)
+ANKI_DEFINE_SUBMODULE_UTIL_CONTAINERS(Physics, PhysicsMemoryPool)
+
+enum class PhysicsLayer : U8
+{
+	kStatic,
+	kMoving,
+	kCharacter,
+	kCount
+};
 
 // Forward
-class Physics2CollisionShape;
+class PhysicsCollisionShape;
+class PhysicsBody;
 
 /// Custom deleter.
-class Physics2CollisionShapePtrDeleter
+class PhysicsCollisionShapePtrDeleter
 {
 public:
-	void operator()(Physics2CollisionShape* ptr);
+	void operator()(PhysicsCollisionShape* ptr);
 };
 
-using Physics2CollisionShapePtr = IntrusivePtr<Physics2CollisionShape, Physics2CollisionShapePtrDeleter>;
+using PhysicsCollisionShapePtr = IntrusivePtr<PhysicsCollisionShape, PhysicsCollisionShapePtrDeleter>;
+
+/// Custom deleter.
+class PhysicsBodyPtrDeleter
+{
+public:
+	void operator()(PhysicsBody* ptr);
+};
+
+using PhysicsBodyPtr = IntrusivePtr<PhysicsBody, PhysicsBodyPtrDeleter>;
 
 inline JPH::RVec3 toJPH(Vec3 ak)
 {
 	return JPH::RVec3(ak.x(), ak.y(), ak.z());
 }
 
+inline JPH::Quat toJPH(Quat ak)
+{
+	return JPH::Quat(ak.x(), ak.y(), ak.z(), ak.w());
+}
+
+inline Vec4 toAnKi(const JPH::Vec4& jph)
+{
+	return Vec4(jph.GetX(), jph.GetY(), jph.GetZ(), jph.GetW());
+}
+
+inline Transform toAnKi(const JPH::RMat44& jph)
+{
+	Mat4 m;
+	for(U32 i = 0; i < 4; ++i)
+	{
+		m.setColumn(i, toAnKi(jph.GetColumn4(i)));
+	}
+	return Transform(m);
+}
+
+} // namespace v2
 } // end namespace anki

AnKi/Physics2/PhysicsBody.cpp

@@ -0,0 +1,57 @@
+// Copyright (C) 2009-present, Panagiotis Christopoulos Charitos and contributors.
+// All rights reserved.
+// Code licensed under the BSD License.
+// http://www.anki3d.org/LICENSE
+
+#include <AnKi/Physics2/PhysicsBody.h>
+#include <AnKi/Physics2/PhysicsWorld.h>
+
+namespace anki {
+namespace v2 {
+
+void PhysicsBody::setTransform(const Transform& trf)
+{
+	ANKI_ASSERT(trf.getScale() == m_worldTrf.getScale() && "Can't handle dynamic scaling for now");
+	ANKI_ASSERT(!m_jphBody->IsStatic());
+
+	const JPH::RVec3 pos = toJPH(trf.getOrigin().xyz());
+	const JPH::Quat rot = toJPH(Quat(trf.getRotation()));
+
+	PhysicsWorld::getSingleton().m_jphPhysicsSystem->GetBodyInterfaceNoLock().SetPositionAndRotation(m_jphBody->GetID(), pos, rot,
+																									 JPH::EActivation::Activate);
+
+	m_worldTrf = trf;
+	++m_worldTrfVersion;
+}
+
+void PhysicsBody::applyForce(const Vec3& force, const Vec3& relPos)
+{
+	ANKI_ASSERT(!m_jphBody->IsStatic());
+	PhysicsWorld::getSingleton().m_jphPhysicsSystem->GetBodyInterfaceNoLock().AddForce(m_jphBody->GetID(), toJPH(force), toJPH(relPos));
+}
+
+void PhysicsBody::applyForce(const Vec3& force)
+{
+	ANKI_ASSERT(!m_jphBody->IsStatic());
+	PhysicsWorld::getSingleton().m_jphPhysicsSystem->GetBodyInterfaceNoLock().AddForce(m_jphBody->GetID(), toJPH(force));
+}
+
+void PhysicsBody::activate(Bool activate)
+{
+	if(activate)
+	{
+		PhysicsWorld::getSingleton().m_jphPhysicsSystem->GetBodyInterfaceNoLock().ActivateBody(m_jphBody->GetID());
+	}
+	else
+	{
+		PhysicsWorld::getSingleton().m_jphPhysicsSystem->GetBodyInterfaceNoLock().DeactivateBody(m_jphBody->GetID());
+	}
+}
+
+void PhysicsBody::setGravityFactor(F32 factor)
+{
+	PhysicsWorld::getSingleton().m_jphPhysicsSystem->GetBodyInterfaceNoLock().SetGravityFactor(m_jphBody->GetID(), factor);
+}
+
+} // namespace v2
+} // namespace anki

AnKi/Physics2/PhysicsBody.h

@@ -7,9 +7,9 @@
 
 #include <AnKi/Physics2/Common.h>
 #include <AnKi/Util/ClassWrapper.h>
-#include <Jolt/Physics/Body/Body.h>
 
 namespace anki {
+namespace v2 {
 
 /// @addtogroup physics
 /// @{
@@ -18,87 +18,75 @@ namespace anki {
 class PhysicsBodyInitInfo
 {
 public:
-	Physics2CollisionShape* m_shape = nullptr;
-	F32 m_mass = 0.0f;
+	PhysicsCollisionShape* m_shape = nullptr;
+	F32 m_mass = 0.0f; ///< Zero mass means static object.
 	Transform m_transform = Transform::getIdentity();
 	F32 m_friction = 0.5f;
+	PhysicsLayer m_layer = PhysicsLayer::kStatic;
 };
 
 /// Rigid body.
 class PhysicsBody
 {
+	ANKI_PHYSICS_COMMON_FRIENDS
+	friend class PhysicsBodyPtrDeleter;
+
 public:
-	const Transform& getTransform() const
-	{
-		return m_trf;
-	}
+	PhysicsBody(const PhysicsBody&) = delete;
 
-	void setTransform(const Transform& trf)
-	{
-		// TODO
-	}
+	PhysicsBody& operator=(const PhysicsBody&) = delete;
 
-	void applyForce(const Vec3& force, const Vec3& relPos)
+	const Transform& getTransform(U32* version = nullptr) const
 	{
-		// TODO
+		if(version)
+		{
+			*version = m_worldTrfVersion;
+		}
+		return m_worldTrf;
 	}
 
-	void setMass(F32 mass);
+	void setTransform(const Transform& trf);
 
-	F32 getMass() const
-	{
-		return m_mass;
-	}
+	void applyForce(const Vec3& force, const Vec3& relPos);
 
-	void activate(Bool activate)
-	{
-		// TODO
-	}
+	/// Apply force to the center of mass.
+	void applyForce(const Vec3& force);
 
-	void clearForces()
-	{
-		// TODO
-	}
+	void activate(Bool activate);
 
-	void setLinearVelocity(const Vec3& velocity)
-	{
-		// TODO
-	}
+	/// Zero means no gravity, 1 means normal gravity.
+	void setGravityFactor(F32 factor);
 
-	void setAngularVelocity(const Vec3& velocity)
+private:
+	JPH::Body* m_jphBody = nullptr;
+	PhysicsCollisionShapePtr m_primaryShape;
+	PhysicsCollisionShapePtr m_scaledShape;
+	mutable Atomic<U32> m_refcount = {0};
+
+	Transform m_worldTrf;
+	U32 m_worldTrfVersion = 1;
+
+	U32 m_arrayIndex : 31 = kMaxU32 >> 1u;
+	U32 m_activated : 1 = false;
+
+	PhysicsBody() = default;
+
+	~PhysicsBody()
 	{
-		// TODO
+		ANKI_ASSERT(m_jphBody == nullptr);
 	}
 
-	void setGravity(const Vec3& gravity)
+	void retain() const
 	{
-		// TODO
+		m_refcount.fetchAdd(1);
 	}
 
-	void setAngularFactor(const Vec3& factor)
+	U32 release() const
 	{
-		// TODO
+		return m_refcount.fetchSub(1);
 	}
-
-private:
-	/// Store the data of the btRigidBody in place to avoid additional allocations.
-	ClassWrapper<btRigidBody> m_body;
-
-	Transform m_trf = Transform::getIdentity();
-	MotionState m_motionState;
-
-	PhysicsCollisionShapePtr m_shape;
-
-	F32 m_mass = 1.0f;
-
-	PhysicsBody(const PhysicsBodyInitInfo& init);
-
-	~PhysicsBody();
-
-	void registerToWorld() override;
-
-	void unregisterFromWorld() override;
 };
 /// @}
 
+} // namespace v2
 } // end namespace anki

AnKi/Physics2/PhysicsCollisionShape.h

@@ -8,45 +8,55 @@
 #include <AnKi/Physics2/Common.h>
 #include <AnKi/Util/WeakArray.h>
 #include <AnKi/Util/ClassWrapper.h>
-#include <Jolt/Physics/Collision/Shape/BoxShape.h>
-#include <Jolt/Physics/Collision/Shape/SphereShape.h>
 
 namespace anki {
+namespace v2 {
 
 /// Wrapper on top of JPH collision shapes.
-class Physics2CollisionShape
+class PhysicsCollisionShape
 {
-	friend class Physics2World;
-	template<typename, typename>
-	friend class IntrusivePtr;
-	template<typename, typename, typename>
-	friend class BlockArray;
+	ANKI_PHYSICS_COMMON_FRIENDS
+	friend class PhysicsCollisionShapePtrDeleter;
 
-protected:
-	enum ShapeType : U8
+public:
+	PhysicsCollisionShape(const PhysicsCollisionShape&) = delete;
+
+	PhysicsCollisionShape& operator=(const PhysicsCollisionShape&) = delete;
+
+private:
+	enum class ShapeType : U8
 	{
 		kBox,
 		kSphere,
 		kConvex,
-		kTrimesh
+		kTrimesh,
+		kScaled, ///< This is for internal use
+
+		kCount
 	};
 
 	union
 	{
+		ClassWrapper<JPH::Shape> m_shapeBase;
 		ClassWrapper<JPH::BoxShape> m_box;
 		ClassWrapper<JPH::SphereShape> m_sphere;
+		ClassWrapper<JPH::ScaledShape> m_scaled; ///< We don't hold a reference to the target shape to avoid locking mutexes twice.
 	};
 
 	mutable Atomic<U32> m_refcount = {0};
-	U32 m_arrayIndex : 24 = 0b111111111111111111111111;
-	U32 m_type : 8;
+	U32 m_arrayIndex = kMaxU32;
+	ShapeType m_type;
 
-	Physics2CollisionShape(ShapeType type)
+	PhysicsCollisionShape(ShapeType type)
 		: m_type(type)
 	{
+		ANKI_ASSERT(type < ShapeType::kCount);
+		ANKI_ASSERT(&m_shapeBase == static_cast<JPH::Shape*>(&m_box));
+		ANKI_ASSERT(&m_shapeBase == static_cast<JPH::Shape*>(&m_sphere));
+		ANKI_ASSERT(&m_shapeBase == static_cast<JPH::Shape*>(&m_scaled));
 	}
 
-	~Physics2CollisionShape()
+	~PhysicsCollisionShape()
 	{
 		switch(m_type)
 		{
@@ -56,6 +66,9 @@ protected:
 		case ShapeType::kSphere:
 			m_sphere.destroy();
 			break;
+		case ShapeType::kScaled:
+			m_scaled.destroy();
+			break;
 		default:
 			ANKI_ASSERT(0);
 		}
@@ -72,4 +85,5 @@ protected:
 	}
 };
 
+} // namespace v2
 } // end namespace anki

AnKi/Physics2/PhysicsWorld.cpp

@@ -4,36 +4,363 @@
 // http://www.anki3d.org/LICENSE
 
 #include <AnKi/Physics2/PhysicsWorld.h>
-#include <AnKi/Physics2/PhysicsCollisionShape.h>
+#include <AnKi/Util/System.h>
 
 namespace anki {
+namespace v2 {
 
-void Physics2CollisionShapePtrDeleter::operator()(Physics2CollisionShape* ptr)
+class BPLayerInterfaceImpl final : public JPH::BroadPhaseLayerInterface
 {
-	Physics2World::getSingleton().destroyCollisionShape(ptr);
+public:
+	static constexpr JPH::BroadPhaseLayer kStaticBroadPhaseLayer{0};
+	static constexpr JPH::BroadPhaseLayer kDynamicBroadPhaseLayer{1};
+
+	virtual U32 GetNumBroadPhaseLayers() const override
+	{
+		return 2;
+	}
+
+	virtual JPH::BroadPhaseLayer GetBroadPhaseLayer(JPH::ObjectLayer inLayer) const override
+	{
+		switch(PhysicsLayer(inLayer))
+		{
+		case PhysicsLayer::kStatic:
+			return kStaticBroadPhaseLayer;
+		default:
+			return kDynamicBroadPhaseLayer;
+		}
+	}
+};
+
+// Class that determines if an object layer can collide with a broadphase layer
+class ObjectVsBroadPhaseLayerFilterImpl final : public JPH::ObjectVsBroadPhaseLayerFilter
+{
+public:
+	virtual bool ShouldCollide(JPH::ObjectLayer inLayer1, JPH::BroadPhaseLayer inLayer2) const override
+	{
+		switch(PhysicsLayer(inLayer1))
+		{
+		case PhysicsLayer::kStatic:
+			return inLayer2 != BPLayerInterfaceImpl::kStaticBroadPhaseLayer; // Static doesn't collide with static.
+		default:
+			return true;
+		}
+	}
+};
+
+/// Class that determines if two object layers can collide
+class ObjectLayerPairFilterImpl final : public JPH::ObjectLayerPairFilter
+{
+public:
+	virtual bool ShouldCollide(JPH::ObjectLayer inObject1, JPH::ObjectLayer inObject2) const override
+	{
+		switch(PhysicsLayer(inObject1))
+		{
+		case PhysicsLayer::kStatic:
+			return PhysicsLayer(inObject2) != PhysicsLayer::kStatic; // Static doesn't collide with static.
+		default:
+			return true; // Moving collides with everything
+		}
+	}
+};
+
+static BPLayerInterfaceImpl g_bPLayerInterfaceImpl;
+static ObjectVsBroadPhaseLayerFilterImpl g_objectVsBroadPhaseLayerFilterImpl;
+static ObjectLayerPairFilterImpl g_objectLayerPairFilterImpl;
+
+void PhysicsCollisionShapePtrDeleter::operator()(PhysicsCollisionShape* ptr)
+{
+	PhysicsWorld& world = PhysicsWorld::getSingleton();
+
+	LockGuard lock(world.m_mtx);
+
+	world.m_collisionShapes.erase(ptr->m_arrayIndex);
+}
+
+void PhysicsBodyPtrDeleter::operator()(PhysicsBody* ptr)
+{
+	PhysicsWorld& world = PhysicsWorld::getSingleton();
+
+	LockGuard lock(world.m_mtx);
+
+	if(!ptr->m_jphBody->IsInBroadPhase())
+	{
+		// Hasn't been added yet to the physics system, do special cleanup
+
+		decltype(world.m_bodiesToAdd)::Iterator it = world.m_bodiesToAdd.getBegin();
+		while(it != world.m_bodiesToAdd.getEnd())
+		{
+			if(*it == ptr)
+			{
+				break;
+			}
+		}
+
+		ANKI_ASSERT(it != world.m_bodiesToAdd.getEnd() && "Should have been in the \"toAdd\" list");
+		world.m_bodiesToAdd.erase(it);
+
+		world.m_bodies.erase(ptr->m_arrayIndex);
+	}
+	else
+	{
+		// Deferred cleanup
+
+		world.m_bodiesToRemove.emplaceBack(ptr);
+	}
+}
+
+void PhysicsWorld::MyBodyActivationListener::OnBodyActivated([[maybe_unused]] const JPH::BodyID& inBodyID, U64 inBodyUserData)
+{
+	PhysicsBody* body = numberToPtr<PhysicsBody*>(inBodyUserData);
+	body->m_activated = 1;
+}
+
+void PhysicsWorld::MyBodyActivationListener::OnBodyDeactivated([[maybe_unused]] const JPH::BodyID& inBodyID, U64 inBodyUserData)
+{
+	PhysicsBody* body = numberToPtr<PhysicsBody*>(inBodyUserData);
+	body->m_activated = 0;
+}
+
+PhysicsWorld::~PhysicsWorld()
+{
+	removeBodies();
+
+	ANKI_ASSERT(m_collisionShapes.getSize() == 0);
+	ANKI_ASSERT(m_bodies.getSize() == 0);
+
+	m_jobSystem.destroy();
+	m_jphPhysicsSystem.destroy();
+
+	JPH::UnregisterTypes();
+
+	delete JPH::Factory::sInstance;
+	JPH::Factory::sInstance = nullptr;
+
+	PhysicsMemoryPool::freeSingleton();
+}
+
+Error PhysicsWorld::init(AllocAlignedCallback allocCb, void* allocCbData)
+{
+	PhysicsMemoryPool::allocateSingleton(allocCb, allocCbData);
+
+	JPH::Allocate = [](PtrSize size) -> void* {
+		return PhysicsMemoryPool::getSingleton().allocate(size, 16);
+	};
+
+	JPH::Reallocate = []([[maybe_unused]] void* in, [[maybe_unused]] PtrSize oldSize, PtrSize newSize) -> void* {
+		void* out;
+
+		if(newSize > 0)
+		{
+			out = PhysicsMemoryPool::getSingleton().allocate(newSize, 16);
+		}
+		else
+		{
+			out = nullptr;
+		}
+
+		if(in && out)
+		{
+			ANKI_ASSERT(oldSize > 0 && newSize > 0);
+			memcpy(out, in, min(oldSize, newSize));
+		}
+
+		PhysicsMemoryPool::getSingleton().free(in);
+
+		return out;
+	};
+
+	JPH::Free = [](void* ptr) {
+		PhysicsMemoryPool::getSingleton().free(ptr);
+	};
+
+	JPH::AlignedAllocate = [](PtrSize size, PtrSize alignment) -> void* {
+		return PhysicsMemoryPool::getSingleton().allocate(size, alignment);
+	};
+
+	JPH::AlignedFree = [](void* ptr) {
+		PhysicsMemoryPool::getSingleton().free(ptr);
+	};
+
+	JPH::Factory::sInstance = new JPH::Factory();
+
+	JPH::RegisterTypes();
+
+	m_jphPhysicsSystem.construct();
+
+	constexpr U32 maxBodies = kMaxU16;
+	constexpr U32 mutexCount = 0;
+	constexpr U32 maxBodyPairs = kMaxU16;
+	constexpr U32 maxConstraints = 10 * 1024;
+	m_jphPhysicsSystem->Init(maxBodies, mutexCount, maxBodyPairs, maxConstraints, g_bPLayerInterfaceImpl, g_objectVsBroadPhaseLayerFilterImpl,
+							 g_objectLayerPairFilterImpl);
+
+	m_jphPhysicsSystem->SetGravity(toJPH(Vec3(0.0f, -9.81f, 0.0f)));
+
+	const U32 threadCount = min(8u, getCpuCoresCount() - 1);
+	m_jobSystem.construct(JPH::cMaxPhysicsJobs, JPH::cMaxPhysicsBarriers, threadCount);
+
+	m_tempAllocator.construct(U32(10_MB));
+
+	return Error::kNone;
 }
 
-Physics2CollisionShapePtr Physics2World::newSphereCollisionShape(F32 radius)
+void PhysicsWorld::update(Second dt)
 {
-	auto it = m_collisionShapes.emplace(Physics2CollisionShape::ShapeType::kSphere);
-	it->m_sphere.construct(radius);
-	it->m_sphere->SetEmbedded();
+	const Bool optimizeBroadphase = m_bodiesToAdd.getSize() || m_bodiesToRemove.getSize();
+
+	removeBodies();
+	addBodies();
+
+	if(optimizeBroadphase)
+	{
+		m_jphPhysicsSystem->OptimizeBroadPhase();
+	}
+
+	constexpr I32 collisionSteps = 1;
+	m_jphPhysicsSystem->Update(F32(dt), collisionSteps, &m_tempAllocator, &m_jobSystem);
+
+	// Update transforms
+	for(auto& body : m_bodies)
+	{
+		if(body.m_activated)
+		{
+			const Transform newTrf = toAnKi(body.m_jphBody->GetWorldTransform());
+			if(newTrf != body.m_worldTrf)
+			{
+				body.m_worldTrf = newTrf;
+				++body.m_worldTrfVersion;
+			}
+		}
+	}
+}
+
+template<typename TJPHCollisionShape, typename... TArgs>
+PhysicsCollisionShapePtr PhysicsWorld::newCollisionShape(PhysicsCollisionShape::ShapeType type, TArgs&&... args)
+{
+	decltype(m_collisionShapes)::Iterator it;
+
+	{
+		LockGuard lock(m_mtx);
+		it = m_collisionShapes.emplace(type);
+	}
+
+	ClassWrapper<TJPHCollisionShape>& classw = reinterpret_cast<ClassWrapper<TJPHCollisionShape>&>(it->m_shapeBase);
+	classw.construct(std::forward<TArgs>(args)...);
+	classw->SetEmbedded();
+
 	it->m_arrayIndex = it.getArrayIndex();
-	return Physics2CollisionShapePtr(&(*it));
+	return PhysicsCollisionShapePtr(&(*it));
+}
+
+PhysicsCollisionShapePtr PhysicsWorld::newSphereCollisionShape(F32 radius)
+{
+	return newCollisionShape<JPH::SphereShape>(PhysicsCollisionShape::ShapeType::kSphere, radius);
+}
+
+PhysicsCollisionShapePtr PhysicsWorld::newBoxCollisionShape(Vec3 extend)
+{
+	return newCollisionShape<JPH::BoxShape>(PhysicsCollisionShape::ShapeType::kBox, toJPH(extend));
 }
 
-Physics2CollisionShapePtr Physics2World::newBoxCollisionShape(Vec3 extend)
+PhysicsBodyPtr PhysicsWorld::newPhysicsBody(const PhysicsBodyInitInfo& init)
 {
-	auto it = m_collisionShapes.emplace(Physics2CollisionShape::ShapeType::kBox);
-	it->m_box.construct(toJPH(extend));
-	it->m_box->SetEmbedded();
+	const Vec3 pos = init.m_transform.getOrigin().xyz();
+	const Quat rot = Quat(init.m_transform.getRotation());
+
+	// Create a scale shape
+	const Bool hasScale = (init.m_transform.getScale().xyz() - 1.0).getLengthSquared() > kEpsilonf * 10.0;
+	PhysicsCollisionShapePtr scaledShape;
+	if(hasScale)
+	{
+		scaledShape = newCollisionShape<JPH::ScaledShape>(PhysicsCollisionShape::ShapeType::kScaled, &init.m_shape->m_shapeBase,
+														  toJPH(init.m_transform.getScale().xyz()));
+	}
+
+	// Create JPH body
+	JPH::BodyCreationSettings settings((scaledShape) ? &scaledShape->m_scaled : &init.m_shape->m_shapeBase, toJPH(pos), toJPH(rot),
+									   (init.m_mass == 0.0f) ? JPH::EMotionType::Static : JPH::EMotionType::Dynamic, JPH::ObjectLayer(init.m_layer));
+
+	if(init.m_mass)
+	{
+		settings.mOverrideMassProperties = JPH::EOverrideMassProperties::CalculateInertia;
+		settings.mMassPropertiesOverride.mMass = init.m_mass;
+	}
+
+	settings.mFriction = init.m_friction;
+
+	JPH::Body* jphBody = m_jphPhysicsSystem->GetBodyInterface().CreateBody(settings);
+
+	// Create AnKi body
+	LockGuard lock(m_mtx);
+
+	auto it = m_bodies.emplace();
+	it->m_jphBody = jphBody;
 	it->m_arrayIndex = it.getArrayIndex();
-	return Physics2CollisionShapePtr(&(*it));
+	it->m_primaryShape.reset(init.m_shape);
+	it->m_scaledShape = scaledShape;
+	it->m_worldTrf = init.m_transform;
+
+	jphBody->SetUserData(ptrToNumber(&(*it)));
+
+	m_bodiesToAdd.emplaceBack(&(*it));
+
+	return PhysicsBodyPtr(&(*it));
 }
 
-void Physics2World::destroyCollisionShape(Physics2CollisionShape* ptr)
+void PhysicsWorld::addBodies()
 {
-	m_collisionShapes.erase(ptr->m_arrayIndex);
+	if(m_bodiesToAdd.getSize() == 0) [[likely]]
+	{
+		return;
+	}
+
+	PhysicsDynamicArray<JPH::BodyID> jphBodies;
+	jphBodies.resize(m_bodiesToAdd.getSize());
+
+	for(U32 i = 0; i < m_bodiesToAdd.getSize(); ++i)
+	{
+		jphBodies[i] = m_bodiesToAdd[i]->m_jphBody->GetID();
+	}
+
+	const JPH::BodyInterface::AddState state = m_jphPhysicsSystem->GetBodyInterface().AddBodiesPrepare(jphBodies.getBegin(), jphBodies.getSize());
+	m_jphPhysicsSystem->GetBodyInterface().AddBodiesFinalize(jphBodies.getBegin(), jphBodies.getSize(), state, JPH::EActivation::Activate);
+
+	for(U32 i = 0; i < m_bodiesToAdd.getSize(); ++i)
+	{
+		ANKI_ASSERT(m_bodiesToAdd[i]->m_jphBody->IsInBroadPhase());
+	}
+
+	m_bodiesToAdd.resize(0);
+}
+
+void PhysicsWorld::removeBodies()
+{
+	if(m_bodiesToRemove.getSize() == 0) [[likely]]
+	{
+		return;
+	}
+
+	// 1st, remove from phys system
+	PhysicsDynamicArray<JPH::BodyID> jphBodies;
+	jphBodies.resize(m_bodiesToRemove.getSize());
+	for(U32 i = 0; i < m_bodiesToRemove.getSize(); ++i)
+	{
+		jphBodies[i] = m_bodiesToRemove[i]->m_jphBody->GetID();
+	}
+	m_jphPhysicsSystem->GetBodyInterface().RemoveBodies(jphBodies.getBegin(), jphBodies.getSize());
+
+	// 2nd, delete the JPH bodies
+	m_jphPhysicsSystem->GetBodyInterface().DestroyBodies(jphBodies.getBegin(), jphBodies.getSize());
+
+	// 3rd, delete the AnKi bodies
+	for(U32 i = 0; i < m_bodiesToRemove.getSize(); ++i)
+	{
+		m_bodies.erase(m_bodiesToRemove[i]->m_arrayIndex);
+	}
+
+	m_bodiesToRemove.resize(0);
 }
 
+} // namespace v2
 } // end namespace anki

AnKi/Physics2/PhysicsWorld.h

@@ -7,38 +7,66 @@
 
 #include <AnKi/Physics2/Common.h>
 #include <AnKi/Physics2/PhysicsCollisionShape.h>
+#include <AnKi/Physics2/PhysicsBody.h>
 #include <AnKi/Util/BlockArray.h>
-#include <Jolt/Physics/PhysicsSystem.h>
 
 namespace anki {
+namespace v2 {
 
 /// @addtogroup physics
 /// @{
 
 /// The master container for all physics related stuff.
-class Physics2World : public MakeSingleton<Physics2World>
+class PhysicsWorld : public MakeSingleton<PhysicsWorld>
 {
 	template<typename>
 	friend class MakeSingleton;
-	friend class Physics2CollisionShapePtrDeleter;
+	friend class PhysicsCollisionShapePtrDeleter;
+	friend class PhysicsBodyPtrDeleter;
+	friend class PhysicsBody;
 
 public:
 	Error init(AllocAlignedCallback allocCb, void* allocCbData);
 
-	Physics2CollisionShapePtr newSphereCollisionShape(F32 radius);
-	Physics2CollisionShapePtr newBoxCollisionShape(Vec3 extend);
+	PhysicsCollisionShapePtr newSphereCollisionShape(F32 radius);
+	PhysicsCollisionShapePtr newBoxCollisionShape(Vec3 extend);
+
+	PhysicsBodyPtr newPhysicsBody(const PhysicsBodyInitInfo& init);
+
+	void update(Second dt);
 
 private:
-	BlockArray<Physics2CollisionShape, SingletonMemoryPoolWrapper<Physics2MemoryPool>> m_collisionShapes;
+	class MyBodyActivationListener final : public JPH::BodyActivationListener
+	{
+	public:
+		virtual void OnBodyActivated(const JPH::BodyID& inBodyID, U64 inBodyUserData) override;
+		virtual void OnBodyDeactivated(const JPH::BodyID& inBodyID, U64 inBodyUserData) override;
+	};
+
+	PhysicsBlockArray<PhysicsCollisionShape> m_collisionShapes;
+	PhysicsBlockArray<PhysicsBody> m_bodies;
+
+	ClassWrapper<JPH::PhysicsSystem> m_jphPhysicsSystem;
+	ClassWrapper<JPH::JobSystemThreadPool> m_jobSystem;
+	ClassWrapper<JPH::TempAllocatorImpl> m_tempAllocator;
+
+	// Add bodies and remove bodies in batches for perf reasons
+	PhysicsDynamicArray<PhysicsBody*> m_bodiesToAdd;
+	PhysicsDynamicArray<PhysicsBody*> m_bodiesToRemove;
+
+	Mutex m_mtx;
 
-	JPH::PhysicsSystem m_jphPhysicsSystem;
+	PhysicsWorld();
 
-	Physics2World();
+	~PhysicsWorld();
 
-	~Physics2World();
+	void addBodies();
+	void removeBodies();
 
-	void destroyCollisionShape(Physics2CollisionShape* ptr);
+	template<typename TJPHCollisionShape, typename... TArgs>
+	PhysicsCollisionShapePtr newCollisionShape(PhysicsCollisionShape::ShapeType type, TArgs&&... args);
 };
 /// @}
 
+} // namespace v2
 } // end namespace anki

AnKi/Resource/CMakeLists.txt

@@ -2,4 +2,4 @@ file(GLOB_RECURSE sources *.cpp)
 file(GLOB_RECURSE headers *.h)
 add_library(AnKiResource ${sources} ${headers})
 target_compile_definitions(AnKiResource PRIVATE -DANKI_SOURCE_FILE)
-target_link_libraries(AnKiResource AnKiCore AnKiGr AnKiPhysics AnKiZLib AnKiShaderCompiler)
+target_link_libraries(AnKiResource AnKiCore AnKiGr AnKiPhysics AnKiPhysics2 AnKiZLib AnKiShaderCompiler)

AnKi/Util/ClassWrapper.h

@@ -60,6 +60,18 @@ public:
 		return *reinterpret_cast<const TClass*>(&m_data[0]);
 	}
 
+	/// Access the instance.
+	const TClass* operator&() const
+	{
+		return reinterpret_cast<const TClass*>(&m_data[0]);
+	}
+
+	/// Access the instance.
+	TClass* operator&()
+	{
+		return reinterpret_cast<TClass*>(&m_data[0]);
+	}
+
 	/// Access the instance.
 	TClass* get()
 	{