Working on debug renderer

CMakeLists.txt

@@ -181,6 +181,7 @@ SET (REACTPHYSICS3D_HEADERS
     "include/reactphysics3d/utils/Profiler.h"
     "include/reactphysics3d/utils/Logger.h"
     "include/reactphysics3d/utils/DefaultLogger.h"
+    "include/reactphysics3d/utils/DebugRenderer.h"
 )
 
 # Source files
@@ -271,6 +272,7 @@ SET (REACTPHYSICS3D_SOURCES
     "src/memory/MemoryManager.cpp"
     "src/utils/Profiler.cpp"
     "src/utils/DefaultLogger.cpp"
+    "src/utils/DebugRenderer.cpp"
 )
 
 # Create the library

include/reactphysics3d/body/CollisionBody.h

@@ -37,7 +37,6 @@
 namespace reactphysics3d {
 
 // Declarations
-struct ContactManifoldListElement;
 class Collider;
 class CollisionShape;
 class PhysicsWorld;

include/reactphysics3d/collision/CollisionCallback.h

@@ -37,7 +37,6 @@ namespace reactphysics3d {
 // Declarations
 class OverlappingPair;
 class ContactManifold;
-struct ContactManifoldListElement;
 class CollisionBody;
 class Collider;
 class MemoryManager;

include/reactphysics3d/collision/shapes/ConcaveMeshShape.h

@@ -208,6 +208,7 @@ class ConcaveMeshShape : public ConcaveShape {
         friend class ConvexTriangleAABBOverlapCallback;
         friend class ConcaveMeshRaycastCallback;
         friend class PhysicsCommon;
+        friend class DebugRenderer;
 };
 
 // Return the number of bytes used by the collision shape

include/reactphysics3d/collision/shapes/HeightFieldShape.h

@@ -189,6 +189,9 @@ inline size_t HeightFieldShape::getSizeInBytes() const {
 // Return the height of a given (x,y) point in the height field
 inline decimal HeightFieldShape::getHeightAt(int x, int y) const {
 
+    assert(x >= 0 && x < mNbColumns);
+    assert(y >= 0 && y < mNbRows);
+
     switch(mHeightDataType) {
         case HeightDataType::HEIGHT_FLOAT_TYPE : return ((float*)mHeightFieldData)[y * mNbColumns + x];
         case HeightDataType::HEIGHT_DOUBLE_TYPE : return ((double*)mHeightFieldData)[y * mNbColumns + x];

include/reactphysics3d/engine/PhysicsWorld.h

@@ -50,6 +50,7 @@
 #include <reactphysics3d/systems/ContactSolverSystem.h>
 #include <reactphysics3d/systems/DynamicsSystem.h>
 #include <reactphysics3d/engine/Islands.h>
+#include <reactphysics3d/utils/DebugRenderer.h>
 #include <sstream>
 
 /// Namespace ReactPhysics3D
@@ -183,6 +184,12 @@ class PhysicsWorld {
         /// Entity Manager for the ECS
         EntityManager mEntityManager;
 
+        /// Debug renderer
+        DebugRenderer mDebugRenderer;
+
+        /// True if debug rendering is enabled
+        bool mIsDebugRenderingEnabled;
+
         /// Collision Body Components
         CollisionBodyComponents mCollisionBodyComponents;
 
@@ -452,6 +459,15 @@ class PhysicsWorld {
         /// Return a pointer to a given RigidBody of the world
         RigidBody* getRigidBody(uint index) ;
 
+        /// Return true if the debug rendering is enabled
+        bool getIsDebugRenderingEnabled() const;
+
+        /// Set to true if debug rendering is enabled
+        void setIsDebugRenderingEnabled(bool isEnabled);
+
+        /// Return a reference to the Debug Renderer of the world
+        DebugRenderer& getDebugRenderer();
+
 #ifdef IS_PROFILING_ACTIVE
 
         /// Return a reference to the profiler
@@ -483,6 +499,7 @@ class PhysicsWorld {
         friend class SliderJoint;
         friend class CollisionCallback::CallbackData;
         friend class OverlapCallback::CallbackData;
+        friend class DebugRenderer;
 };
 
 // Set the collision dispatch configuration
@@ -864,6 +881,21 @@ inline RigidBody* PhysicsWorld::getRigidBody(uint index) {
     return mRigidBodies[index];
 }
 
+// Return true if the debug rendering is enabled
+inline bool PhysicsWorld::getIsDebugRenderingEnabled() const {
+    return mIsDebugRenderingEnabled;
+}
+
+// Set to true if debug rendering is enabled
+inline void PhysicsWorld::setIsDebugRenderingEnabled(bool isEnabled) {
+    mIsDebugRenderingEnabled = isEnabled;
+}
+
+// Return a reference to the Debug Renderer of the world
+inline DebugRenderer& PhysicsWorld::getDebugRenderer() {
+    return mDebugRenderer;
+}
+
 }
 
 #endif

include/reactphysics3d/systems/CollisionDetectionSystem.h

@@ -73,7 +73,7 @@ class CollisionDetectionSystem {
         // -------------------- Constants -------------------- //
 
         /// Maximum number of contact points in a reduced contact manifold
-        const int8 MAX_CONTACT_POINTS_IN_MANIFOLD = 4;
+        static const int8 MAX_CONTACT_POINTS_IN_MANIFOLD = 4;
 
         // -------------------- Attributes -------------------- //
 
@@ -263,6 +263,9 @@ class CollisionDetectionSystem {
         /// Report all triggers
         void reportTriggers(EventListener& eventListener, List<ContactPair>* contactPairs, List<ContactPair>& lostContactPairs);
 
+        /// Report all contacts for debug rendering
+        void reportDebugRenderingContacts(List<ContactPair>* contactPairs, List<ContactManifold>* manifolds, List<ContactPoint>* contactPoints, List<ContactPair>& lostContactPairs);
+
         /// Return the largest depth of all the contact points of a potential manifold
         decimal computePotentialManifoldLargestContactDepth(const ContactManifoldInfo& manifold,
                                                             const List<ContactPointInfo>& potentialContactPoints) const;
@@ -373,6 +376,7 @@ class CollisionDetectionSystem {
         friend class PhysicsWorld;
         friend class ConvexMeshShape;
         friend class RigidBody;
+        friend class DebugRenderer;
 };
 
 // Return a reference to the collision dispatch configuration

include/reactphysics3d/utils/DebugRenderer.h

@@ -0,0 +1,300 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2019 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_DEBUG_RENDERER_H
+#define REACTPHYSICS3D_DEBUG_RENDERER_H
+
+// Libraries
+#include <reactphysics3d/containers/List.h>
+#include <reactphysics3d/containers/Map.h>
+#include <reactphysics3d/mathematics/mathematics.h>
+#include <reactphysics3d/engine/EventListener.h>
+#include <string>
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Forward declarations
+class ConcaveMeshShape;
+class ConvexMeshShape;
+class HeightFieldShape;
+class Collider;
+class PhysicsWorld;
+
+// Class DebugRenderer
+/**
+ * This class is used to display physics debug information directly into the user application view.
+ * For instance, it is possible to display AABBs of colliders, colliders or contact points. This class
+ * can be used to get the debug information as lists of basic primitives (points, linges, triangles, ...).
+ * You can use this to render physics debug information in your simulation on top of your object. Note that
+ * you should use this only for debugging purpose and you should disable it when you compile the final release
+ * version of your application because computing/rendering phyiscs debug information can be expensive.
+ */
+class DebugRenderer : public EventListener {
+
+    public:
+
+		/// Enumeration with basic colors
+		enum class DebugColor {
+
+            RED = 0xff0000,
+            GREEN = 0x00ff00,
+            BLUE = 0x0000ff,
+            BLACK = 0x000000,
+            WHITE = 0xffffff,
+            YELLOW = 0xffff00,
+            MAGENTA = 0xff00ff,
+            CYAN = 0x00ffff,
+		};
+
+		/// Enumeration with debug item to renderer
+		enum class DebugItem {
+			COLLIDER_AABB				= 1 << 0,
+			COLLIDER_BROADPHASE_AABB	= 1 << 1,
+			COLLISION_SHAPE				= 1 << 2,
+			CONTACT_POINT				= 1 << 3,
+		};
+
+        /// Struture that represents a point of the DebugRenderer
+		struct DebugPoint {
+			
+			/// Constructor
+			DebugPoint(const Vector3& point, uint32 color) :point(point), color(color) {
+
+			}
+
+			Vector3 point;
+			uint32 color;
+		};
+
+		/// Struture that represents a line of the DebugRenderer
+		struct DebugLine {
+			
+			/// Constructor
+            DebugLine(const Vector3& point1, const Vector3& point2, uint32 color)
+                :point1(point1), color1(color), point2(point2), color2(color) {
+
+			}
+
+			Vector3 point1;
+            uint32 color1;
+            Vector3 point2;
+            uint32 color2;
+		};
+
+		/// Struture that represents a triangle of the DebugRenderer
+		struct DebugTriangle {
+			
+			/// Constructor
+			DebugTriangle(const Vector3& point1, const Vector3& point2, const Vector3& point3, uint32 color)
+                :point1(point1), color1(color), point2(point2), color2(color), point3(point3), color3(color) {
+
+			}
+
+			Vector3 point1;
+            uint32 color1;
+            Vector3 point2;
+            uint32 color2;
+            Vector3 point3;
+            uint32 color3;
+		};
+
+    private:
+
+		// -------------------- Constants -------------------- //
+
+		/// Number of sectors used to draw a sphere or a capsule
+        static constexpr int NB_SECTORS_SPHERE = 18;
+
+		/// Number of stacks used to draw a sphere or a capsule
+        static constexpr int NB_STACKS_SPHERE = 10;
+
+        /// Default radius of the sphere displayed to represent contact points
+        static constexpr decimal DEFAULT_CONTACT_POINT_SPHERE_RADIUS = decimal(0.1);
+
+		// -------------------- Attributes -------------------- //
+
+		/// Memory allocator
+		MemoryAllocator& mAllocator;
+
+		/// List with all the debug lines
+		List<DebugLine> mLines;
+
+		/// List with all the debug triangles
+		List<DebugTriangle> mTriangles;
+
+        /// 32-bits integer that contains all the flags of debug items to display
+		uint32 mDisplayedDebugItems;
+
+		/// Map a debug item with the color used to display it
+		Map<DebugItem, uint32> mMapDebugItemWithColor;
+
+        /// Radius of the sphere displayed to represent contact points
+        decimal mContactPointSphereRadius;
+
+        // -------------------- Methods -------------------- //
+
+		/// Draw an AABB
+		void drawAABB(const AABB& aabb, uint32 color);
+
+		/// Draw a box
+		void drawBox(const Transform& transform, const Vector3& extents, uint32 color);
+
+		/// Draw a sphere
+		void drawSphere(const Vector3& position, decimal radius, uint32 color);
+
+		/// Draw a capsule
+		void drawCapsule(const Transform& transform, decimal radius, decimal height, uint32 color);
+
+		/// Draw a convex mesh
+		void drawConvexMesh(const Transform& transform, const ConvexMeshShape* convexMesh, uint32 color);
+
+		/// Draw a concave mesh shape
+		void drawConcaveMeshShape(const Transform& transform, const ConcaveMeshShape* concaveMeshShape, uint32 color);
+
+		/// Draw a height field shape
+		void drawHeightFieldShape(const Transform& transform, const HeightFieldShape* heightFieldShape, uint32 color);
+
+		/// Draw the collision shape of a collider
+		void drawCollisionShapeOfCollider(const Collider* collider, uint32 color);
+
+    public :
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        DebugRenderer(MemoryAllocator& allocator);
+
+        /// Destructor
+        ~DebugRenderer();
+
+		/// Return the number of points
+		uint32 getNbPoints() const;
+
+		/// Return a reference to the list of points
+		const List<DebugPoint>& getPoints() const;
+
+		/// Return a pointer to the array of points
+		const DebugPoint* getPointsArray() const;
+
+		/// Return the number of lines
+		uint32 getNbLines() const;
+
+		/// Return a reference to the list of lines
+		const List<DebugLine>& getLines() const;
+
+		/// Return a pointer to the array of lines
+		const DebugLine* getLinesArray() const;
+
+		/// Return the number of triangles
+		uint32 getNbTriangles() const;
+
+		/// Return a reference to the list of triangles
+		const List<DebugTriangle>& getTriangles() const;
+
+		/// Return a pointer to the array of triangles
+		const DebugTriangle* getTrianglesArray() const;
+
+		/// Return whether a debug item is displayed or not
+		bool getIsDebugItemDisplayed(DebugItem item) const;
+
+		/// Set whether a debug info is displayed or not
+		void setIsDebugItemDisplayed(DebugItem item, bool isDisplayed);
+
+        /// Get the contact point sphere radius
+        decimal getContactPointSphereRadius() const;
+
+        /// Set the contact point sphere radius
+        void setContactPointSphereRadius(decimal radius);
+
+		/// Generate the rendering primitives (triangles, lines, ...) of a physics world
+		void computeDebugRenderingPrimitives(const PhysicsWorld& world);
+
+        /// Clear all the debugging primitives (points, lines, triangles, ...)
+        void reset();
+
+        /// Called when some contacts occur
+        virtual void onContact(const CollisionCallback::CallbackData& callbackData) override;
+};
+
+// Return the number of lines
+inline uint32 DebugRenderer::getNbLines() const {
+	return mLines.size();
+}
+
+// Return a reference to the list of lines
+inline const List<DebugRenderer::DebugLine>& DebugRenderer::getLines() const {
+	return mLines;
+}
+
+// Return a pointer to the array of lines
+inline const DebugRenderer::DebugLine* DebugRenderer::getLinesArray() const {
+	return &(mLines[0]);
+}
+
+// Return the number of triangles
+inline uint32 DebugRenderer::getNbTriangles() const {
+	return mTriangles.size();
+}
+
+// Return a reference to the list of triangles
+inline const List<DebugRenderer::DebugTriangle>& DebugRenderer::getTriangles() const {
+	return mTriangles;
+}
+
+// Return a pointer to the array of triangles
+inline const DebugRenderer::DebugTriangle* DebugRenderer::getTrianglesArray() const {
+	return &(mTriangles[0]);
+}
+
+// Return whether a debug item is displayed or not
+inline bool DebugRenderer::getIsDebugItemDisplayed(DebugItem item) const {
+	return mDisplayedDebugItems & static_cast<uint32>(item);
+}
+
+// Set whether a debug info is displayed or not
+inline void DebugRenderer::setIsDebugItemDisplayed(DebugItem item, bool isDisplayed) {
+	const uint32 itemFlag = static_cast<uint32>(item);
+	uint32 resetBit = ~(itemFlag);
+	mDisplayedDebugItems &= resetBit;
+	if (isDisplayed) {
+		mDisplayedDebugItems |= itemFlag;
+	}
+}
+
+// Get the contact point sphere radius
+inline decimal DebugRenderer::getContactPointSphereRadius() const {
+    return mContactPointSphereRadius;
+}
+
+// Set the contact point sphere radius
+inline void DebugRenderer::setContactPointSphereRadius(decimal radius) {
+    assert(radius > decimal(0.0));
+    mContactPointSphereRadius = radius;
+}
+
+}
+
+#endif

src/engine/PhysicsWorld.cpp

@@ -51,7 +51,7 @@ uint PhysicsWorld::mNbWorlds = 0;
  * @param profiler Pointer to the profiler
  */
 PhysicsWorld::PhysicsWorld(MemoryManager& memoryManager, const WorldSettings& worldSettings, Logger* logger, Profiler* profiler)
-              : mMemoryManager(memoryManager), mConfig(worldSettings), mEntityManager(mMemoryManager.getHeapAllocator()),
+              : mMemoryManager(memoryManager), mConfig(worldSettings), mEntityManager(mMemoryManager.getHeapAllocator()), mDebugRenderer(mMemoryManager.getHeapAllocator()),
                 mCollisionBodyComponents(mMemoryManager.getHeapAllocator()), mRigidBodyComponents(mMemoryManager.getHeapAllocator()),
                 mTransformComponents(mMemoryManager.getHeapAllocator()), mCollidersComponents(mMemoryManager.getHeapAllocator()),
                 mJointsComponents(mMemoryManager.getHeapAllocator()), mBallAndSocketJointsComponents(mMemoryManager.getHeapAllocator()),
@@ -331,6 +331,11 @@ void PhysicsWorld::update(decimal timeStep) {
 
     RP3D_PROFILE("PhysicsWorld::update()", mProfiler);
 
+    // Reset the debug renderer
+    if (mIsDebugRenderingEnabled) {
+        mDebugRenderer.reset();
+    }
+
     // Compute the collision detection
     mCollisionDetection.computeCollisionDetection();
 
@@ -369,6 +374,11 @@ void PhysicsWorld::update(decimal timeStep) {
     // Reset the islands
     mIslands.clear();
 
+    // Generate debug rendering primitives (if enabled)
+    if (mIsDebugRenderingEnabled) {
+        mDebugRenderer.computeDebugRenderingPrimitives(*this);
+    }
+
     // Reset the single frame memory allocator
     mMemoryManager.resetFrameAllocator();
 }

src/systems/CollisionDetectionSystem.cpp

@@ -1386,18 +1386,25 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold
 // Report contacts and triggers
 void CollisionDetectionSystem::reportContactsAndTriggers() {
 
-   if (mWorld->mEventListener != nullptr) {
+    // Report contacts and triggers to the user
+    if (mWorld->mEventListener != nullptr) {
 
         reportContacts(*(mWorld->mEventListener), mCurrentContactPairs, mCurrentContactManifolds, mCurrentContactPoints, mLostContactPairs);
         reportTriggers(*(mWorld->mEventListener), mCurrentContactPairs, mLostContactPairs);
-   }
+    }
 
-   mOverlappingPairs.updateCollidingInPreviousFrame();
+    // Report contacts for debug rendering (if enabled)
+    if (mWorld->mIsDebugRenderingEnabled) {
 
-   mLostContactPairs.clear(true);
+        reportDebugRenderingContacts(mCurrentContactPairs, mCurrentContactManifolds, mCurrentContactPoints, mLostContactPairs);
+    }
+
+    mOverlappingPairs.updateCollidingInPreviousFrame();
+
+    mLostContactPairs.clear(true);
 }
 
-// Report all contacts
+// Report all contacts to the user
 void CollisionDetectionSystem::reportContacts(CollisionCallback& callback, List<ContactPair>* contactPairs,
                                               List<ContactManifold>* manifolds, List<ContactPoint>* contactPoints, List<ContactPair>& lostContactPairs) {
 
@@ -1413,7 +1420,7 @@ void CollisionDetectionSystem::reportContacts(CollisionCallback& callback, List<
     }
 }
 
-// Report all triggers
+// Report all triggers to the user
 void CollisionDetectionSystem::reportTriggers(EventListener& eventListener, List<ContactPair>* contactPairs, List<ContactPair>& lostContactPairs) {
 
     RP3D_PROFILE("CollisionDetectionSystem::reportTriggers()", mProfiler);
@@ -1428,6 +1435,21 @@ void CollisionDetectionSystem::reportTriggers(EventListener& eventListener, List
     }
 }
 
+// Report all contacts for debug rendering
+void CollisionDetectionSystem::reportDebugRenderingContacts(List<ContactPair>* contactPairs, List<ContactManifold>* manifolds, List<ContactPoint>* contactPoints, List<ContactPair>& lostContactPairs) {
+
+    RP3D_PROFILE("CollisionDetectionSystem::reportDebugRenderingContacts()", mProfiler);
+
+    // If there are contacts
+    if (contactPairs->size() + lostContactPairs.size() > 0) {
+
+        CollisionCallback::CallbackData callbackData(contactPairs, manifolds, contactPoints, lostContactPairs, *mWorld);
+
+        // Call the callback method to report the contacts
+        mWorld->mDebugRenderer.onContact(callbackData);
+    }
+}
+
 // Return true if two bodies overlap (collide)
 bool CollisionDetectionSystem::testOverlap(CollisionBody* body1, CollisionBody* body2) {
 

src/utils/DebugRenderer.cpp

@@ -0,0 +1,462 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2019 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include <reactphysics3d/utils/DebugRenderer.h>
+#include <reactphysics3d/memory/MemoryManager.h>
+#include <cassert>
+#include <reactphysics3d/collision/shapes/ConvexMeshShape.h>
+#include <reactphysics3d/collision/shapes/ConcaveMeshShape.h>
+#include <reactphysics3d/collision/shapes/HeightFieldShape.h>
+#include <reactphysics3d/collision/shapes/BoxShape.h>
+#include <reactphysics3d/collision/shapes/SphereShape.h>
+#include <reactphysics3d/collision/shapes/CapsuleShape.h>
+#include <reactphysics3d/collision/Collider.h>
+#include <reactphysics3d/engine/PhysicsWorld.h>
+#include <reactphysics3d/containers/Pair.h>
+
+using namespace reactphysics3d;
+
+// Constructor
+DebugRenderer::DebugRenderer(MemoryAllocator& allocator)
+              :mAllocator(allocator), mLines(allocator), mTriangles(allocator), mDisplayedDebugItems(0), mMapDebugItemWithColor(allocator),
+               mContactPointSphereRadius(DEFAULT_CONTACT_POINT_SPHERE_RADIUS) {
+
+    mMapDebugItemWithColor.add(Pair<DebugItem, uint32>(DebugItem::COLLIDER_AABB, static_cast<uint32>(DebugColor::MAGENTA)));
+	mMapDebugItemWithColor.add(Pair<DebugItem, uint32>(DebugItem::COLLIDER_BROADPHASE_AABB, static_cast<uint32>(DebugColor::YELLOW)));
+	mMapDebugItemWithColor.add(Pair<DebugItem, uint32>(DebugItem::COLLISION_SHAPE, static_cast<uint32>(DebugColor::GREEN)));
+    mMapDebugItemWithColor.add(Pair<DebugItem, uint32>(DebugItem::CONTACT_POINT, static_cast<uint32>(DebugColor::RED)));
+}
+
+// Destructor
+DebugRenderer::~DebugRenderer() {
+
+}
+
+// Clear all the debugging primitives (points, lines, triangles, ...)
+void DebugRenderer::reset() {
+
+	mLines.clear();
+	mTriangles.clear();
+}
+
+// Draw an AABB
+void DebugRenderer::drawAABB(const AABB& aabb, uint32 color) {
+	
+	const Vector3& min = aabb.getMin();
+	const Vector3& max = aabb.getMax();
+
+	// Bottom edges
+	mLines.add(DebugLine(Vector3(min.x, min.y, max.z), Vector3(max.x, min.y, max.z), color));
+	mLines.add(DebugLine(Vector3(max.x, min.y, max.z),  Vector3(max.x, min.y, min.z), color));
+	mLines.add(DebugLine(Vector3(max.x, min.y, min.z), Vector3(min.x, min.y, min.z), color));
+	mLines.add(DebugLine(Vector3(min.x, min.y, min.z), Vector3(min.x, min.y, max.z), color));
+
+	// Top edges
+	mLines.add(DebugLine(Vector3(min.x, max.y, max.z), Vector3(max.x, max.y, max.z), color));
+	mLines.add(DebugLine(Vector3(max.x, max.y, max.z), Vector3(max.x, max.y, min.z), color));
+	mLines.add(DebugLine(Vector3(max.x, max.y, min.z), Vector3(min.x, max.y, min.z), color));
+	mLines.add(DebugLine(Vector3(min.x, max.y, min.z), Vector3(min.x, max.y, max.z), color));
+
+	// Side edges
+	mLines.add(DebugLine(Vector3(min.x, min.y, max.z), Vector3(min.x, max.y, max.z), color));
+	mLines.add(DebugLine(Vector3(max.x, min.y, max.z), Vector3(max.x, max.y, max.z), color));
+	mLines.add(DebugLine(Vector3(max.x, min.y, min.z), Vector3(max.x, max.y, min.z), color));
+	mLines.add(DebugLine(Vector3(min.x, min.y, min.z), Vector3(min.x, max.y, min.z), color));
+}
+
+// Draw a box
+void DebugRenderer::drawBox(const Transform& transform, const Vector3& halfExtents, uint32 color) {
+
+	Vector3 vertices[8];
+
+	// Vertices
+	vertices[0] = transform * Vector3(-halfExtents.x, -halfExtents.y, halfExtents.z);
+	vertices[1] = transform * Vector3(halfExtents.x, -halfExtents.y, halfExtents.z);
+	vertices[2] = transform * Vector3(halfExtents.x, -halfExtents.y, -halfExtents.z);
+	vertices[3] = transform * Vector3(-halfExtents.x, -halfExtents.y, -halfExtents.z);
+	vertices[4] = transform * Vector3(-halfExtents.x, halfExtents.y, halfExtents.z);
+	vertices[5] = transform * Vector3(halfExtents.x, halfExtents.y, halfExtents.z);
+	vertices[6] = transform * Vector3(halfExtents.x, halfExtents.y, -halfExtents.z);
+	vertices[7] = transform * Vector3(-halfExtents.x, halfExtents.y, -halfExtents.z);
+
+	// Triangle faces
+	mTriangles.add(DebugTriangle(vertices[0], vertices[1], vertices[5], color));
+	mTriangles.add(DebugTriangle(vertices[0], vertices[5], vertices[4], color));
+	mTriangles.add(DebugTriangle(vertices[1], vertices[2], vertices[6], color));
+	mTriangles.add(DebugTriangle(vertices[1], vertices[6], vertices[5], color));
+	mTriangles.add(DebugTriangle(vertices[2], vertices[3], vertices[6], color));
+	mTriangles.add(DebugTriangle(vertices[3], vertices[7], vertices[6], color));
+	mTriangles.add(DebugTriangle(vertices[0], vertices[7], vertices[3], color));
+	mTriangles.add(DebugTriangle(vertices[0], vertices[4], vertices[7], color));
+	mTriangles.add(DebugTriangle(vertices[0], vertices[2], vertices[1], color));
+	mTriangles.add(DebugTriangle(vertices[0], vertices[3], vertices[2], color));
+	mTriangles.add(DebugTriangle(vertices[5], vertices[6], vertices[4], color));
+	mTriangles.add(DebugTriangle(vertices[4], vertices[6], vertices[7], color));
+}
+
+/// Draw a sphere
+void DebugRenderer::drawSphere(const Vector3& position, decimal radius, uint32 color) {
+
+    Vector3 vertices[(NB_SECTORS_SPHERE + 1) * (NB_STACKS_SPHERE + 1) + (NB_SECTORS_SPHERE + 1)];
+	
+	// Vertices
+	const decimal sectorStep = 2 * PI / NB_SECTORS_SPHERE;
+	const decimal stackStep = PI / NB_STACKS_SPHERE;
+	
+	for (uint i = 0; i <= NB_STACKS_SPHERE; i++) {
+
+		const decimal stackAngle = PI / 2 - i * stackStep;
+		const decimal radiusCosStackAngle = radius * std::cos(stackAngle);
+		const decimal z = radius * std::sin(stackAngle);
+
+        for (uint j = 0; j <= NB_SECTORS_SPHERE; j++) {
+		
+			const decimal sectorAngle = j * sectorStep;
+			const decimal x = radiusCosStackAngle * std::cos(sectorAngle);
+			const decimal y = radiusCosStackAngle * std::sin(sectorAngle);
+
+            vertices[i * (NB_SECTORS_SPHERE + 1) + j] = position + Vector3(x, y, z);
+		}
+	}
+
+	// Faces
+	for (uint i = 0; i < NB_STACKS_SPHERE; i++) {
+
+		uint a1 = i * (NB_SECTORS_SPHERE + 1);
+		uint a2 = a1 + NB_SECTORS_SPHERE + 1;
+
+		for (uint j = 0; j < NB_SECTORS_SPHERE; j++, a1++, a2++) {
+		
+			// 2 triangles per sector except for the first and last stacks
+
+			if (i != 0) {
+			
+				mTriangles.add(DebugTriangle(vertices[a1], vertices[a2], vertices[a1 + 1], color));
+			}
+
+			if (i != (NB_STACKS_SPHERE - 1)) {
+				
+				mTriangles.add(DebugTriangle(vertices[a1 + 1], vertices[a2], vertices[a2 + 1], color));
+			}
+		}
+	}
+}
+
+// Draw a capsule
+void DebugRenderer::drawCapsule(const Transform& transform, decimal radius, decimal height, uint32 color) {
+
+    Vector3 vertices[(NB_SECTORS_SPHERE + 1) * (NB_STACKS_SPHERE + 1) + (NB_SECTORS_SPHERE + 1)];
+
+	const decimal halfHeight = 0.5 * height;
+
+	// Use an even number of stacks
+    const uint nbStacks = NB_STACKS_SPHERE % 2 == 0 ? NB_STACKS_SPHERE : NB_STACKS_SPHERE - 1;
+	const uint nbHalfStacks = nbStacks / 2;
+	
+	// Vertices
+	const decimal sectorStep = 2 * PI / NB_SECTORS_SPHERE;
+	const decimal stackStep = PI / nbStacks;
+	
+	uint vertexIndex = 0;
+	
+	// Top cap sphere vertices
+    for (uint i = 0; i <= nbHalfStacks; i++) {
+
+		const decimal stackAngle = PI / 2 - i * stackStep;
+		const decimal radiusCosStackAngle = radius * std::cos(stackAngle);
+        const decimal y = radius * std::sin(stackAngle);
+
+        for (uint j = 0; j <= NB_SECTORS_SPHERE; j++) {
+		
+			const decimal sectorAngle = j * sectorStep;
+            const decimal x = radiusCosStackAngle * std::sin(sectorAngle);
+            const decimal z = radiusCosStackAngle * std::cos(sectorAngle);
+
+            assert(vertexIndex < (NB_SECTORS_SPHERE + 1) * (nbStacks + 1) + (NB_SECTORS_SPHERE + 1));
+			vertices[vertexIndex] = transform * Vector3(x, y + halfHeight, z);
+
+			vertexIndex++;
+		}
+	}
+
+	// Bottom cap sphere vertices
+    for (uint i = 0; i <= nbHalfStacks; i++) {
+
+        const decimal stackAngle = PI / 2 - (nbHalfStacks + i) * stackStep;
+		const decimal radiusCosStackAngle = radius * std::cos(stackAngle);
+        const decimal y = radius * std::sin(stackAngle);
+
+        for (uint j = 0; j <= NB_SECTORS_SPHERE; j++) {
+		
+			const decimal sectorAngle = j * sectorStep;
+            const decimal x = radiusCosStackAngle * std::sin(sectorAngle);
+            const decimal z = radiusCosStackAngle * std::cos(sectorAngle);
+
+            assert(vertexIndex < (NB_SECTORS_SPHERE + 1) * (nbStacks + 1) + (NB_SECTORS_SPHERE + 1));
+            vertices[vertexIndex] = transform * Vector3(x, y - halfHeight, z);
+
+			vertexIndex++;
+		}
+	}
+
+	// Faces of the top cap sphere
+	for (uint i = 0; i < nbHalfStacks; i++) {
+
+		uint a1 = i * (NB_SECTORS_SPHERE + 1);
+		uint a2 = a1 + NB_SECTORS_SPHERE + 1;
+
+		for (uint j = 0; j < NB_SECTORS_SPHERE; j++, a1++, a2++) {
+		
+			// 2 triangles per sector except for the first stack
+
+            if (i != 0) {
+
+                mTriangles.add(DebugTriangle(vertices[a1], vertices[a2], vertices[a1 + 1], color));
+            }
+
+            mTriangles.add(DebugTriangle(vertices[a1 + 1], vertices[a2], vertices[a2 + 1], color));
+        }
+	}
+
+	// Faces of the bottom cap sphere
+	for (uint i = 0; i < nbHalfStacks; i++) {
+
+        uint a1 = (nbHalfStacks + 1) * (NB_SECTORS_SPHERE + 1) + i * (NB_SECTORS_SPHERE + 1);
+		uint a2 = a1 + NB_SECTORS_SPHERE + 1;
+
+		for (uint j = 0; j < NB_SECTORS_SPHERE; j++, a1++, a2++) {
+		
+			// 2 triangles per sector except for the last stack
+
+            mTriangles.add(DebugTriangle(vertices[a1], vertices[a2], vertices[a1 + 1], color));
+
+            if (i != (nbHalfStacks - 1)) {
+
+                mTriangles.add(DebugTriangle(vertices[a1 + 1], vertices[a2], vertices[a2 + 1], color));
+            }
+        }
+	}
+
+	// Faces of the cylinder between the two spheres
+    uint a1 = nbHalfStacks * (NB_SECTORS_SPHERE + 1);
+    uint a2 = a1 + NB_SECTORS_SPHERE + 1;
+    for (uint i = 0; i < NB_SECTORS_SPHERE; i++, a1++, a2++) {
+
+		mTriangles.add(DebugTriangle(vertices[a1 + 1], vertices[a2], vertices[a2 + 1], color));
+	}
+}
+
+// Draw a convex mesh
+void DebugRenderer::drawConvexMesh(const Transform& transform, const ConvexMeshShape* convexMesh, uint32 color) {
+
+	// For each face of the convex mesh
+	for (uint32 f = 0; f < convexMesh->getNbFaces(); f++) {
+
+		const HalfEdgeStructure::Face& face = convexMesh->getFace(f);
+		assert(face.faceVertices.size() >= 3);
+
+		// Perform a fan triangulation of the convex polygon face
+		for (uint32 v = 2; v < face.faceVertices.size(); v++) {
+
+			uint v1Index = face.faceVertices[v - 2];
+			uint v2Index = face.faceVertices[v - 1];
+			uint v3Index = face.faceVertices[v];
+
+            Vector3 v1 = convexMesh->getVertexPosition(v1Index);
+            Vector3 v2 = convexMesh->getVertexPosition(v2Index);
+            Vector3 v3 = convexMesh->getVertexPosition(v3Index);
+
+            v1 = transform * v1;
+            v2 = transform * v2;
+            v3 = transform * v3;
+
+            mTriangles.add(DebugTriangle(v1, v2, v3, color));
+		}
+	}
+}
+
+// Draw a concave mesh shape
+void DebugRenderer::drawConcaveMeshShape(const Transform& transform, const ConcaveMeshShape* concaveMeshShape, uint32 color) {
+
+	// For each sub-part of the mesh
+	for (uint p = 0; p < concaveMeshShape->getNbSubparts(); p++) {
+
+		// For each triangle of the sub-part
+		for (uint t = 0; t < concaveMeshShape->getNbTriangles(p); t++) {
+			
+			Vector3 triangleVertices[3];
+			concaveMeshShape->getTriangleVertices(p, t, triangleVertices);
+
+            triangleVertices[0] = transform * triangleVertices[0];
+            triangleVertices[1] = transform * triangleVertices[1];
+            triangleVertices[2] = transform * triangleVertices[2];
+
+            mTriangles.add(DebugTriangle(triangleVertices[0], triangleVertices[1], triangleVertices[2], color));
+		}
+	}
+}
+
+// Draw a height field shape
+void DebugRenderer::drawHeightFieldShape(const Transform& transform, const HeightFieldShape* heightFieldShape, uint32 color) {
+
+    // For each sub-grid points (except the last ones one each dimension)
+    for (int i = 0; i < heightFieldShape->getNbColumns() - 1; i++) {
+        for (int j = 0; j < heightFieldShape->getNbRows() - 1; j++) {
+
+            // Compute the four point of the current quad
+            Vector3 p1 = heightFieldShape->getVertexAt(i, j);
+            Vector3 p2 = heightFieldShape->getVertexAt(i, j + 1);
+            Vector3 p3 = heightFieldShape->getVertexAt(i + 1, j);
+            Vector3 p4 = heightFieldShape->getVertexAt(i + 1, j + 1);
+
+            p1 = transform * p1;
+            p2 = transform * p2;
+            p3 = transform * p3;
+            p4 = transform * p4;
+
+            mTriangles.add(DebugTriangle(p1, p2, p3, color));
+			mTriangles.add(DebugTriangle(p3, p2, p4, color));
+       }
+   }
+}
+
+// Draw the collision shape of a collider
+void DebugRenderer::drawCollisionShapeOfCollider(const Collider* collider, uint32 color) {
+	
+    switch (collider->getCollisionShape()->getName()) {
+		
+        case CollisionShapeName::BOX:
+        {
+            const BoxShape* boxShape = static_cast<const BoxShape*>(collider->getCollisionShape());
+            drawBox(collider->getLocalToWorldTransform(), boxShape->getHalfExtents(), color);
+            break;
+        }
+        case CollisionShapeName::SPHERE:
+        {
+            const SphereShape* sphereShape = static_cast<const SphereShape*>(collider->getCollisionShape());
+            drawSphere(collider->getLocalToWorldTransform().getPosition(), sphereShape->getRadius(), color);
+            break;
+        }
+        case CollisionShapeName::CAPSULE:
+        {
+            const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(collider->getCollisionShape());
+            drawCapsule(collider->getLocalToWorldTransform(), capsuleShape->getRadius(), capsuleShape->getHeight(), color);
+            break;
+        }
+        case CollisionShapeName::CONVEX_MESH:
+        {
+            const ConvexMeshShape*  convexMeshShape = static_cast<const ConvexMeshShape*>(collider->getCollisionShape());
+            drawConvexMesh(collider->getLocalToWorldTransform(), convexMeshShape, color);
+            break;
+        }
+        case CollisionShapeName::TRIANGLE_MESH:
+        {
+            const ConcaveMeshShape* concaveMeshShape = static_cast<const ConcaveMeshShape*>(collider->getCollisionShape());
+            drawConcaveMeshShape(collider->getLocalToWorldTransform(), concaveMeshShape, color);
+            break;
+        }
+        case CollisionShapeName::HEIGHTFIELD:
+        {
+            const HeightFieldShape* heighFieldShape = static_cast<const HeightFieldShape*>(collider->getCollisionShape());
+            drawHeightFieldShape(collider->getLocalToWorldTransform(), heighFieldShape, color);
+            break;
+        }
+        default:
+        {
+            assert(false);
+        }
+    }
+}
+
+// Generate the rendering primitives (triangles, lines, ...) of a physics world
+void DebugRenderer::computeDebugRenderingPrimitives(const PhysicsWorld& world) {
+
+	const bool drawColliderAABB = getIsDebugItemDisplayed(DebugItem::COLLIDER_AABB);
+	const bool drawColliderBroadphaseAABB = getIsDebugItemDisplayed(DebugItem::COLLIDER_BROADPHASE_AABB);
+	const bool drawCollisionShape = getIsDebugItemDisplayed(DebugItem::COLLISION_SHAPE);
+	
+    const uint nbCollisionBodies = world.getNbCollisionBodies();
+    const uint nbRigidBodies = world.getNbRigidBodies();
+
+    // For each body of the world
+    for (uint b = 0; b < nbCollisionBodies + nbRigidBodies; b++) {
+
+		// Get a body
+        const CollisionBody* body = b < nbCollisionBodies ? world.getCollisionBody(b) : world.getRigidBody(b - nbCollisionBodies);
+
+		// For each collider of the body
+		for (uint c = 0; c < body->getNbColliders(); c++) {
+
+			// Get a collider
+            const Collider* collider = body->getCollider(c);
+
+			// If we need to draw the collider AABB
+			if (drawColliderAABB) {
+
+				drawAABB(collider->getWorldAABB(), mMapDebugItemWithColor[DebugItem::COLLIDER_AABB]);
+            }
+
+			// If we need to draw the collider broad-phase AABB
+			if (drawColliderBroadphaseAABB) {
+
+				drawAABB(world.mCollisionDetection.mBroadPhaseSystem.getFatAABB(collider->getBroadPhaseId()), mMapDebugItemWithColor[DebugItem::COLLIDER_BROADPHASE_AABB]);
+			}
+
+			// If we need to draw the collision shape
+			if (drawCollisionShape) {
+
+				drawCollisionShapeOfCollider(collider, mMapDebugItemWithColor[DebugItem::COLLISION_SHAPE]);
+			}
+		}
+    }
+}
+
+// Called when some contacts occur
+void DebugRenderer::onContact(const CollisionCallback::CallbackData& callbackData) {
+
+	// If we need to draw contact points
+	if (getIsDebugItemDisplayed(DebugItem::CONTACT_POINT)) {
+
+		// For each contact pair
+		for (uint p = 0; p < callbackData.getNbContactPairs(); p++) {
+
+			CollisionCallback::ContactPair contactPair = callbackData.getContactPair(p);
+
+            if (contactPair.getEventType() != CollisionCallback::ContactPair::EventType::ContactExit) {
+
+                // For each contact point of the contact pair
+                for (uint c = 0; c < contactPair.getNbContactPoints(); c++) {
+
+                    CollisionCallback::ContactPoint contactPoint = contactPair.getContactPoint(c);
+
+                    Vector3 point = contactPair.getCollider1()->getLocalToWorldTransform() * contactPoint.getLocalPointOnShape1();
+
+                    drawSphere(point, DEFAULT_CONTACT_POINT_SPHERE_RADIUS, mMapDebugItemWithColor[DebugItem::CONTACT_POINT]);
+                }
+            }
+		}
+	}
+}

testbed/common/AABB.cpp

@@ -87,7 +87,8 @@ void AABB::render(const openglframework::Vector3& position, const openglframewor
 
     // Set the vertex color
     openglframework::Vector4 colorVec(color.r, color.g, color.b, color.a);
-    shader.setVector4Uniform("vertexColor", colorVec, false);
+    shader.setIntUniform("isGlobalVertexColorEnabled", 1, false);
+    shader.setVector4Uniform("globalVertexColor", colorVec, false);
 
     // Bind the VAO
     mVAO.bind();

testbed/common/Box.cpp

@@ -122,7 +122,7 @@ void Box::render(openglframework::Shader& shader, const openglframework::Matrix4
     rp3d::RigidBody* rigidBody = dynamic_cast<rp3d::RigidBody*>(mBody);
     openglframework::Color currentColor = rigidBody != nullptr && rigidBody->isSleeping() ? mSleepingColor : mColor;
     openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a);
-    shader.setVector4Uniform("vertexColor", color, false);
+    shader.setVector4Uniform("globalVertexColor", color, false);
 
     // Bind the VAO
     mVAO.bind();

testbed/common/Capsule.cpp

@@ -117,7 +117,7 @@ void Capsule::render(openglframework::Shader& shader,
     rp3d::RigidBody* rigidBody = dynamic_cast<rp3d::RigidBody*>(mBody);
     openglframework::Color currentColor = rigidBody != nullptr && rigidBody->isSleeping() ? mSleepingColor : mColor;
     openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a);
-    shader.setVector4Uniform("vertexColor", color, false);
+    shader.setVector4Uniform("globalVertexColor", color, false);
 
     // Bind the VAO
     mVAO.bind();

testbed/common/ConcaveMesh.cpp

@@ -118,7 +118,7 @@ void ConcaveMesh::render(openglframework::Shader& shader,
     rp3d::RigidBody* rigidBody = dynamic_cast<rp3d::RigidBody*>(mBody);
     openglframework::Color currentColor = rigidBody != nullptr && rigidBody->isSleeping() ? mSleepingColor : mColor;
     openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a);
-    shader.setVector4Uniform("vertexColor", color, false);
+    shader.setVector4Uniform("globalVertexColor", color, false);
 
     // Bind the VAO
     mVAO.bind();

testbed/common/ConvexMesh.cpp

@@ -136,7 +136,7 @@ void ConvexMesh::render(openglframework::Shader& shader,
     rp3d::RigidBody* rigidBody = dynamic_cast<rp3d::RigidBody*>(mBody);
     openglframework::Color currentColor = rigidBody != nullptr && rigidBody->isSleeping() ? mSleepingColor : mColor;
     openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a);
-    shader.setVector4Uniform("vertexColor", color, false);
+    shader.setVector4Uniform("globalVertexColor", color, false);
 
     // Bind the VAO
     mVAO.bind();

testbed/common/Dumbbell.cpp

@@ -139,7 +139,7 @@ void Dumbbell::render(openglframework::Shader& shader,
     rp3d::RigidBody* rigidBody = dynamic_cast<rp3d::RigidBody*>(mBody);
     openglframework::Color currentColor = rigidBody != nullptr && rigidBody->isSleeping() ? mSleepingColor : mColor;
     openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a);
-    shader.setVector4Uniform("vertexColor", color, false);
+    shader.setVector4Uniform("globalVertexColor", color, false);
 
     // Bind the VAO
     mVAO.bind();

testbed/common/HeightField.cpp

@@ -108,7 +108,7 @@ void HeightField::render(openglframework::Shader& shader,
     rp3d::RigidBody* rigidBody = dynamic_cast<rp3d::RigidBody*>(mBody);
     openglframework::Color currentColor = rigidBody != nullptr && rigidBody->isSleeping() ? mSleepingColor : mColor;
     openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a);
-    shader.setVector4Uniform("vertexColor", color, false);
+    shader.setVector4Uniform("globalVertexColor", color, false);
 
     // Bind the VAO
     mVAO.bind();

testbed/common/Line.cpp

@@ -40,8 +40,7 @@ Line::~Line() {
 }
 
 // Render the sphere at the correct position and with the correct orientation
-void Line::render(openglframework::Shader& shader,
+void Line::render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) {
-                    const openglframework::Matrix4& worldToCameraMatrix) {
 
     // Bind the shader
     shader.bind();
@@ -52,7 +51,8 @@ void Line::render(openglframework::Shader& shader,
 
     // Set the vertex color
     openglframework::Vector4 color(1, 0, 0, 1);
-    shader.setVector4Uniform("vertexColor", color, false);
+    shader.setIntUniform("isGlobalVertexColorEnabled", 1, false);
+    shader.setVector4Uniform("globalVertexColor", color, false);
 
     /*
     glBegin(GL_LINES);

testbed/common/Sphere.cpp

@@ -115,7 +115,7 @@ void Sphere::render(openglframework::Shader& shader, const openglframework::Matr
     rp3d::RigidBody* rigidBody = dynamic_cast<rp3d::RigidBody*>(mBody);
     openglframework::Color currentColor = rigidBody != nullptr && rigidBody->isSleeping() ? mSleepingColor : mColor;
     openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a);
-    shader.setVector4Uniform("vertexColor", color, false);
+    shader.setVector4Uniform("globalVertexColor", color, false);
 
     // Bind the VAO
     mVAO.bind();

testbed/common/VisualContactPoint.cpp

@@ -101,7 +101,8 @@ void VisualContactPoint::render(openglframework::Shader& shader, const openglfra
 
 	// Set the vertex color
 	openglframework::Vector4 color(mColor.r, mColor.g, mColor.b, mColor.a);
-	shader.setVector4Uniform("vertexColor", color, false);
+    shader.setIntUniform("isGlobalVertexColorEnabled", 1, false);
+    shader.setVector4Uniform("globalVertexColor", color, false);
 
     mVBOVertices.bind();
 
@@ -172,7 +173,8 @@ void VisualContactPoint::renderContactNormalLine(openglframework::Shader& shader
 
 	// Set the vertex color
 	openglframework::Vector4 color(0, 1, 0, 1);
-	shader.setVector4Uniform("vertexColor", color, false);
+    shader.setIntUniform("isGlobalVertexColorEnabled", 1, false);
+    shader.setVector4Uniform("globalVertexColor", color, false);
 
 	// Get the location of shader attribute variables
 	GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition");

testbed/opengl-framework/src/Shader.h

@@ -176,7 +176,7 @@ inline GLint Shader::getAttribLocation(const std::string& variableName, bool err
     GLint location = glGetAttribLocation(mProgramObjectID, variableName.c_str());
     if (location == -1 && errorIfMissing) {
         std::cerr << "Error in vertex shader " << mFilenameVertexShader << " or in fragment shader"
-                  << mFilenameFragmentShader << " : No Uniform variable : " << variableName
+                  << mFilenameFragmentShader << " : No variable : " << variableName
                   << std::endl;
         throw std::logic_error("Error in Shader");
     }

testbed/opengl-framework/src/VertexBufferObject.cpp

@@ -67,6 +67,16 @@ void VertexBufferObject::copyDataIntoVBO(GLsizei size, const void* data, GLenum
     glBufferData(mTargetData, size, data, usage);
 }
 
+// Map VBO data memory into client's memory
+void* VertexBufferObject::mapBuffer(GLenum access) {
+    return glMapBuffer(mTargetData, access);
+}
+
+// Unmap VBO data memory from client's memory
+void VertexBufferObject::unmapBuffer() {
+    glUnmapBuffer(mTargetData);
+}
+
 // Destroy the VBO
 void VertexBufferObject::destroy() {
 

testbed/opengl-framework/src/VertexBufferObject.h

@@ -65,6 +65,12 @@ class VertexBufferObject {
         /// Copy data into the VBO
         void copyDataIntoVBO(GLsizei size, const void* data, GLenum usage);
 
+        /// Map VBO data memory into client's memory
+        void* mapBuffer(GLenum access);
+
+        /// Unmap VBO data memory from client's memory
+        void unmapBuffer();
+
         /// Bind the VBO
         void bind() const;
 

testbed/scenes/collisionshapes/CollisionShapesScene.cpp

@@ -53,6 +53,13 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name, EngineSettin
     physicsWorld->setEventListener(this);
     mPhysicsWorld = physicsWorld;
 
+    // TODO : Do not enable by default
+    mPhysicsWorld->setIsDebugRenderingEnabled(true);
+    mPhysicsWorld->getDebugRenderer().setIsDebugItemDisplayed(rp3d::DebugRenderer::DebugItem::COLLIDER_AABB, true);
+    mPhysicsWorld->getDebugRenderer().setIsDebugItemDisplayed(rp3d::DebugRenderer::DebugItem::COLLIDER_BROADPHASE_AABB, true);
+    mPhysicsWorld->getDebugRenderer().setIsDebugItemDisplayed(rp3d::DebugRenderer::DebugItem::COLLISION_SHAPE, true);
+    mPhysicsWorld->getDebugRenderer().setIsDebugItemDisplayed(rp3d::DebugRenderer::DebugItem::CONTACT_POINT, true);
+
     for (int i=0; i<NB_COMPOUND_SHAPES; i++) {
 
         // Create a convex mesh and a corresponding rigid in the physics world

testbed/scenes/collisionshapes/CollisionShapesScene.h

@@ -42,11 +42,11 @@ namespace collisionshapesscene {
 
 // Constants
 const float SCENE_RADIUS = 30.0f;
-const int NB_BOXES = 5;
+const int NB_BOXES = 2;
-const int NB_SPHERES = 5;
+const int NB_SPHERES = 2;
-const int NB_CAPSULES = 5;
+const int NB_CAPSULES = 2;
-const int NB_MESHES = 4;
+const int NB_MESHES = 2;
-const int NB_COMPOUND_SHAPES = 3;
+const int NB_COMPOUND_SHAPES = 1;
 const openglframework::Vector3 BOX_SIZE(2, 2, 2);
 const float SPHERE_RADIUS = 1.5f;
 const float CONE_RADIUS = 2.0f;

testbed/scenes/cubes/CubesScene.h

@@ -36,7 +36,7 @@ namespace cubesscene {
 
 // Constants
 const float SCENE_RADIUS = 30.0f;                           // Radius of the scene in meters
-const int NB_CUBES = 30;                                    // Number of boxes in the scene
+const int NB_CUBES = 1;                                    // Number of boxes in the scene
 const openglframework::Vector3 BOX_SIZE(2, 2, 2);          // Box dimensions in meters
 const openglframework::Vector3 FLOOR_SIZE(50, 1, 50);   // Floor dimensions in meters
 

testbed/scenes/heightfield/HeightFieldScene.cpp

@@ -53,7 +53,7 @@ HeightFieldScene::HeightFieldScene(const std::string& name, EngineSettings& sett
     physicsWorld->setEventListener(this);
     mPhysicsWorld = physicsWorld;
 
-	for (int i = 0; i<NB_COMPOUND_SHAPES; i++) {
+    for (int i = 0; i<NB_COMPOUND_SHAPES; i++) {
 
         // Create a convex mesh and a corresponding rigid in the physics world
         Dumbbell* dumbbell = new Dumbbell(true, mPhysicsCommon, mPhysicsWorld, meshFolderPath);

testbed/scenes/raycast/RaycastScene.cpp

@@ -309,7 +309,8 @@ void RaycastScene::renderSinglePass(openglframework::Shader& shader, const openg
 
     // Set the vertex color
     openglframework::Vector4 color(1, 0.55f, 0, 1);
-	mColorShader.setVector4Uniform("vertexColor", color, false);
+    mColorShader.setIntUniform("isGlobalVertexColorEnabled", 1, false);
+    mColorShader.setVector4Uniform("globalVertexColor", color, false);
 
     // Get the location of shader attribute variables
     GLint vertexPositionLoc = mColorShader.getAttribLocation("vertexPosition");

testbed/shaders/color.frag

@@ -26,13 +26,22 @@
 ********************************************************************************/
 
 // Uniform variables
-uniform vec4 vertexColor;                   // Vertex color
+uniform bool isGlobalVertexColorEnabled;          // True if we need to use the global vertex color
+uniform vec4 globalVertexColor;                   // Vertex color
 
-// Out variable
+// In variables
+in vec4 vertexColorOut;
+
+// Out variables
 out vec4 color;                        // Output color
 
 void main() {
 
     // Compute the final color
-    color = vertexColor;
+    if (isGlobalVertexColorEnabled) {
+        color = globalVertexColor;
+    }
+    else {
+        color = vertexColorOut;
+    }
 }

testbed/shaders/color.vert

@@ -32,6 +32,10 @@ uniform mat4 projectionMatrix;          // Projection matrix
 
 // In variables
 in vec4 vertexPosition;
+in uint vertexColor;
+
+// Out variables
+out vec4 vertexColorOut;
 
 void main() {
 
@@ -40,4 +44,7 @@ void main() {
 
     // Compute the clip-space vertex coordinates
     gl_Position = projectionMatrix * positionCameraSpace;
+
+    // Transfer the vertex color to the fragment shader
+    vertexColorOut = vec4((vertexColor & 0xFF0000u) >> 16, (vertexColor & 0x00FF00u) >> 8, vertexColor & 0x0000FFu, 0xFF);
 }

testbed/shaders/phong.frag

@@ -38,7 +38,7 @@ uniform sampler2D shadowMapSampler0;      // Shadow map texture sampler
 uniform sampler2D shadowMapSampler1;      // Shadow map texture sampler
 uniform sampler2D shadowMapSampler2;      // Shadow map texture sampler
 uniform bool isTexture;                     // True if we need to use the texture
-uniform vec4 vertexColor;                   // Vertex color
+uniform vec4 globalVertexColor;                   // Vertex color
 uniform bool isShadowEnabled;               // True if shadow mapping is enabled
 uniform vec2 shadowMapDimension;            // Shadow map dimension
 
@@ -64,7 +64,7 @@ void main() {
 	vec3 ambient = lightAmbientColor;
 
 	// Get the texture color
-	vec3 textureColor = vertexColor.rgb;
+        vec3 textureColor = globalVertexColor.rgb;
 	if (isTexture) textureColor = texture(textureSampler, texCoords).rgb;
 
 	// Compute the surface normal vector

testbed/src/SceneDemo.cpp

@@ -45,8 +45,8 @@ SceneDemo::SceneDemo(const std::string& name, EngineSettings& settings, bool isP
                      mPhongShader("shaders/phong.vert", "shaders/phong.frag"),
 					 mColorShader("shaders/color.vert", "shaders/color.frag"),
                      mQuadShader("shaders/quad.vert", "shaders/quad.frag"),
-                     mVBOQuad(GL_ARRAY_BUFFER), mMeshFolderPath("meshes/"),
+                     mVBOQuad(GL_ARRAY_BUFFER), mDebugVBOLinesVertices(GL_ARRAY_BUFFER), mDebugVBOTrianglesVertices(GL_ARRAY_BUFFER),
-                     mPhysicsWorld(nullptr), mIsPhysicsWorldSimulated(isPhysicsWorldSimulated) {
+                     mMeshFolderPath("meshes/"), mPhysicsWorld(nullptr), mIsPhysicsWorldSimulated(isPhysicsWorldSimulated) {
 
     shadowMapTextureLevel++;
 
@@ -91,6 +91,8 @@ SceneDemo::SceneDemo(const std::string& name, EngineSettings& settings, bool isP
 
     createQuadVBO();
 
+    createDebugVBO();
+
     // Init rendering for the AABBs
     AABB::init();
 
@@ -153,6 +155,9 @@ void SceneDemo::updatePhysics() {
 // Render the scene (in multiple passes for shadow mapping)
 void SceneDemo::render() {
 
+    // Update the VBO for the debug infos
+    updateDebugVBO();
+
     glEnable(GL_DEPTH_TEST);
     glEnable(GL_CULL_FACE);
 
@@ -274,6 +279,11 @@ void SceneDemo::render() {
         renderAABBs(worldToCameraMatrix);
     }
 
+    // Render the debug infos
+    if (mPhysicsWorld->getIsDebugRenderingEnabled()) {
+        renderDebugInfos(mColorShader, worldToCameraMatrix);
+    }
+
 	// Is shadow mapping is enabled
 	if (mIsShadowMappingEnabled) {
 
@@ -367,6 +377,44 @@ void SceneDemo::createQuadVBO() {
     mVAOQuad.unbind();
 }
 
+// Create a the VAO and VBOs to render the debug infos
+void SceneDemo::createDebugVBO() {
+
+    // ----- Lines ----- //
+
+    // Create the VBO for the vertices data
+    mDebugVBOLinesVertices.create();
+
+    // Create the VAO for both VBOs
+    mDebugLinesVAO.create();
+    mDebugLinesVAO.bind();
+
+    // Bind the VBO of vertices
+    mDebugVBOLinesVertices.bind();
+
+    // Unbind the VAO
+    mDebugLinesVAO.unbind();
+
+    mDebugVBOLinesVertices.unbind();
+
+    // ----- Triangles ----- //
+
+    // Create the VBO for the vertices data
+    mDebugVBOTrianglesVertices.create();
+
+    // Create the VAO for both VBOs
+    mDebugTrianglesVAO.create();
+    mDebugTrianglesVAO.bind();
+
+    // Bind the VBO of vertices
+    mDebugVBOTrianglesVertices.bind();
+
+    // Unbind the VAO
+    mDebugTrianglesVAO.unbind();
+
+    mDebugVBOTrianglesVertices.unbind();
+}
+
 void SceneDemo::drawTextureQuad() {
 
     glViewport(mViewportX, mViewportY, mViewportWidth, mViewportHeight);
@@ -437,6 +485,127 @@ void SceneDemo::renderContactPoints(openglframework::Shader& shader, const openg
     }
 }
 
+// Update VBO with vertices and indices of debug info
+void SceneDemo::updateDebugVBO() {
+
+    rp3d::DebugRenderer& debugRenderer = mPhysicsWorld->getDebugRenderer();
+
+    if (mPhysicsWorld->getIsDebugRenderingEnabled()) {
+
+        // ----- Lines ---- //
+
+        const uint nbLines = debugRenderer.getNbLines();
+
+        if (nbLines > 0) {
+
+            // Vertices
+            mDebugVBOLinesVertices.bind();
+            GLsizei sizeVertices = static_cast<GLsizei>(nbLines * sizeof(rp3d::DebugRenderer::DebugLine));
+            mDebugVBOLinesVertices.copyDataIntoVBO(sizeVertices, debugRenderer.getLinesArray(), GL_STREAM_DRAW);
+            mDebugVBOLinesVertices.unbind();
+        }
+
+        // ----- Triangles ---- //
+
+        const uint nbTriangles = debugRenderer.getNbTriangles();
+
+        if (nbTriangles > 0) {
+
+            // Vertices
+            mDebugVBOTrianglesVertices.bind();
+            GLsizei sizeVertices = static_cast<GLsizei>(nbTriangles * sizeof(rp3d::DebugRenderer::DebugTriangle));
+            mDebugVBOTrianglesVertices.copyDataIntoVBO(sizeVertices, debugRenderer.getTrianglesArray(), GL_STREAM_DRAW);
+            mDebugVBOTrianglesVertices.unbind();
+        }
+    }
+}
+
+// Render Debug Infos
+void SceneDemo::renderDebugInfos(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) {
+
+    rp3d::DebugRenderer& debugRenderer = mPhysicsWorld->getDebugRenderer();
+
+    // Render in wireframe mode
+    glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
+
+    // Bind the shader
+    shader.bind();
+
+    // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the
+    // model-view matrix)
+    const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix;
+    const openglframework::Matrix3 normalMatrix = localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose();
+    shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false);
+
+    // Set the model to camera matrix
+    shader.setMatrix4x4Uniform("localToWorldMatrix", openglframework::Matrix4::identity());
+    shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix);
+
+    shader.setIntUniform("isGlobalVertexColorEnabled", 0, false);
+
+    // Get the location of shader attribute variables
+    GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition");
+    GLint vertexColorLoc = shader.getAttribLocation("vertexColor");
+
+    // Lines
+    if (debugRenderer.getNbLines() > 0) {
+
+        // Bind the VAO
+        mDebugLinesVAO.bind();
+
+        mDebugVBOLinesVertices.bind();
+
+        glEnableVertexAttribArray(vertexPositionLoc);
+        glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, sizeof(rp3d::Vector3) + sizeof(rp3d::uint32), (char*)nullptr);
+
+        glEnableVertexAttribArray(vertexColorLoc);
+        glVertexAttribIPointer(vertexColorLoc, 3, GL_UNSIGNED_INT, sizeof(rp3d::Vector3) + sizeof(rp3d::uint32), (void*)sizeof(rp3d::Vector3));
+
+        // Draw the lines geometry
+        glDrawArrays(GL_LINES, 0, debugRenderer.getNbLines() * 2);
+
+        glDisableVertexAttribArray(vertexPositionLoc);
+        glDisableVertexAttribArray(vertexColorLoc);
+
+        mDebugVBOLinesVertices.unbind();
+
+        // Unbind the VAO
+        mDebugLinesVAO.unbind();
+    }
+
+    // Triangles
+    if (debugRenderer.getNbTriangles() > 0) {
+
+        // Bind the VAO
+        mDebugTrianglesVAO.bind();
+
+        mDebugVBOTrianglesVertices.bind();
+
+        glEnableVertexAttribArray(vertexPositionLoc);
+        glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, sizeof(rp3d::Vector3) + sizeof(rp3d::uint32), (char*)nullptr);
+
+        glEnableVertexAttribArray(vertexColorLoc);
+        glVertexAttribIPointer(vertexColorLoc, 3, GL_UNSIGNED_INT, sizeof(rp3d::Vector3) + sizeof(rp3d::uint32), (void*)sizeof(rp3d::Vector3));
+
+        // Draw the triangles geometry
+        glDrawArrays(GL_TRIANGLES, 0, debugRenderer.getNbTriangles() * 3);
+
+        glDisableVertexAttribArray(vertexPositionLoc);
+        glDisableVertexAttribArray(vertexColorLoc);
+
+        mDebugVBOTrianglesVertices.unbind();
+
+        // Unbind the VAO
+        mDebugTrianglesVAO.unbind();
+    }
+
+    // Unbind the shader
+    shader.unbind();
+
+    // Disable wireframe mode
+    glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
+}
+
 // Render the AABBs
 void SceneDemo::renderAABBs(const openglframework::Matrix4& worldToCameraMatrix) {
 

testbed/src/SceneDemo.h

@@ -95,6 +95,18 @@ class SceneDemo : public Scene {
 
         openglframework::VertexBufferObject mVBOQuad;
 
+        /// Vertex Buffer Object for the debug info lines vertices data
+        openglframework::VertexBufferObject mDebugVBOLinesVertices;
+
+        /// Vertex Array Object for the lines vertex data
+        openglframework::VertexArrayObject mDebugLinesVAO;
+
+        /// Vertex Buffer Object for the debug info trinangles vertices data
+        openglframework::VertexBufferObject mDebugVBOTrianglesVertices;
+
+        /// Vertex Array Object for the triangles vertex data
+        openglframework::VertexArrayObject mDebugTrianglesVAO;
+
         static openglframework::Color mObjectColorDemo;
         static openglframework::Color mFloorColorDemo;
         static openglframework::Color mSleepingColorDemo;
@@ -113,22 +125,31 @@ class SceneDemo : public Scene {
 
         // -------------------- Methods -------------------- //
 
-        // Create the Shadow map FBO and texture
+        /// Create the Shadow map FBO and texture
         void createShadowMapFBOAndTexture();
 
-        // Used for debugging shadow maps
+        /// Used for debugging shadow maps
         void createQuadVBO();
 
-        // TODO : Delete this
+        /// Create a the VAO and VBOs to render the debug infos
+        void createDebugVBO();
+
+        /// TODO : Delete this
         void drawTextureQuad();
 
-        // Update the contact points
+        /// Update the contact points
         void updateContactPoints();
 
-        // Render the contact points
+        /// Render the contact points
         void renderContactPoints(openglframework::Shader& shader,
                                  const openglframework::Matrix4& worldToCameraMatrix);
 
+        /// Update VBO with vertices and indices of debug info
+        void updateDebugVBO();
+
+        /// Render Debug Infos
+        void renderDebugInfos(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix);
+
         /// Render the AABBs
         void renderAABBs(const openglframework::Matrix4& worldToCameraMatrix);