Add debug draw in Jolt

AnKi/Physics2/CMakeLists.txt

@@ -1,5 +1,5 @@
 file(GLOB_RECURSE sources *.cpp)
 file(GLOB_RECURSE headers *.h)
 add_library(AnKiPhysics2 ${sources} ${headers})
-target_compile_definitions(AnKiPhysics2 PRIVATE -DANKI_SOURCE_FILE)
+target_compile_definitions(AnKiPhysics2 PRIVATE "-DANKI_SOURCE_FILE" "-DJPH_DEBUG_RENDERER")
 target_link_libraries(AnKiPhysics2 AnKiUtil)

AnKi/Physics2/Common.h

@@ -86,7 +86,7 @@ public:
 
 	PhysicsObjectType getType() const
 	{
-		return m_type;
+		return PhysicsObjectType(m_type);
 	}
 
 	void* getUserData() const
@@ -100,9 +100,17 @@ public:
 	}
 
 protected:
-	PhysicsObjectBase(PhysicsObjectType type, void* userData)
-		: m_userData(userData)
-		, m_type(type)
+	static constexpr U32 kTypeBits = 5u; ///< 5 is more than enough
+
+	void* m_userData = nullptr;
+
+	mutable Atomic<U32> m_refcount = {0};
+
+	U32 m_type : kTypeBits;
+	U32 m_blockArrayIndex : 32 - kTypeBits = kMaxU32 >> kTypeBits;
+
+	PhysicsObjectBase(PhysicsObjectType type)
+		: m_type(U32(type))
 	{
 		ANKI_ASSERT(type < PhysicsObjectType::kCount);
 	}
@@ -116,11 +124,6 @@ protected:
 	{
 		return m_refcount.fetchSub(1);
 	}
-
-private:
-	void* m_userData;
-	mutable Atomic<U32> m_refcount = {0};
-	PhysicsObjectType m_type;
 };
 
 /// The physics layers a PhysicsBody can be.

AnKi/Physics2/PhysicsBody.h

@@ -82,17 +82,16 @@ private:
 	PhysicsCollisionShapePtr m_primaryShape;
 	PhysicsCollisionShapePtr m_scaledShape;
 
+	PhysicsTriggerCallbacks* m_triggerCallbacks = nullptr;
+
 	Transform m_worldTrf;
 	U32 m_worldTrfVersion = 1;
 
-	PhysicsTriggerCallbacks* m_triggerCallbacks = nullptr;
-
-	U32 m_arrayIndex : 30 = kMaxU32 >> 2u;
 	U32 m_activated : 1 = false;
 	U32 m_isTrigger : 1 = false;
 
 	PhysicsBody()
-		: PhysicsObjectBase(PhysicsObjectType::kBody, nullptr)
+		: PhysicsObjectBase(PhysicsObjectType::kBody)
 	{
 	}
 

AnKi/Physics2/PhysicsCollisionShape.h

@@ -26,18 +26,6 @@ public:
 	PhysicsCollisionShape& operator=(const PhysicsCollisionShape&) = delete;
 
 private:
-	enum class ShapeType : U8
-	{
-		kBox,
-		kSphere,
-		kCapsule,
-		kConvex,
-		kTrimesh,
-		kScaled, ///< This is for internal use
-
-		kCount
-	};
-
 	union
 	{
 		ClassWrapper<JPH::Shape> m_shapeBase;
@@ -47,14 +35,9 @@ private:
 		ClassWrapper<JPH::ScaledShape> m_scaled; ///< We don't hold a reference to the target shape to avoid locking mutexes twice.
 	};
 
-	U32 m_arrayIndex = kMaxU32;
-	ShapeType m_type;
-
-	PhysicsCollisionShape(ShapeType type)
-		: PhysicsObjectBase(PhysicsObjectType::kCollisionShape, nullptr)
-		, m_type(type)
+	PhysicsCollisionShape()
+		: PhysicsObjectBase(PhysicsObjectType::kCollisionShape)
 	{
-		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/Physics2/PhysicsJoint.h

@@ -21,14 +21,6 @@ class PhysicsJoint : public PhysicsObjectBase
 	friend class PhysicsJointPtrDeleter;
 
 private:
-	enum class Type : U8
-	{
-		kPoint,
-		kHinge,
-
-		kCount
-	};
-
 	union
 	{
 		ClassWrapper<JPH::TwoBodyConstraint> m_base;
@@ -39,14 +31,9 @@ private:
 	PhysicsBodyPtr m_body1;
 	PhysicsBodyPtr m_body2;
 
-	U32 m_arrayIndex = kMaxU32;
-	Type m_type;
-
-	PhysicsJoint(Type type)
-		: PhysicsObjectBase(PhysicsObjectType::kJoint, nullptr)
-		, m_type(type)
+	PhysicsJoint()
+		: PhysicsObjectBase(PhysicsObjectType::kJoint)
 	{
-		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));
 	}

AnKi/Physics2/PhysicsPlayerController.h

@@ -95,13 +95,12 @@ private:
 	Vec3 m_position;
 	U32 m_positionVersion = 0;
 
-	U32 m_arrayIndex : 29 = kMaxU32 >> 3u;
 	U32 m_controlMovementDuringJump : 1 = true;
 	U32 m_allowSliding : 1 = false;
 	U32 m_crouching : 1 = false;
 
 	PhysicsPlayerController()
-		: PhysicsObjectBase(PhysicsObjectType::kPlayerController, nullptr)
+		: PhysicsObjectBase(PhysicsObjectType::kPlayerController)
 	{
 	}
 

AnKi/Physics2/PhysicsWorld.cpp

@@ -6,6 +6,8 @@
 #include <AnKi/Physics2/PhysicsWorld.h>
 #include <AnKi/Util/System.h>
 
+#include <Jolt/Renderer/DebugRendererSimple.h>
+
 namespace anki {
 namespace v2 {
 
@@ -229,7 +231,7 @@ void PhysicsCollisionShapePtrDeleter::operator()(PhysicsCollisionShape* ptr)
 
 	LockGuard lock(world.m_collisionShapes.m_mtx);
 
-	world.m_collisionShapes.m_array.erase(ptr->m_arrayIndex);
+	world.m_collisionShapes.m_array.erase(ptr->m_blockArrayIndex);
 }
 
 void PhysicsBodyPtrDeleter::operator()(PhysicsBody* ptr)
@@ -241,7 +243,7 @@ void PhysicsBodyPtrDeleter::operator()(PhysicsBody* ptr)
 	world.m_jphPhysicsSystem->GetBodyInterface().DestroyBody(ptr->m_jphBody->GetID());
 
 	LockGuard lock(world.m_bodies.m_mtx);
-	world.m_bodies.m_array.erase(ptr->m_arrayIndex);
+	world.m_bodies.m_array.erase(ptr->m_blockArrayIndex);
 	world.m_optimizeBroadphase = true;
 }
 
@@ -251,7 +253,7 @@ void PhysicsJointPtrDeleter::operator()(PhysicsJoint* ptr)
 	PhysicsWorld& world = PhysicsWorld::getSingleton();
 
 	LockGuard lock(world.m_joints.m_mtx);
-	world.m_joints.m_array.erase(ptr->m_arrayIndex);
+	world.m_joints.m_array.erase(ptr->m_blockArrayIndex);
 }
 
 void PhysicsPlayerControllerPtrDeleter::operator()(PhysicsPlayerController* ptr)
@@ -260,9 +262,34 @@ void PhysicsPlayerControllerPtrDeleter::operator()(PhysicsPlayerController* ptr)
 	PhysicsWorld& world = PhysicsWorld::getSingleton();
 
 	LockGuard lock(world.m_characters.m_mtx);
-	world.m_characters.m_array.erase(ptr->m_arrayIndex);
+	world.m_characters.m_array.erase(ptr->m_blockArrayIndex);
 }
 
+class PhysicsWorld::MyDebugRenderer final : public JPH::DebugRendererSimple
+{
+public:
+	PhysicsDebugDrawerInterface* m_interface = nullptr;
+
+	void DrawLine(JPH::RVec3Arg inFrom, JPH::RVec3Arg inTo, JPH::ColorArg inColor) override
+	{
+		const Array<Vec3, 2> line = {toAnKi(inFrom), toAnKi(inTo)};
+		m_interface->drawLines(line, {inColor.r, inColor.g, inColor.b, inColor.a});
+	}
+
+	void DrawTriangle(JPH::RVec3Arg inV1, JPH::RVec3Arg inV2, JPH::RVec3Arg inV3, JPH::ColorArg inColor,
+					  [[maybe_unused]] ECastShadow inCastShadow) override
+	{
+		Array<Vec3, 6> line = {toAnKi(inV1), toAnKi(inV2), toAnKi(inV2), toAnKi(inV3), toAnKi(inV3), toAnKi(inV1)};
+		m_interface->drawLines(line, {inColor.r, inColor.g, inColor.b, inColor.a});
+	}
+
+	void DrawText3D([[maybe_unused]] JPH::RVec3Arg inPosition, [[maybe_unused]] const std::string_view& inString,
+					[[maybe_unused]] JPH::ColorArg inColor, [[maybe_unused]] F32 inHeight) override
+	{
+		// Nothing for now
+	}
+};
+
 PhysicsWorld::~PhysicsWorld()
 {
 	ANKI_ASSERT(m_collisionShapes.m_array.getSize() == 0);
@@ -397,41 +424,41 @@ void PhysicsWorld::update(Second dt)
 }
 
 template<typename TJPHCollisionShape, typename... TArgs>
-PhysicsCollisionShapePtr PhysicsWorld::newCollisionShape(PhysicsCollisionShape::ShapeType type, TArgs&&... args)
+PhysicsCollisionShapePtr PhysicsWorld::newCollisionShape(TArgs&&... args)
 {
 	decltype(m_collisionShapes.m_array)::Iterator it;
 
 	{
 		LockGuard lock(m_collisionShapes.m_mtx);
-		it = m_collisionShapes.m_array.emplace(type);
+		it = m_collisionShapes.m_array.emplace();
 	}
 
 	ClassWrapper<TJPHCollisionShape>& classw = reinterpret_cast<ClassWrapper<TJPHCollisionShape>&>(it->m_shapeBase);
 	classw.construct(std::forward<TArgs>(args)...);
 	classw->SetEmbedded();
 
-	it->m_arrayIndex = it.getArrayIndex();
+	it->m_blockArrayIndex = it.getArrayIndex();
 	return PhysicsCollisionShapePtr(&(*it));
 }
 
 PhysicsCollisionShapePtr PhysicsWorld::newSphereCollisionShape(F32 radius)
 {
-	return newCollisionShape<JPH::SphereShape>(PhysicsCollisionShape::ShapeType::kSphere, radius);
+	return newCollisionShape<JPH::SphereShape>(radius);
 }
 
 PhysicsCollisionShapePtr PhysicsWorld::newBoxCollisionShape(Vec3 extend)
 {
-	return newCollisionShape<JPH::BoxShape>(PhysicsCollisionShape::ShapeType::kBox, toJPH(extend));
+	return newCollisionShape<JPH::BoxShape>(toJPH(extend));
 }
 
 PhysicsCollisionShapePtr PhysicsWorld::newCapsuleCollisionShape(F32 height, F32 radius)
 {
-	return newCollisionShape<JPH::CapsuleShape>(PhysicsCollisionShape::ShapeType::kCapsule, height / 2.0f, radius);
+	return newCollisionShape<JPH::CapsuleShape>(height / 2.0f, radius);
 }
 
 PhysicsCollisionShapePtr PhysicsWorld::newScaleCollisionObject(const Vec3& scale, PhysicsCollisionShape* baseShape)
 {
-	return newCollisionShape<JPH::ScaledShape>(PhysicsCollisionShape::ShapeType::kScaled, &baseShape->m_shapeBase, toJPH(scale));
+	return newCollisionShape<JPH::ScaledShape>(&baseShape->m_shapeBase, toJPH(scale));
 }
 
 PhysicsBodyPtr PhysicsWorld::newPhysicsBody(const PhysicsBodyInitInfo& init)
@@ -444,7 +471,7 @@ PhysicsBodyPtr PhysicsWorld::newPhysicsBody(const PhysicsBodyInitInfo& init)
 		auto it = m_bodies.m_array.emplace();
 
 		newBody = &(*it);
-		newBody->m_arrayIndex = it.getArrayIndex();
+		newBody->m_blockArrayIndex = it.getArrayIndex();
 	}
 
 	newBody->init(init);
@@ -452,14 +479,14 @@ PhysicsBodyPtr PhysicsWorld::newPhysicsBody(const PhysicsBodyInitInfo& init)
 }
 
 template<typename TJPHJoint, typename... TArgs>
-PhysicsJointPtr PhysicsWorld::newJoint(PhysicsJoint::Type type, PhysicsBody* body1, PhysicsBody* body2, TArgs&&... args)
+PhysicsJointPtr PhysicsWorld::newJoint(PhysicsBody* body1, PhysicsBody* body2, TArgs&&... args)
 {
 	ANKI_ASSERT(body1 && body2);
 
 	decltype(m_joints.m_array)::Iterator it;
 	{
 		LockGuard lock(m_joints.m_mtx);
-		it = m_joints.m_array.emplace(type);
+		it = m_joints.m_array.emplace();
 	}
 
 	ClassWrapper<TJPHJoint>& classw = reinterpret_cast<ClassWrapper<TJPHJoint>&>(it->m_base);
@@ -468,7 +495,7 @@ PhysicsJointPtr PhysicsWorld::newJoint(PhysicsJoint::Type type, PhysicsBody* bod
 
 	it->m_body1.reset(body1);
 	it->m_body2.reset(body2);
-	it->m_arrayIndex = it.getArrayIndex();
+	it->m_blockArrayIndex = it.getArrayIndex();
 
 	m_jphPhysicsSystem->AddConstraint(&it->m_base); // It's thread-safe
 
@@ -485,7 +512,7 @@ PhysicsJointPtr PhysicsWorld::newPointJoint(PhysicsBody* body1, PhysicsBody* bod
 	settings.mPoint1 = toJPH(body1Point);
 	settings.mPoint2 = toJPH(body2Point);
 
-	return newJoint<JPH::PointConstraint>(PhysicsJoint::Type::kPoint, body1, body2, settings);
+	return newJoint<JPH::PointConstraint>(body1, body2, settings);
 }
 
 PhysicsPlayerControllerPtr PhysicsWorld::newPlayerController(const PhysicsPlayerControllerInitInfo& init)
@@ -496,7 +523,7 @@ PhysicsPlayerControllerPtr PhysicsWorld::newPlayerController(const PhysicsPlayer
 
 		auto it = m_characters.m_array.emplace();
 		newChar = &(*it);
-		newChar->m_arrayIndex = it.getArrayIndex();
+		newChar->m_blockArrayIndex = it.getArrayIndex();
 	}
 
 	newChar->init(init);
@@ -586,5 +613,16 @@ Bool PhysicsWorld::castRayAllHitsInternal(const Vec3& rayStart, const Vec3& rayE
 	return results.getSize() > 0;
 }
 
+void PhysicsWorld::debugDraw(PhysicsDebugDrawerInterface& interface)
+{
+	MyDebugRenderer renderer;
+	renderer.m_interface = &interface;
+
+	JPH::BodyManager::DrawSettings settings;
+	m_jphPhysicsSystem->DrawBodies(settings, &renderer);
+
+	m_jphPhysicsSystem->DrawConstraints(&renderer);
+}
+
 } // namespace v2
 } // end namespace anki

AnKi/Physics2/PhysicsWorld.h

@@ -27,6 +27,14 @@ public:
 	Vec3 m_hitPosition; ///< In world space.
 };
 
+/// @memberof PhysicsWorld
+class PhysicsDebugDrawerInterface
+{
+public:
+	/// The implementer is responsible of batching lines together.
+	virtual void drawLines(ConstWeakArray<Vec3> lines, Array<U8, 4> color) = 0;
+};
+
 /// The master container for all physics related stuff.
 class PhysicsWorld : public MakeSingleton<PhysicsWorld>
 {
@@ -73,15 +81,28 @@ public:
 		return success;
 	}
 
+	void debugDraw(PhysicsDebugDrawerInterface& interface);
+
 private:
 	class MyBodyActivationListener;
 	class MyContactListener;
+	class MyDebugRenderer;
+
+	template<U32 kElementsPerBlock>
+	class BlockArrayConfig
+	{
+	public:
+		static constexpr U32 getElementCountPerBlock()
+		{
+			return kElementsPerBlock;
+		}
+	};
 
-	template<typename T>
+	template<typename T, U32 kElementsPerBlock>
 	class ObjArray
 	{
 	public:
-		PhysicsBlockArray<T> m_array;
+		PhysicsBlockArray<T, BlockArrayConfig<kElementsPerBlock>> m_array;
 		Mutex m_mtx;
 	};
 
@@ -96,10 +117,10 @@ private:
 	ClassWrapper<JPH::JobSystemThreadPool> m_jobSystem;
 	ClassWrapper<JPH::TempAllocatorImpl> m_tempAllocator;
 
-	ObjArray<PhysicsCollisionShape> m_collisionShapes;
-	ObjArray<PhysicsBody> m_bodies;
-	ObjArray<PhysicsJoint> m_joints;
-	ObjArray<PhysicsPlayerController> m_characters;
+	ObjArray<PhysicsCollisionShape, 32> m_collisionShapes;
+	ObjArray<PhysicsBody, 64> m_bodies;
+	ObjArray<PhysicsJoint, 16> m_joints;
+	ObjArray<PhysicsPlayerController, 8> m_characters;
 
 	DynamicArray<Contact> m_insertedContacts;
 	DynamicArray<Contact> m_deletedContacts;
@@ -116,10 +137,10 @@ private:
 	~PhysicsWorld();
 
 	template<typename TJPHCollisionShape, typename... TArgs>
-	PhysicsCollisionShapePtr newCollisionShape(PhysicsCollisionShape::ShapeType type, TArgs&&... args);
+	PhysicsCollisionShapePtr newCollisionShape(TArgs&&... args);
 
 	template<typename TJPHJoint, typename... TArgs>
-	PhysicsJointPtr newJoint(PhysicsJoint::Type type, PhysicsBody* body1, PhysicsBody* body2, TArgs&&... args);
+	PhysicsJointPtr newJoint(PhysicsBody* body1, PhysicsBody* body2, TArgs&&... args);
 
 	PhysicsCollisionShapePtr newScaleCollisionObject(const Vec3& scale, PhysicsCollisionShape* baseShape);