More Jolt integration

AnKi/Physics2/Common.h

@@ -17,8 +17,12 @@
 #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/CapsuleShape.h>
 #include <Jolt/Physics/Collision/Shape/ScaledShape.h>
 #include <Jolt/Physics/PhysicsSystem.h>
+#include <Jolt/Physics/Constraints/PointConstraint.h>
+#include <Jolt/Physics/Constraints/HingeConstraint.h>
+#include <Jolt/Physics/Character/CharacterVirtual.h>
 #include <Jolt/RegisterTypes.h>
 #include <Jolt/Core/JobSystemThreadPool.h>
 
@@ -61,28 +65,47 @@ enum class PhysicsLayer : U8
 	kCount
 };
 
-// Forward
-class PhysicsCollisionShape;
-class PhysicsBody;
-
-/// Custom deleter.
-class PhysicsCollisionShapePtrDeleter
+#define ANKI_PHYSICS_DEFINE_PTRS(className) \
+	class className; \
+	class className##PtrDeleter \
+	{ \
+	public: \
+		void operator()(className* ptr); \
+	}; \
+	using className##Ptr = IntrusivePtr<className, className##PtrDeleter>;
+
+ANKI_PHYSICS_DEFINE_PTRS(PhysicsCollisionShape)
+ANKI_PHYSICS_DEFINE_PTRS(PhysicsBody)
+ANKI_PHYSICS_DEFINE_PTRS(PhysicsJoint)
+ANKI_PHYSICS_DEFINE_PTRS(PhysicsPlayerController)
+#undef ANKI_PHYSICS_DEFINE_PTRS
+
+template<U32 kSize>
+class StaticTempAllocator final : public JPH::TempAllocator
 {
 public:
-	void operator()(PhysicsCollisionShape* ptr);
-};
+	alignas(JPH_RVECTOR_ALIGNMENT) Array<U8, kSize> m_memory;
+	U32 m_base = 0;
 
-using PhysicsCollisionShapePtr = IntrusivePtr<PhysicsCollisionShape, PhysicsCollisionShapePtrDeleter>;
+	void* Allocate(U32 size) override
+	{
+		ANKI_ASSERT(size);
+		size = getAlignedRoundUp(JPH_RVECTOR_ALIGNMENT, size);
+		ANKI_ASSERT(m_base + size <= sizeof(m_memory));
+		void* out = &m_memory[0] + size;
+		m_base += size;
+		return out;
+	}
 
-/// Custom deleter.
-class PhysicsBodyPtrDeleter
-{
-public:
-	void operator()(PhysicsBody* ptr);
+	void Free([[maybe_unused]] void* address, U32 size) override
+	{
+		ANKI_ASSERT(address && size);
+		size = getAlignedRoundUp(JPH_RVECTOR_ALIGNMENT, size);
+		ANKI_ASSERT(m_base >= size);
+		m_base -= size;
+	}
 };
 
-using PhysicsBodyPtr = IntrusivePtr<PhysicsBody, PhysicsBodyPtrDeleter>;
-
 inline JPH::RVec3 toJPH(Vec3 ak)
 {
 	return JPH::RVec3(ak.x(), ak.y(), ak.z());
@@ -98,6 +121,11 @@ inline Vec4 toAnKi(const JPH::Vec4& jph)
 	return Vec4(jph.GetX(), jph.GetY(), jph.GetZ(), jph.GetW());
 }
 
+inline Vec3 toAnKi(JPH::Vec3 x)
+{
+	return Vec3(x.GetX(), x.GetY(), x.GetZ());
+}
+
 inline Transform toAnKi(const JPH::RMat44& jph)
 {
 	Mat4 m;

AnKi/Physics2/PhysicsBody.cpp

@@ -12,7 +12,6 @@ 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()));
@@ -26,13 +25,11 @@ void PhysicsBody::setTransform(const Transform& trf)
 
 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));
 }
 
@@ -53,5 +50,19 @@ void PhysicsBody::setGravityFactor(F32 factor)
 	PhysicsWorld::getSingleton().m_jphPhysicsSystem->GetBodyInterfaceNoLock().SetGravityFactor(m_jphBody->GetID(), factor);
 }
 
+void PhysicsBody::postPhysicsUpdate()
+{
+	if(m_activated)
+	{
+		const Transform newTrf =
+			toAnKi(PhysicsWorld::getSingleton().m_jphPhysicsSystem->GetBodyInterfaceNoLock().GetWorldTransform(m_jphBody->GetID()));
+		if(newTrf != m_worldTrf)
+		{
+			m_worldTrf = newTrf;
+			++m_worldTrfVersion;
+		}
+	}
+}
+
 } // namespace v2
 } // namespace anki

AnKi/Physics2/PhysicsBody.h

@@ -71,10 +71,7 @@ private:
 
 	PhysicsBody() = default;
 
-	~PhysicsBody()
-	{
-		ANKI_ASSERT(m_jphBody == nullptr);
-	}
+	~PhysicsBody() = default;
 
 	void retain() const
 	{
@@ -85,6 +82,8 @@ private:
 	{
 		return m_refcount.fetchSub(1);
 	}
+
+	void postPhysicsUpdate();
 };
 /// @}
 

AnKi/Physics2/PhysicsCollisionShape.cpp

@@ -1,6 +0,0 @@
-// 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/PhysicsCollisionShape.h>

AnKi/Physics2/PhysicsCollisionShape.h

@@ -17,6 +17,7 @@ class PhysicsCollisionShape
 {
 	ANKI_PHYSICS_COMMON_FRIENDS
 	friend class PhysicsCollisionShapePtrDeleter;
+	friend class PhysicsPlayerController;
 
 public:
 	PhysicsCollisionShape(const PhysicsCollisionShape&) = delete;
@@ -28,6 +29,7 @@ private:
 	{
 		kBox,
 		kSphere,
+		kCapsule,
 		kConvex,
 		kTrimesh,
 		kScaled, ///< This is for internal use
@@ -40,6 +42,7 @@ private:
 		ClassWrapper<JPH::Shape> m_shapeBase;
 		ClassWrapper<JPH::BoxShape> m_box;
 		ClassWrapper<JPH::SphereShape> m_sphere;
+		ClassWrapper<JPH::CapsuleShape> m_capsule;
 		ClassWrapper<JPH::ScaledShape> m_scaled; ///< We don't hold a reference to the target shape to avoid locking mutexes twice.
 	};
 
@@ -53,25 +56,13 @@ private:
 		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_capsule));
 		ANKI_ASSERT(&m_shapeBase == static_cast<JPH::Shape*>(&m_scaled));
 	}
 
 	~PhysicsCollisionShape()
 	{
-		switch(m_type)
-		{
-		case ShapeType::kBox:
-			m_box.destroy();
-			break;
-		case ShapeType::kSphere:
-			m_sphere.destroy();
-			break;
-		case ShapeType::kScaled:
-			m_scaled.destroy();
-			break;
-		default:
-			ANKI_ASSERT(0);
-		}
+		m_shapeBase.destroy();
 	}
 
 	void retain() const

AnKi/Physics2/PhysicsJoint.h

@@ -6,14 +6,72 @@
 #pragma once
 
 #include <AnKi/Physics2/Common.h>
-#include <AnKi/Physics2/PhysicsCollisionShape.h>
-#include <AnKi/Util/BlockArray.h>
+#include <AnKi/Util/ClassWrapper.h>
 
 namespace anki {
+namespace v2 {
 
 /// @addtogroup physics
 /// @{
 
+/// Wrapper on top of Jolt joints.
+class PhysicsJoint
+{
+	ANKI_PHYSICS_COMMON_FRIENDS
+	friend class PhysicsJointPtrDeleter;
+
+public:
+	PhysicsJoint(const PhysicsJoint&) = delete;
+
+	PhysicsJoint& operator=(const PhysicsJoint&) = delete;
+
+private:
+	enum class Type : U8
+	{
+		kPoint,
+		kHinge,
+
+		kCount
+	};
+
+	union
+	{
+		ClassWrapper<JPH::TwoBodyConstraint> m_base;
+		ClassWrapper<JPH::PointConstraint> m_point;
+		ClassWrapper<JPH::HingeConstraint> m_hinge;
+	};
+
+	PhysicsBodyPtr m_body1;
+	PhysicsBodyPtr m_body2;
+
+	mutable Atomic<U32> m_refcount = {0};
+	U32 m_arrayIndex = kMaxU32;
+	Type m_type;
+
+	PhysicsJoint(Type type)
+		: m_type(type)
+	{
+		ANKI_ASSERT(type < Type::kCount);
+		ANKI_ASSERT(&m_base == static_cast<JPH::TwoBodyConstraint*>(&m_point));
+		ANKI_ASSERT(&m_base == static_cast<JPH::TwoBodyConstraint*>(&m_hinge));
+	}
+
+	~PhysicsJoint()
+	{
+		m_base.destroy();
+	}
+
+	void retain() const
+	{
+		m_refcount.fetchAdd(1);
+	}
+
+	U32 release() const
+	{
+		return m_refcount.fetchSub(1);
+	}
+};
 /// @}
 
+} // namespace v2
 } // end namespace anki

AnKi/Physics2/PhysicsPlayerController.cpp

@@ -0,0 +1,155 @@
+// Copyright (C) 2009-present, Panagiotis Christopoulos Charitos and contributors.
+// All rights reserved.
+// Code licensed under the BSD License.
+// http://www.anki3d.org/LICENSE
+
+#pragma once
+
+#include <AnKi/Physics2/PhysicsPlayerController.h>
+#include <AnKi/Physics2/PhysicsWorld.h>
+#include <AnKi/Physics2/PhysicsCollisionShape.h>
+
+namespace anki {
+namespace v2 {
+
+thread_local static StaticTempAllocator<1_MB> g_tempAllocator;
+
+PhysicsPlayerController::~PhysicsPlayerController()
+{
+	m_jphCharacter.destroy();
+}
+
+void PhysicsPlayerController::init(const PhysicsPlayerControllerInitInfo& init)
+{
+	m_standingShape = PhysicsWorld::getSingleton().newCapsuleCollisionShape(init.m_standingHeight, init.m_waistWidth);
+	m_crouchingShape = PhysicsWorld::getSingleton().newCapsuleCollisionShape(init.m_crouchingHeight, init.m_waistWidth);
+
+	JPH::CharacterVirtualSettings settings;
+	settings.SetEmbedded();
+
+	settings.mMaxSlopeAngle = kMaxSlopeAngle;
+	settings.mMaxStrength = kMaxStrength;
+	settings.mShape = &m_standingShape->m_capsule;
+	settings.mBackFaceMode = kBackFaceMode;
+	settings.mCharacterPadding = kCharacterPadding;
+	settings.mPenetrationRecoverySpeed = kPenetrationRecoverySpeed;
+	settings.mPredictiveContactDistance = kPredictiveContactDistance;
+	settings.mSupportingVolume = JPH::Plane(JPH::Vec3::sAxisY(), -init.m_waistWidth); // Accept contacts that touch the lower sphere of the capsule
+	settings.mEnhancedInternalEdgeRemoval = kEnhancedInternalEdgeRemoval;
+	settings.mInnerBodyShape = nullptr;
+	m_jphCharacter.construct(&settings, toJPH(init.m_initialPosition), JPH::Quat::sIdentity(), ptrToNumber(this),
+							 &PhysicsWorld::getSingleton().m_jphPhysicsSystem);
+
+	m_position = init.m_initialPosition;
+}
+
+void PhysicsPlayerController::prePhysicsUpdate(Second dt)
+{
+	if(!m_input.m_updated)
+	{
+		return;
+	}
+
+	m_input.m_updated = false;
+
+	const Bool bJump = m_input.m_jumpSpeed > 0.0f;
+	const JPH::Vec3 inMovementDirection = toJPH(m_input.m_forwardDir);
+
+	const Bool playerControlsHorizontalVelocity = m_controlMovementDuringJump || m_jphCharacter->IsSupported();
+	if(playerControlsHorizontalVelocity)
+	{
+		// Smooth the player input
+		m_desiredVelocity = kEnableCharacterInertia ? 0.25f * inMovementDirection * m_input.m_forwardSpeed + 0.75f * m_desiredVelocity
+													: inMovementDirection * m_input.m_forwardSpeed;
+
+		// True if the player intended to move
+		m_allowSliding = !inMovementDirection.IsNearZero();
+	}
+	else
+	{
+		// While in air we allow sliding
+		m_allowSliding = true;
+	}
+
+	// Update the character rotation and its up vector to match the up vector set by the user settings
+	const JPH::Quat characterUpRotation = JPH::Quat::sEulerAngles(JPH::Vec3(0.0f, 0.0f, 0.0f));
+	m_jphCharacter->SetUp(characterUpRotation.RotateAxisY());
+	m_jphCharacter->SetRotation(characterUpRotation);
+
+	// A cheaper way to update the character's ground velocity,
+	// the platforms that the character is standing on may have changed velocity
+	m_jphCharacter->UpdateGroundVelocity();
+
+	// Determine new basic velocity
+	const JPH::Vec3 currentVerticalVelocity = m_jphCharacter->GetLinearVelocity().Dot(m_jphCharacter->GetUp()) * m_jphCharacter->GetUp();
+	const JPH::Vec3 groundVelocity = m_jphCharacter->GetGroundVelocity();
+	JPH::Vec3 newVelocity;
+	bool movingTowardsGround = (currentVerticalVelocity.GetY() - groundVelocity.GetY()) < 0.1f;
+	if(m_jphCharacter->GetGroundState() == JPH::CharacterVirtual::EGroundState::OnGround // If on ground
+	   && (kEnableCharacterInertia
+			   ? movingTowardsGround // Inertia enabled: And not moving away from ground
+			   : !m_jphCharacter->IsSlopeTooSteep(m_jphCharacter->GetGroundNormal()))) // Inertia disabled: And not on a slope that is too steep
+	{
+		// Assume velocity of ground when on ground
+		newVelocity = groundVelocity;
+
+		// Jump
+		if(bJump && movingTowardsGround)
+		{
+			newVelocity += m_input.m_jumpSpeed * m_jphCharacter->GetUp();
+		}
+	}
+	else
+	{
+		newVelocity = currentVerticalVelocity;
+	}
+
+	// Gravity
+	const auto& physicsSystem = *PhysicsWorld::getSingleton().m_jphPhysicsSystem;
+	newVelocity += (characterUpRotation * physicsSystem.GetGravity()) * F32(dt);
+
+	if(playerControlsHorizontalVelocity)
+	{
+		// Player input
+		newVelocity += characterUpRotation * m_desiredVelocity;
+	}
+	else
+	{
+		// Preserve horizontal velocity
+		const JPH::Vec3 currentHorizontalVelocity = m_jphCharacter->GetLinearVelocity() - currentVerticalVelocity;
+		newVelocity += currentHorizontalVelocity;
+	}
+
+	// Update character velocity
+	m_jphCharacter->SetLinearVelocity(newVelocity);
+
+	// Stance switch
+	if(m_crouching != m_input.m_crouching)
+	{
+		m_crouching = m_input.m_crouching;
+
+		const Bool isStanding = m_jphCharacter->GetShape() == &m_standingShape->m_shapeBase;
+		const JPH::Shape* shape = (isStanding) ? &m_crouchingShape->m_shapeBase : &m_standingShape->m_shapeBase;
+		if(m_jphCharacter->SetShape(shape, 1.5f * physicsSystem.GetPhysicsSettings().mPenetrationSlop,
+									physicsSystem.GetDefaultBroadPhaseLayerFilter(JPH::ObjectLayer(PhysicsLayer::kCharacter)),
+									physicsSystem.GetDefaultLayerFilter(JPH::ObjectLayer(PhysicsLayer::kCharacter)), {}, {}, g_tempAllocator))
+		{
+			// Not sure why Jolt does that, disable for now
+			// const JPH::Shape* innerShape = (isStanding) ? mInnerCrouchingShape : mInnerStandingShape;
+			// m_jphCharacter->SetInnerBodyShape(innerShape);
+		}
+	}
+}
+
+void PhysicsPlayerController::postPhysicsUpdate()
+{
+	const Vec3 newPos = toAnKi(m_jphCharacter->GetPosition());
+	if(newPos != m_position)
+	{
+		m_position = newPos;
+		++m_positionVersion;
+	}
+}
+
+} // namespace v2
+} // namespace anki

AnKi/Physics2/PhysicsPlayerController.h

@@ -0,0 +1,123 @@
+// Copyright (C) 2009-present, Panagiotis Christopoulos Charitos and contributors.
+// All rights reserved.
+// Code licensed under the BSD License.
+// http://www.anki3d.org/LICENSE
+
+#pragma once
+
+#include <AnKi/Physics2/Common.h>
+#include <AnKi/Util/ClassWrapper.h>
+
+namespace anki {
+namespace v2 {
+
+/// @addtogroup physics
+/// @{
+
+/// Init info for PhysicsPlayerController.
+class PhysicsPlayerControllerInitInfo
+{
+public:
+	F32 m_mass = 83.0f;
+
+	F32 m_standingHeight = 1.9f;
+	F32 m_crouchingHeight = 1.2f;
+	F32 m_waistWidth = 0.3f;
+
+	Vec3 m_initialPosition = Vec3(0.0f);
+
+	Bool m_controlMovementDuringJump = true;
+};
+
+/// A player controller that walks the world.
+class PhysicsPlayerController
+{
+	ANKI_PHYSICS_COMMON_FRIENDS
+	friend class PhysicsPlayerControllerPtrDeleter;
+
+public:
+	// Update the state machine
+	void updateState(F32 forwardSpeed, const Vec3& forwardDir, F32 jumpSpeed, Bool crouching)
+	{
+		m_input.m_forwardSpeed = forwardSpeed;
+		m_input.m_forwardDir = forwardDir;
+		m_input.m_jumpSpeed = jumpSpeed;
+		m_input.m_crouching = crouching;
+		m_input.m_updated = 1;
+	}
+
+	/// This is a deferred operation, will happen on the next PhysicsWorld::update.
+	void moveToPosition(const Vec3& position)
+	{
+		m_jphCharacter->SetPosition(toJPH(position));
+	}
+
+	const Vec3& getPosition(U32* version = nullptr) const
+	{
+		if(version)
+		{
+			*version = m_positionVersion;
+		}
+		return m_position;
+	}
+
+private:
+	static constexpr F32 kMaxSlopeAngle = toRad(45.0f);
+	static constexpr F32 kMaxStrength = 100.0f;
+	static constexpr JPH::EBackFaceMode kBackFaceMode = JPH::EBackFaceMode::CollideWithBackFaces;
+	static constexpr F32 kCharacterPadding = 0.02f;
+	static constexpr F32 kPenetrationRecoverySpeed = 1.0f;
+	static constexpr F32 kPredictiveContactDistance = 0.1f;
+	static constexpr Bool kEnhancedInternalEdgeRemoval = false;
+	static constexpr Bool kEnableCharacterInertia = true;
+
+	ClassWrapper<JPH::CharacterVirtual> m_jphCharacter;
+	PhysicsCollisionShapePtr m_standingShape;
+	PhysicsCollisionShapePtr m_crouchingShape;
+
+	JPH::Vec3 m_desiredVelocity = JPH::Vec3::sZero();
+
+	class
+	{
+	public:
+		F32 m_forwardSpeed = 0.0f;
+		Vec3 m_forwardDir;
+		F32 m_jumpSpeed = 0.0f;
+		U8 m_crouching : 1 = false;
+		U8 m_updated : 1 = false;
+	} m_input;
+
+	Vec3 m_position;
+	U32 m_positionVersion = 0;
+
+	mutable Atomic<U32> m_refcount = {0};
+
+	U32 m_arrayIndex : 29 = kMaxU32 >> 3u;
+	U32 m_controlMovementDuringJump : 1 = true;
+	U32 m_allowSliding : 1 = false;
+	U32 m_crouching : 1 = false;
+
+	PhysicsPlayerController() = default;
+
+	~PhysicsPlayerController();
+
+	void retain() const
+	{
+		m_refcount.fetchAdd(1);
+	}
+
+	U32 release() const
+	{
+		return m_refcount.fetchSub(1);
+	}
+
+	void init(const PhysicsPlayerControllerInitInfo& init);
+
+	void prePhysicsUpdate(Second dt);
+
+	void postPhysicsUpdate();
+};
+/// @}
+
+} // namespace v2
+} // end namespace anki

AnKi/Physics2/PhysicsWorld.cpp

@@ -70,43 +70,43 @@ static ObjectLayerPairFilterImpl g_objectLayerPairFilterImpl;
 
 void PhysicsCollisionShapePtrDeleter::operator()(PhysicsCollisionShape* ptr)
 {
+	ANKI_ASSERT(ptr);
 	PhysicsWorld& world = PhysicsWorld::getSingleton();
 
-	LockGuard lock(world.m_mtx);
+	LockGuard lock(world.m_collisionShapes.m_mtx);
 
-	world.m_collisionShapes.erase(ptr->m_arrayIndex);
+	world.m_collisionShapes.m_array.erase(ptr->m_arrayIndex);
 }
 
 void PhysicsBodyPtrDeleter::operator()(PhysicsBody* ptr)
 {
+	ANKI_ASSERT(ptr);
 	PhysicsWorld& world = PhysicsWorld::getSingleton();
 
-	LockGuard lock(world.m_mtx);
+	world.m_jphPhysicsSystem->GetBodyInterface().RemoveBody(ptr->m_jphBody->GetID());
+	world.m_jphPhysicsSystem->GetBodyInterface().DestroyBody(ptr->m_jphBody->GetID());
 
-	if(!ptr->m_jphBody->IsInBroadPhase())
-	{
-		// Hasn't been added yet to the physics system, do special cleanup
+	LockGuard lock(world.m_bodies.m_mtx);
+	world.m_bodies.m_array.erase(ptr->m_arrayIndex);
+	world.m_optimizeBroadphase = true;
+}
 
-		decltype(world.m_bodiesToAdd)::Iterator it = world.m_bodiesToAdd.getBegin();
-		while(it != world.m_bodiesToAdd.getEnd())
-		{
-			if(*it == ptr)
-			{
-				break;
-			}
-		}
+void PhysicsJointPtrDeleter::operator()(PhysicsJoint* ptr)
+{
+	ANKI_ASSERT(ptr);
+	PhysicsWorld& world = PhysicsWorld::getSingleton();
 
-		ANKI_ASSERT(it != world.m_bodiesToAdd.getEnd() && "Should have been in the \"toAdd\" list");
-		world.m_bodiesToAdd.erase(it);
+	LockGuard lock(world.m_joints.m_mtx);
+	world.m_joints.m_array.erase(ptr->m_arrayIndex);
+}
 
-		world.m_bodies.erase(ptr->m_arrayIndex);
-	}
-	else
-	{
-		// Deferred cleanup
+void PhysicsPlayerControllerPtrDeleter::operator()(PhysicsPlayerController* ptr)
+{
+	ANKI_ASSERT(ptr);
+	PhysicsWorld& world = PhysicsWorld::getSingleton();
 
-		world.m_bodiesToRemove.emplaceBack(ptr);
-	}
+	LockGuard lock(world.m_characters.m_mtx);
+	world.m_characters.m_array.erase(ptr->m_arrayIndex);
 }
 
 void PhysicsWorld::MyBodyActivationListener::OnBodyActivated([[maybe_unused]] const JPH::BodyID& inBodyID, U64 inBodyUserData)
@@ -123,10 +123,10 @@ void PhysicsWorld::MyBodyActivationListener::OnBodyDeactivated([[maybe_unused]]
 
 PhysicsWorld::~PhysicsWorld()
 {
-	removeBodies();
-
-	ANKI_ASSERT(m_collisionShapes.getSize() == 0);
-	ANKI_ASSERT(m_bodies.getSize() == 0);
+	ANKI_ASSERT(m_collisionShapes.m_array.getSize() == 0);
+	ANKI_ASSERT(m_bodies.m_array.getSize() == 0);
+	ANKI_ASSERT(m_joints.m_array.getSize() == 0);
+	ANKI_ASSERT(m_characters.m_array.getSize() == 0);
 
 	m_jobSystem.destroy();
 	m_jphPhysicsSystem.destroy();
@@ -207,30 +207,33 @@ Error PhysicsWorld::init(AllocAlignedCallback allocCb, void* allocCbData)
 
 void PhysicsWorld::update(Second dt)
 {
-	const Bool optimizeBroadphase = m_bodiesToAdd.getSize() || m_bodiesToRemove.getSize();
-
-	removeBodies();
-	addBodies();
-
-	if(optimizeBroadphase)
+	// Pre-update work
 	{
-		m_jphPhysicsSystem->OptimizeBroadPhase();
+		for(PhysicsPlayerController& charController : m_characters.m_array)
+		{
+			charController.prePhysicsUpdate(dt);
+		}
+
+		if(m_optimizeBroadphase)
+		{
+			m_optimizeBroadphase = false;
+			m_jphPhysicsSystem->OptimizeBroadPhase();
+		}
 	}
 
 	constexpr I32 collisionSteps = 1;
 	m_jphPhysicsSystem->Update(F32(dt), collisionSteps, &m_tempAllocator, &m_jobSystem);
 
-	// Update transforms
-	for(auto& body : m_bodies)
+	// Post-update work
 	{
-		if(body.m_activated)
+		for(PhysicsBody& body : m_bodies.m_array)
+		{
+			body.postPhysicsUpdate();
+		}
+
+		for(PhysicsPlayerController& charController : m_characters.m_array)
 		{
-			const Transform newTrf = toAnKi(body.m_jphBody->GetWorldTransform());
-			if(newTrf != body.m_worldTrf)
-			{
-				body.m_worldTrf = newTrf;
-				++body.m_worldTrfVersion;
-			}
+			charController.postPhysicsUpdate();
 		}
 	}
 }
@@ -238,11 +241,11 @@ void PhysicsWorld::update(Second dt)
 template<typename TJPHCollisionShape, typename... TArgs>
 PhysicsCollisionShapePtr PhysicsWorld::newCollisionShape(PhysicsCollisionShape::ShapeType type, TArgs&&... args)
 {
-	decltype(m_collisionShapes)::Iterator it;
+	decltype(m_collisionShapes.m_array)::Iterator it;
 
 	{
-		LockGuard lock(m_mtx);
-		it = m_collisionShapes.emplace(type);
+		LockGuard lock(m_collisionShapes.m_mtx);
+		it = m_collisionShapes.m_array.emplace(type);
 	}
 
 	ClassWrapper<TJPHCollisionShape>& classw = reinterpret_cast<ClassWrapper<TJPHCollisionShape>&>(it->m_shapeBase);
@@ -263,8 +266,24 @@ PhysicsCollisionShapePtr PhysicsWorld::newBoxCollisionShape(Vec3 extend)
 	return newCollisionShape<JPH::BoxShape>(PhysicsCollisionShape::ShapeType::kBox, toJPH(extend));
 }
 
+PhysicsCollisionShapePtr PhysicsWorld::newCapsuleCollisionShape(F32 height, F32 radius)
+{
+	return newCollisionShape<JPH::CapsuleShape>(PhysicsCollisionShape::ShapeType::kCapsule, height / 2.0f, radius);
+}
+
 PhysicsBodyPtr PhysicsWorld::newPhysicsBody(const PhysicsBodyInitInfo& init)
 {
+	PhysicsBody* newBody;
+	{
+		LockGuard lock(m_bodies.m_mtx);
+
+		m_optimizeBroadphase = true;
+		auto it = m_bodies.m_array.emplace();
+
+		newBody = &(*it);
+		newBody->m_arrayIndex = it.getArrayIndex();
+	}
+
 	const Vec3 pos = init.m_transform.getOrigin().xyz();
 	const Quat rot = Quat(init.m_transform.getRotation());
 
@@ -288,78 +307,71 @@ PhysicsBodyPtr PhysicsWorld::newPhysicsBody(const PhysicsBodyInitInfo& init)
 	}
 
 	settings.mFriction = init.m_friction;
+	settings.mUserData = ptrToNumber(newBody);
 
+	// Call the thread-safe version because characters will be creating bodies as well
 	JPH::Body* jphBody = m_jphPhysicsSystem->GetBodyInterface().CreateBody(settings);
+	m_jphPhysicsSystem->GetBodyInterface().AddBody(jphBody->GetID(), JPH::EActivation::Activate);
 
 	// Create AnKi body
-	LockGuard lock(m_mtx);
+	newBody->m_jphBody = jphBody;
+	newBody->m_primaryShape.reset(init.m_shape);
+	newBody->m_scaledShape = scaledShape;
+	newBody->m_worldTrf = init.m_transform;
 
-	auto it = m_bodies.emplace();
-	it->m_jphBody = jphBody;
-	it->m_arrayIndex = it.getArrayIndex();
-	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));
+	return PhysicsBodyPtr(newBody);
 }
 
-void PhysicsWorld::addBodies()
+template<typename TJPHJoint, typename... TArgs>
+PhysicsJointPtr PhysicsWorld::newJoint(PhysicsJoint::Type type, PhysicsBody* body1, PhysicsBody* body2, TArgs&&... args)
 {
-	if(m_bodiesToAdd.getSize() == 0) [[likely]]
-	{
-		return;
-	}
-
-	PhysicsDynamicArray<JPH::BodyID> jphBodies;
-	jphBodies.resize(m_bodiesToAdd.getSize());
+	ANKI_ASSERT(body1 && body2);
 
-	for(U32 i = 0; i < m_bodiesToAdd.getSize(); ++i)
+	decltype(m_joints.m_array)::Iterator it;
 	{
-		jphBodies[i] = m_bodiesToAdd[i]->m_jphBody->GetID();
+		LockGuard lock(m_joints.m_mtx);
+		it = m_joints.m_array.emplace(type);
 	}
 
-	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);
+	ClassWrapper<TJPHJoint>& classw = reinterpret_cast<ClassWrapper<TJPHJoint>&>(it->m_base);
+	classw.construct(*body1->m_jphBody, *body2->m_jphBody, std::forward<TArgs>(args)...);
+	classw->SetEmbedded();
 
-	for(U32 i = 0; i < m_bodiesToAdd.getSize(); ++i)
-	{
-		ANKI_ASSERT(m_bodiesToAdd[i]->m_jphBody->IsInBroadPhase());
-	}
+	it->m_body1.reset(body1);
+	it->m_body2.reset(body2);
+	it->m_arrayIndex = it.getArrayIndex();
+
+	m_jphPhysicsSystem->AddConstraint(&it->m_base); // It's thread-safe
 
-	m_bodiesToAdd.resize(0);
+	return PhysicsJointPtr(&(*it));
 }
 
-void PhysicsWorld::removeBodies()
+PhysicsJointPtr PhysicsWorld::newPointJoint(PhysicsBody* body1, PhysicsBody* body2, Bool pointsInWorldSpace, const Vec3& body1Point,
+											const Vec3& body2Point)
 {
-	if(m_bodiesToRemove.getSize() == 0) [[likely]]
-	{
-		return;
-	}
+	JPH::PointConstraintSettings settings;
+	settings.SetEmbedded();
 
-	// 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());
+	settings.mSpace = (pointsInWorldSpace) ? JPH::EConstraintSpace::WorldSpace : JPH::EConstraintSpace::LocalToBodyCOM;
+	settings.mPoint1 = toJPH(body1Point);
+	settings.mPoint2 = toJPH(body2Point);
 
-	// 2nd, delete the JPH bodies
-	m_jphPhysicsSystem->GetBodyInterface().DestroyBodies(jphBodies.getBegin(), jphBodies.getSize());
+	return newJoint<JPH::PointConstraint>(PhysicsJoint::Type::kPoint, body1, body2, settings);
+}
 
-	// 3rd, delete the AnKi bodies
-	for(U32 i = 0; i < m_bodiesToRemove.getSize(); ++i)
+PhysicsPlayerControllerPtr PhysicsWorld::newPlayerController(const PhysicsPlayerControllerInitInfo& init)
+{
+	PhysicsPlayerController* newChar;
 	{
-		m_bodies.erase(m_bodiesToRemove[i]->m_arrayIndex);
+		LockGuard lock(m_characters.m_mtx);
+
+		auto it = m_characters.m_array.emplace();
+		newChar = &(*it);
+		newChar->m_arrayIndex = it.getArrayIndex();
 	}
 
-	m_bodiesToRemove.resize(0);
+	newChar->init(init);
+	return PhysicsPlayerControllerPtr(newChar);
 }
 
 } // namespace v2

AnKi/Physics2/PhysicsWorld.h

@@ -8,6 +8,8 @@
 #include <AnKi/Physics2/Common.h>
 #include <AnKi/Physics2/PhysicsCollisionShape.h>
 #include <AnKi/Physics2/PhysicsBody.h>
+#include <AnKi/Physics2/PhysicsJoint.h>
+#include <AnKi/Physics2/PhysicsPlayerController.h>
 #include <AnKi/Util/BlockArray.h>
 
 namespace anki {
@@ -24,15 +26,23 @@ class PhysicsWorld : public MakeSingleton<PhysicsWorld>
 	friend class PhysicsCollisionShapePtrDeleter;
 	friend class PhysicsBodyPtrDeleter;
 	friend class PhysicsBody;
+	friend class PhysicsPlayerController;
+	friend class PhysicsPlayerControllerPtrDeleter;
+	friend class PhysicsJointPtrDeleter;
 
 public:
 	Error init(AllocAlignedCallback allocCb, void* allocCbData);
 
 	PhysicsCollisionShapePtr newSphereCollisionShape(F32 radius);
 	PhysicsCollisionShapePtr newBoxCollisionShape(Vec3 extend);
+	PhysicsCollisionShapePtr newCapsuleCollisionShape(F32 height, F32 radius); ///< Capsule axis is in Y.
 
 	PhysicsBodyPtr newPhysicsBody(const PhysicsBodyInitInfo& init);
 
+	PhysicsJointPtr newPointJoint(PhysicsBody* body1, PhysicsBody* body2, Bool pointsInWorldSpace, const Vec3& body1Point, const Vec3& body2Point);
+
+	PhysicsPlayerControllerPtr newPlayerController(const PhysicsPlayerControllerInitInfo& init);
+
 	void update(Second dt);
 
 private:
@@ -43,28 +53,34 @@ private:
 		virtual void OnBodyDeactivated(const JPH::BodyID& inBodyID, U64 inBodyUserData) override;
 	};
 
-	PhysicsBlockArray<PhysicsCollisionShape> m_collisionShapes;
-	PhysicsBlockArray<PhysicsBody> m_bodies;
+	template<typename T>
+	class ObjArray
+	{
+	public:
+		PhysicsBlockArray<T> m_array;
+		Mutex m_mtx;
+	};
 
 	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;
+	ObjArray<PhysicsCollisionShape> m_collisionShapes;
+	ObjArray<PhysicsBody> m_bodies;
+	ObjArray<PhysicsJoint> m_joints;
+	ObjArray<PhysicsPlayerController> m_characters;
 
-	Mutex m_mtx;
+	Bool m_optimizeBroadphase = true;
 
 	PhysicsWorld();
 
 	~PhysicsWorld();
 
-	void addBodies();
-	void removeBodies();
-
 	template<typename TJPHCollisionShape, typename... TArgs>
 	PhysicsCollisionShapePtr newCollisionShape(PhysicsCollisionShape::ShapeType type, TArgs&&... args);
+
+	template<typename TJPHJoint, typename... TArgs>
+	PhysicsJointPtr newJoint(PhysicsJoint::Type type, PhysicsBody* body1, PhysicsBody* body2, TArgs&&... args);
 };
 /// @}