improve friction quality, add shader-only support, improve debug drawing facilities, fix couple things

external/imgui/imgui_impl_glfw_gl3.cpp

@@ -210,7 +210,7 @@ bool ImGui_ImplGlfwGL3_CreateDeviceObjects()
     glGetIntegerv(GL_VERTEX_ARRAY_BINDING, &last_vertex_array);
 
     const GLchar *vertex_shader =
-        "#version 130\n"
+        "#version 400\n"
         "uniform mat4 ProjMtx;\n"
         "in vec2 Position;\n"
         "in vec2 UV;\n"
@@ -225,7 +225,7 @@ bool ImGui_ImplGlfwGL3_CreateDeviceObjects()
         "}\n";
 
     const GLchar* fragment_shader =
-        "#version 130\n"
+        "#version 400\n"
         "uniform sampler2D Texture;\n"
         "in vec2 Frag_UV;\n"
         "in vec4 Frag_Color;\n"

include/bounce/collision/shapes/mesh.h

@@ -19,31 +19,10 @@
 #ifndef B3_MESH_H
 #define B3_MESH_H
 
-#include <bounce/common/geometry.h>
 #include <bounce/collision/trees/static_tree.h>
 
-// A triangle in indexed form.
 struct b3Triangle
 {
-	// Does nothing for performance.
-	b3Triangle() { }
-
-	// Set this triangle from three vertices.
-	b3Triangle(u32 _v1, u32 _v2, u32 _v3)
-	{
-		v1 = _v1;
-		v2 = _v2;
-		v3 = _v3;
-	}
-
-	// Set this triangle from three vertices.
-	void Set(u32 _v1, u32 _v2, u32 _v3)
-	{
-		v1 = _v1;
-		v2 = _v2;
-		v3 = _v3;
-	}
-
 	// Test if this triangle contains a given vertex.
 	bool TestVertex(u32 v) const
 	{
@@ -67,33 +46,15 @@ struct b3Mesh
 	b3Triangle* triangles;
 	b3StaticTree tree;
 
-	void BuildTree();
-	
 	const b3Vec3& GetVertex(u32 index) const;
 	const b3Triangle& GetTriangle(u32 index) const;
-	
-	void GetTriangleVertices(b3Vec3 out[3], u32 index) const;
-	b3Plane GetTrianglePlane(u32 index) const;
-	b3AABB3 GetTriangleAABB(u32 index) const;
 	u32 GetSize() const;
+
+	b3AABB3 GetTriangleAABB(u32 index) const;
+
+	void BuildTree();
 };
 
-inline void b3Mesh::BuildTree()
-{
-	b3AABB3* aabbs = (b3AABB3*)b3Alloc(triangleCount * sizeof(b3AABB3));
-	u32* indices = (u32*)b3Alloc(triangleCount * sizeof(u32));
-	for (u32 i = 0; i < triangleCount; ++i)
-	{
-		aabbs[i] = GetTriangleAABB(i);
-		indices[i] = i;
-	}
-
-	tree.Build(indices, aabbs, triangleCount);
-
-	b3Free(indices);
-	b3Free(aabbs);
-}
-
 inline const b3Vec3& b3Mesh::GetVertex(u32 index) const
 {
 	return vertices[index];
@@ -104,22 +65,14 @@ inline const b3Triangle& b3Mesh::GetTriangle(u32 index) const
 	return triangles[index];
 }
 
-inline void b3Mesh::GetTriangleVertices(b3Vec3 out[3], u32 index) const
+inline u32 b3Mesh::GetSize() const
 {
-	const b3Triangle* triangle = triangles + index;
+	u32 size = 0;
-	u32 i1 = triangle->v1;
+	size += sizeof(b3Mesh);
-	u32 i2 = triangle->v2;
+	size += sizeof(b3Vec3) * vertexCount;
-	u32 i3 = triangle->v3;
+	size += sizeof(b3Triangle) * triangleCount;
-	out[0] = vertices[i1];
+	size += tree.GetSize();
-	out[1] = vertices[i2];
+	return size;
-	out[2] = vertices[i3];
-}
-
-inline b3Plane b3Mesh::GetTrianglePlane(u32 index) const
-{
-	b3Vec3 vs[3];
-	GetTriangleVertices(vs, index);
-	return b3Plane(vs[0], vs[1], vs[2]);
 }
 
 inline b3AABB3 b3Mesh::GetTriangleAABB(u32 index) const
@@ -133,18 +86,20 @@ inline b3AABB3 b3Mesh::GetTriangleAABB(u32 index) const
 	b3AABB3 aabb;
 	aabb.m_lower = b3Min(b3Min(vertices[i1], vertices[i2]), vertices[i3]);
 	aabb.m_upper = b3Max(b3Max(vertices[i1], vertices[i2]), vertices[i3]);
-	
 	return aabb;
 }
 
-inline u32 b3Mesh::GetSize() const
+inline void b3Mesh::BuildTree()
 {
-	u32 size = 0;
+	b3AABB3* aabbs = (b3AABB3*)b3Alloc(triangleCount * sizeof(b3AABB3));
-	size += sizeof(b3Mesh);
+	for (u32 i = 0; i < triangleCount; ++i)
-	size += sizeof(b3Vec3) * vertexCount;
+	{
-	size += sizeof(b3Triangle) * triangleCount;
+		aabbs[i] = GetTriangleAABB(i);
-	size += tree.GetSize();
+	}
-	return size;
+
+	tree.Build(aabbs, triangleCount);
+
+	b3Free(aabbs);
 }
 
 #endif

include/bounce/collision/trees/static_tree.h

@@ -33,9 +33,8 @@ public:
 	b3StaticTree();
 	~b3StaticTree();
 
-	// Build this tree from a list of AABBs and the list 
+	// Build this tree from a list of AABBs.
-	// of indices to the AABBs.
+	void Build(const b3AABB3* aabbs, u32 count);
-	void Build(u32* indices, const b3AABB3* aabbs, u32 count);
 
 	// Get the AABB of a given proxy.
 	const b3AABB3& GetAABB(u32 proxyId) const;

include/bounce/common/draw.h

@@ -62,16 +62,22 @@ public :
 	void AppendFlags(u32 flags);
 	
 	// Draw a point.
-	virtual void DrawPoint(const b3Vec3& point, const b3Color& color) = 0;
+	virtual void DrawPoint(const b3Vec3& p, float32 size, const b3Color& color) = 0;
 
 	// Draw a line segment.
-	virtual void DrawSegment(const b3Vec3& a, const b3Vec3& b, const b3Color& color) = 0;
+	virtual void DrawSegment(const b3Vec3& p1, const b3Vec3& p2, const b3Color& color) = 0;
-	
+
+	// Draw a triangle with vertices ordered CCW.
+	virtual void DrawTriangle(const b3Vec3& p1, const b3Vec3& p2, const b3Vec3& p3, const b3Color& color) = 0;
+
+	// Draw a solid triangle with vertices ordered CCW.
+	virtual void DrawSolidTriangle(const b3Vec3& normal, const b3Vec3& p1, const b3Vec3& p2, const b3Vec3& p3, const b3Color& color) = 0;
+
 	// Draw a polygon with vertices ordered CCW.
 	virtual void DrawPolygon(const b3Vec3* vertices, u32 count, const b3Color& color) = 0;
 	
 	// Draw a solid polygon with vertices ordered CCW.
-	virtual void DrawSolidPolygon(const b3Vec3* vertices, u32 count, const b3Color& color) = 0;
+	virtual void DrawSolidPolygon(const b3Vec3& normal, const b3Vec3* vertices, u32 count, const b3Color& color) = 0;
 
 	// Draw a circle with center, normal, and radius.
 	virtual void DrawCircle(const b3Vec3& normal, const b3Vec3& center, float32 radius, const b3Color& color) = 0;

include/bounce/dynamics/contacts/contact_solver.h

@@ -62,9 +62,16 @@ struct b3VelocityConstraintPoint
 {
 	b3Vec3 rA;
 	b3Vec3 rB;
+	
 	b3Vec3 normal;
 	float32 normalMass;
 	float32 normalImpulse;
+	
+	b3Vec3 tangent1;
+	b3Vec3 tangent2;
+	b3Mat22 tangentMass;
+	b3Vec2 tangentImpulse;
+
 	float32 velocityBias;
 };
 
@@ -80,9 +87,7 @@ struct b3VelocityConstraintManifold
 	b3Vec2 tangentImpulse;	
 	float32 motorImpulse;
 	float32 motorMass;
-	//float32 maxTangentImpulse;
+	
-	//float32 area;
-
 	b3VelocityConstraintPoint* points;
 	u32 pointCount;
 };

include/bounce/dynamics/contacts/manifold.h

@@ -33,6 +33,7 @@ struct b3ManifoldPoint
 	b3Vec3 localPoint; // local point on the first shape	
 	b3Vec3 localPoint2; // local point on the other shape
 	float32 normalImpulse; // normal impulse
+	b3Vec2 tangentImpulse; // tangent impulses 
 	u8 persisting; // indicates that the point is persisting
 };
 
@@ -65,8 +66,9 @@ struct b3WorldManifoldPoint
 		const b3Transform& xfA, float32 radiusA,
 		const b3Transform& xfB, float32 radiusB);
 
-	b3Vec3 normal;
 	b3Vec3 point;
+	b3Vec3 normal;
+	b3Vec2 tangents[2];
 	float32 separation;
 };
 

include/testbed/framework/debug_draw.h

@@ -25,7 +25,8 @@
 struct DrawPoints;
 struct DrawLines;
 struct DrawTriangles;
-struct DrawShapes;
+struct DrawWire;
+struct DrawSolid;
 
 class Camera
 {
@@ -66,13 +67,17 @@ public:
 	DebugDraw();
 	~DebugDraw();
 
-	void DrawPoint(const b3Vec3& point, const b3Color& color);
+	void DrawPoint(const b3Vec3& p, float32 size, const b3Color& color);
 
-	void DrawSegment(const b3Vec3& a, const b3Vec3& b, const b3Color& color);
+	void DrawSegment(const b3Vec3& p1, const b3Vec3& p2, const b3Color& color);
+	
+	void DrawTriangle(const b3Vec3& p1, const b3Vec3& p2, const b3Vec3& p3, const b3Color& color);
+
+	void DrawSolidTriangle(const b3Vec3& normal, const b3Vec3& p1, const b3Vec3& p2, const b3Vec3& p3, const b3Color& color);
 
 	void DrawPolygon(const b3Vec3* vertices, u32 count, const b3Color& color);
 	
-	void DrawSolidPolygon(const b3Vec3* vertices, u32 count, const b3Color& color);
+	void DrawSolidPolygon(const b3Vec3& normal, const b3Vec3* vertices, u32 count, const b3Color& color);
 
 	void DrawCircle(const b3Vec3& normal, const b3Vec3& center, float32 radius, const b3Color& color);
 
@@ -86,15 +91,30 @@ public:
 
 	void DrawTransform(const b3Transform& xf);
 
+	//
 	void DrawString(const char* string, const b3Color& color, ...);
 
-	void Submit();
+	void DrawSphere(const b3SphereShape* s, const b3Color& c, const b3Transform& xf);
-	void Submit(const b3World& world);
+
+	void DrawCapsule(const b3CapsuleShape* s, const b3Color& c, const b3Transform& xf);
+	
+	void DrawHull(const b3HullShape* s, const b3Color& c, const b3Transform& xf);
+	
+	void DrawMesh(const b3MeshShape* s, const b3Color& c, const b3Transform& xf);
+
+	void DrawShape(const b3Shape* s, const b3Color& c, const b3Transform& xf);
+
+	void Draw(const b3World& world);
+
+	void Draw();
 private:
+	friend struct DrawShapes;
+
 	DrawPoints* m_points;
 	DrawLines* m_lines;
 	DrawTriangles* m_triangles;
-	DrawShapes* m_shapes;
+	DrawWire* m_wire;
+	DrawSolid* m_solid;
 };
 
 #endif

include/testbed/tests/capsule_distance.h

@@ -61,10 +61,10 @@ public:
 		
 		if (b3Distance(pointA, pointB) > 0.0f)
 		{
-			g_debugDraw->DrawPoint(pointA, b3Color(1.0f, 0.0f, 0.0f));
+			g_debugDraw->DrawPoint(pointA, 4.0f, b3Color(0.0f, 1.0f, 0.0f));
-			g_debugDraw->DrawPoint(pointB, b3Color(1.0f, 0.0f, 0.0f));
+			g_debugDraw->DrawPoint(pointB, 4.0f, b3Color(0.0f, 1.0f, 0.0f));
 			
-			g_debugDraw->DrawSegment(pointA, pointB, b3Color(1.0f, 1.0f, 0.0f));
+			g_debugDraw->DrawSegment(pointA, pointB, b3Color(1.0f, 1.0f, 1.0f));
 		}
 
 		g_debugDraw->DrawTransform(m_xfA);

include/testbed/tests/cluster_test.h

@@ -63,14 +63,14 @@ public:
 		for (u32 i = 0; i < m_clusters.Count(); ++i)
 		{
 			g_debugDraw->DrawSegment(b3Vec3(0, 0, 0), m_clusters[i].centroid, b3Color(1, 1, 1));
-			g_debugDraw->DrawPoint(m_clusters[i].centroid, m_colors[i]);
+			g_debugDraw->DrawPoint(m_clusters[i].centroid, 4.0f, m_colors[i]);
 
 			for (u32 j = 0; j < m_observs.Count(); ++j)
 			{
 				b3Observation obs = m_observs[j];
 				if (obs.cluster == i)
 				{
-					g_debugDraw->DrawPoint(obs.point, m_colors[i]);
+					g_debugDraw->DrawPoint(obs.point, 4.0f, m_colors[i]);
 				}
 			}
 		}

include/testbed/tests/collide_test.h

@@ -51,13 +51,13 @@ public:
 			b3Vec3 pw = wmp->point;
 			b3Vec2 ps = g_camera.ConvertWorldToScreen(pw);
 			
-			g_debugDraw->DrawPoint(pw, b3Color(0.0f, 1.0f, 0.0f));
+			g_debugDraw->DrawPoint(pw, 4.0f, b3Color(0.0f, 1.0f, 0.0f));
 			g_debugDraw->DrawSegment(pw, pw + wmp->normal, b3Color(1.0f, 1.0f, 1.0f));
 		}
 
 		if (wm.pointCount > 0)
 		{
-			g_debugDraw->DrawPoint(wm.center, b3Color(1.0f, 1.0f, 0.0f));
+			g_debugDraw->DrawPoint(wm.center, 4.0f, b3Color(1.0f, 1.0f, 0.0f));
 			g_debugDraw->DrawSegment(wm.center, wm.center + wm.normal, b3Color(1.0f, 1.0f, 0.0f));
 			g_debugDraw->DrawSegment(wm.center, wm.center + wm.tangent1, b3Color(1.0f, 1.0f, 0.0f));
 			g_debugDraw->DrawSegment(wm.center, wm.center + wm.tangent2, b3Color(1.0f, 1.0f, 0.0f));

include/testbed/tests/distance_test.h

@@ -57,18 +57,18 @@ public:
 			for (u32 i = 0; i < featurePair.countA; ++i)
 			{
 				u32 index = featurePair.indexA[i];
-				g_debugDraw->DrawPoint(m_xfA * m_proxyA.GetVertex(index), b3Color(1.0f, 1.0f, 0.0f));
+				g_debugDraw->DrawPoint(m_xfA * m_proxyA.GetVertex(index), 4.0f, b3Color(1.0f, 1.0f, 0.0f));
 			}
 
 			for (u32 i = 0; i < featurePair.countB; ++i)
 			{
 				u32 index = featurePair.indexB[i];
-				g_debugDraw->DrawPoint(m_xfB * m_proxyB.GetVertex(index), b3Color(1.0f, 1.0f, 0.0f));
+				g_debugDraw->DrawPoint(m_xfB * m_proxyB.GetVertex(index), 4.0f, b3Color(1.0f, 1.0f, 0.0f));
 			}
 		}
 		
-		g_debugDraw->DrawPoint(out.pointA, b3Color(0.0f, 1.0f, 0.0f));
+		g_debugDraw->DrawPoint(out.pointA, 4.0f, b3Color(0.0f, 1.0f, 0.0f));
-		g_debugDraw->DrawPoint(out.pointB, b3Color(0.0f, 1.0f, 0.0f));
+		g_debugDraw->DrawPoint(out.pointB, 4.0f, b3Color(0.0f, 1.0f, 0.0f));
 		g_debugDraw->DrawSegment(out.pointA, out.pointB, b3Color(1.0f, 1.0f, 1.0f));
 
 		g_debugDraw->DrawTransform(m_xfA);

include/testbed/tests/ray_cast.h

@@ -214,7 +214,7 @@ public:
 		{
 			// Replace current hit
 			g_debugDraw->DrawSegment(p1, hit.point, b3Color(0.0f, 1.0f, 0.0f));
-			g_debugDraw->DrawPoint(hit.point, b3Color(1.0f, 0.0f, 0.0f));
+			g_debugDraw->DrawPoint(hit.point, 4.0f, b3Color(1.0f, 0.0f, 0.0f));
 			g_debugDraw->DrawSegment(hit.point, hit.point + hit.normal, b3Color(1.0f, 1.0f, 1.0f));
 		}
 		else

include/testbed/tests/test.h

@@ -36,7 +36,8 @@ struct Settings
 		warmStart = true;
 		convexCache = true;
 		drawCenterOfMasses = false;
-		drawShapes = true;
+		drawVerticesEdges = true;
+		drawFaces = true;
 		drawBounds = false;
 		drawJoints = true;
 		drawContactPoints = true;
@@ -62,7 +63,8 @@ struct Settings
 	bool convexCache;
 	bool drawCenterOfMasses;
 	bool drawBounds;
-	bool drawShapes;
+	bool drawVerticesEdges;
+	bool drawFaces;
 	bool drawSolidShapes;
 	bool drawJoints;
 	bool drawContactPoints;

premake5.lua

@@ -166,7 +166,7 @@ solution (solution_name)
 		location ( solution_dir .. action )
 		includedirs { external_dir, inc_dir }
 		vpaths { ["Headers"] = "**.h", ["Sources"] = "**.cpp" }
-		buildoptions { "-std=c++11" } -- require C++11
+		buildoptions { "-std=c++11" } -- GNU/GCC C++11
 
 		--common
 		files 
@@ -179,11 +179,11 @@ solution (solution_name)
 
 		configuration { "windows" }
 			links { "glu32", "opengl32", "winmm" }
-
+			
 		configuration { "not windows", "not macosx" }
 			links 
 			{ 		
-				"GL", "GLU", "rt", "Xrandr", "Xinerama", "Xi", "Xcursor", 
+				"GL", "rt", "Xrandr", "Xinerama", "Xi", "Xcursor", 
 				"m", "dl", "Xrender", "drm", "Xdamage", "X11-xcb", "xcb-glx", 
 				"xcb-dri2", "xcb-dri3", "xcb-present", "xcb-sync", "xshmfence", 
 				"Xxf86vm", "Xfixes", "Xext", "X11", "pthread", "xcb", "Xau", "Xdmcp" 

src/bounce/cloth/cloth.cpp

@@ -385,44 +385,16 @@ void b3Cloth::Draw(b3Draw* draw) const
 		b3Particle* p2 = m_ps + t->v2;
 		b3Particle* p3 = m_ps + t->v3;
 
-		b3Vec3 vs1[3];
+		b3Vec3 v1 = p1->p;
-		vs1[0] = p1->p;
+		b3Vec3 v2 = p2->p;
-		vs1[1] = p2->p;
+		b3Vec3 v3 = p3->p;
-		vs1[2] = p3->p;
 
-		draw->DrawPolygon(vs1, 3, color4);
+		b3Vec3 n1 = b3Cross(v2 - v1, v3 - v1);
-		draw->DrawSolidPolygon(vs1, 3, color3);
-
-		b3Vec3 vs2[3];
-		vs2[0] = p1->p;
-		vs2[1] = p3->p;
-		vs2[2] = p2->p;
-
-		draw->DrawPolygon(vs2, 3, color4);
-		draw->DrawSolidPolygon(vs2, 3, color3);
-	}
-
-#if 0
-	for (u32 i = 0; i < m_c2Count; ++i)
-	{
-		b3C2* c = m_c2s + i;
-
-		b3Particle* p1 = m_ps + c->i1;
-		b3Particle* p2 = m_ps + c->i2;
-		b3Particle* p3 = m_ps + c->i3;
-		b3Particle* p4 = m_ps + c->i4;
-
-		b3Vec3 c1 = (p1->p + p2->p + p3->p) / 3.0f;
-		b3Vec3 n1 = b3Cross(p2->p - p1->p, p3->p - p1->p);
 		n1.Normalize();
 
-		draw->DrawSegment(c1, c1 + n1, color1);
+		b3Vec3 n2 = -n1;
 
-		b3Vec3 c2 = (p1->p + p4->p + p2->p) / 3.0f;
+		draw->DrawSolidTriangle(n1, v1, v2, v3, color3);
-		b3Vec3 n2 = b3Cross(p2->p - p1->p, p4->p - p1->p);
+		draw->DrawSolidTriangle(n2, v1, v3, v2, color3);
-		n2.Normalize();
-
-		draw->DrawSegment(c2, c2 + n2, color1);
 	}
-#endif
 }

src/bounce/collision/trees/static_tree.cpp

@@ -60,10 +60,16 @@ struct b3SortPredicate
 	}
 };
 
-void b3StaticTree::Build(u32* ids, const b3AABB3* set, u32 n)
+void b3StaticTree::Build(const b3AABB3* set, u32 n)
 {
 	B3_ASSERT(n > 0);
 
+	u32* ids = (u32*)b3Alloc(n * sizeof(u32));
+	for (u32 i = 0; i < n; ++i)
+	{
+		ids[i] = i;
+	}
+
 	// Leafs = n, Internals = n - 1, Total = 2n - 1, if we assume
 	// each leaf node contains exactly 1 object.
 	const u32 kMinObjectsPerLeaf = 1;
@@ -184,6 +190,8 @@ void b3StaticTree::Build(u32* ids, const b3AABB3* set, u32 n)
 		}
 	}
 	
+	b3Free(ids);
+
 	B3_ASSERT(leafCount == leafCapacity);
 	B3_ASSERT(internalCount == internalCapacity);
 	B3_ASSERT(m_nodeCount == nodeCapacity);

src/bounce/dynamics/contacts/contact_solver.cpp

@@ -22,6 +22,9 @@
 #include <bounce/dynamics/body.h>
 #include <bounce/common/memory/stack_allocator.h>
 
+// Turn on or off central friction. This is an important optimization for old hardwares. 
+#define B3_CENTRAL_FRICTION 0
+
 // This solver implements PGS for solving velocity constraints and 
 // NGS for solving position constraints.
 
@@ -138,6 +141,7 @@ void b3ContactSolver::InitializeConstraints()
 				pcp->localPointB = cp->localPoint2;
 
 				vcp->normalImpulse = cp->normalImpulse;
+				vcp->tangentImpulse = cp->tangentImpulse;
 			}
 		}
 	}
@@ -225,8 +229,43 @@ void b3ContactSolver::InitializeConstraints()
 						vcp->velocityBias = -vc->restitution * vn;
 					}
 				}
+
+#if B3_CENTRAL_FRICTION == 0
+				// Add friction constraints.
+				{
+					vcp->tangent1 = vcm->tangent1;
+					vcp->tangent2 = vcm->tangent2;
+
+					b3Vec3 t1 = vcp->tangent1;
+					b3Vec3 t2 = vcp->tangent2;
+
+					// Compute effective mass.
+					// Identities used:
+					// I = I^T because I is symmetric. Its inverse is also symmetric.
+					// dot(a * u, b * v) = a * b * dot(u, v)
+					// dot(t1, t2) = 0
+					b3Vec3 rt1A = b3Cross(rA, t1);
+					b3Vec3 rt1B = b3Cross(rB, t1);
+					b3Vec3 rt2A = b3Cross(rA, t2);
+					b3Vec3 rt2B = b3Cross(rB, t2);
+
+					float32 kTan11 = mA + mB + b3Dot(iA * rt1A, rt1A) + b3Dot(iB * rt2B, rt2B);
+					float32 kTan12 = b3Dot(iA * rt1A, rt2A) + b3Dot(iB * rt1B, rt2B);
+					float32 kTan21 = kTan12;
+					float32 kTan22 = mA + mB + b3Dot(iA * rt2A, rt2A) + b3Dot(iB * rt2B, rt2B);
+
+					b3Mat22 K;
+					K.x.x = kTan11;
+					K.x.y = kTan12;
+					K.y.x = kTan21;
+					K.y.y = kTan22;
+
+					vcp->tangentMass = K;
+				}
+#endif
 			}
 
+#if B3_CENTRAL_FRICTION == 1
 			if (pointCount > 0)
 			{
 				b3Vec3 rA = vcm->center - xA;
@@ -235,7 +274,7 @@ void b3ContactSolver::InitializeConstraints()
 				vcm->rA = rA;
 				vcm->rB = rB;
 
-				// Add friction constraint.
+				// Add friction constraints.
 				{
 					b3Vec3 t1 = vcm->tangent1;
 					b3Vec3 t2 = vcm->tangent2;
@@ -270,6 +309,7 @@ void b3ContactSolver::InitializeConstraints()
 					vcm->motorMass = mass > 0.0f ? 1.0f / mass : 0.0f;
 				}
 			}
+#endif
 		}
 	}
 }
@@ -304,7 +344,10 @@ void b3ContactSolver::WarmStart()
 			{
 				b3VelocityConstraintPoint* vcp = vcm->points + k;
 
-				b3Vec3 P = vcp->normalImpulse * vcp->normal;
+				b3Vec3 P1 = vcp->normalImpulse * vcp->normal;
+				b3Vec3 P2 = vcp->tangentImpulse.x * vcp->tangent1;
+				b3Vec3 P3 = vcp->tangentImpulse.y * vcp->tangent2;
+				b3Vec3 P = P1 + P2 + P3;
 
 				vA -= mA * P;
 				wA -= iA * b3Cross(vcp->rA, P);
@@ -313,18 +356,20 @@ void b3ContactSolver::WarmStart()
 				wB += iB * b3Cross(vcp->rB, P);
 			}
 
+#if B3_CENTRAL_FRICTION == 1
 			if (pointCount > 0)
 			{
 				b3Vec3 P1 = vcm->tangentImpulse.x * vcm->tangent1;
 				b3Vec3 P2 = vcm->tangentImpulse.y * vcm->tangent2;
 				b3Vec3 P3 = vcm->motorImpulse * vcm->normal;
-
+				
 				vA -= mA * (P1 + P2);
 				wA -= iA * (b3Cross(vcm->rA, P1 + P2) + P3);
 
 				vB += mB * (P1 + P2);
 				wB += iB * (b3Cross(vcm->rB, P1 + P2) + P3);
 			}
+#endif
 		}
 
 		m_velocities[indexA].v = vA;
@@ -387,8 +432,43 @@ void b3ContactSolver::SolveVelocityConstraints()
 
 					normalImpulse += vcp->normalImpulse;
 				}
+
+#if B3_CENTRAL_FRICTION == 0
+				// Solve tangent constraints.
+				{
+					b3Vec3 dv = vB + b3Cross(wB, vcp->rB) - vA - b3Cross(wA, vcp->rA);
+
+					b3Vec2 Cdot;
+					Cdot.x = b3Dot(dv, vcp->tangent1);
+					Cdot.y = b3Dot(dv, vcp->tangent2);
+
+					b3Vec2 impulse = vcp->tangentMass.Solve(-Cdot);
+					b3Vec2 oldImpulse = vcp->tangentImpulse;
+					vcp->tangentImpulse += impulse;
+
+					float32 maxImpulse = vc->friction * vcp->normalImpulse;
+					if (b3Dot(vcp->tangentImpulse, vcp->tangentImpulse) > maxImpulse * maxImpulse)
+					{
+						vcp->tangentImpulse.Normalize();
+						vcp->tangentImpulse *= maxImpulse;
+					}
+
+					impulse = vcp->tangentImpulse - oldImpulse;
+
+					b3Vec3 P1 = impulse.x * vcp->tangent1;
+					b3Vec3 P2 = impulse.y * vcp->tangent2;
+					b3Vec3 P = P1 + P2;
+
+					vA -= mA * P;
+					wA -= iA * b3Cross(vcp->rA, P);
+
+					vB += mB * P;
+					wB += iB * b3Cross(vcp->rB, P);
+				}
+#endif
 			}
 			
+#if B3_CENTRAL_FRICTION == 1
 			if (pointCount > 0)
 			{
 				// Solve tangent constraints.
@@ -438,6 +518,7 @@ void b3ContactSolver::SolveVelocityConstraints()
 					wB += iB * P;
 				}
 			}
+#endif
 		}
 
 		m_velocities[indexA].v = vA;
@@ -471,6 +552,7 @@ void b3ContactSolver::StoreImpulses()
 				b3ManifoldPoint* cp = m->points + k;
 				b3VelocityConstraintPoint* vcp = vcm->points + k;
 				cp->normalImpulse = vcp->normalImpulse;
+				cp->tangentImpulse = vcp->tangentImpulse;
 			}
 		}
 	}

src/bounce/dynamics/contacts/manifold.cpp

@@ -27,6 +27,7 @@ void b3Manifold::GuessImpulses()
 	{
 		b3ManifoldPoint* p = points + i;
 		p->normalImpulse = 0.0f;
+		p->tangentImpulse.SetZero();
 		p->persisting = 0;
 	}
 }
@@ -45,6 +46,7 @@ void b3Manifold::FindImpulses(const b3Manifold& oldManifold)
 			if (p2->triangleKey == p1->triangleKey && p2->key == p1->key)
 			{
 				p2->normalImpulse = p1->normalImpulse;
+				p2->tangentImpulse = p1->tangentImpulse;
 				p2->persisting = 1;
 				break;
 			}
@@ -81,6 +83,7 @@ void b3WorldManifold::Initialize(const b3Manifold* manifold,
 	{
 		const b3ManifoldPoint* mp = manifold->points + i;
 		b3WorldManifoldPoint* wmp = points + i;
+		
 		wmp->Initialize(mp, xfA, radiusA, xfB, radiusB);
 	}
 }

src/bounce/dynamics/draw_world.cpp

@@ -122,7 +122,7 @@ void b3World::DebugDraw() const
 				
 				if (flags & b3Draw::e_contactPointsFlag)
 				{
-					m_debugDraw->DrawPoint(p, yellow);
+					m_debugDraw->DrawPoint(p, 4.0f, yellow);
 				}
 
 				if (flags & b3Draw::e_contactNormalsFlag)
@@ -143,18 +143,28 @@ void b3World::DebugDraw() const
 				const b3WorldManifoldPoint* wmp = wm.points + j;
 
 				b3Vec3 n = wmp->normal;
+				b3Vec3 t1 = wm.tangent1;
+				b3Vec3 t2 = wm.tangent2;
 				b3Vec3 p = wmp->point;
 				float32 Pn = mp->normalImpulse;
-				
+				float32 Pt1 = mp->tangentImpulse.x;
+				float32 Pt2 = mp->tangentImpulse.y;
+
 				if (flags & b3Draw::e_contactPointsFlag)
 				{
-					m_debugDraw->DrawPoint(p, mp->persisting ? green : red);
+					m_debugDraw->DrawPoint(p, 4.0f, mp->persisting ? green : red);
 				}
 				
 				if (flags & b3Draw::e_contactNormalsFlag)
 				{
 					m_debugDraw->DrawSegment(p, p + n, white);
 				}
+				
+				if (flags & b3Draw::e_contactTangentsFlag)
+				{
+					m_debugDraw->DrawSegment(p, p + t1, yellow);
+					m_debugDraw->DrawSegment(p, p + t2, yellow);
+				}
 			}
 		}
 	}
@@ -168,44 +178,34 @@ void b3World::DrawShape(const b3Transform& xf, const b3Shape* shape) const
 	case e_sphereShape:
 	{
 		const b3SphereShape* sphere = (b3SphereShape*)shape;
-		b3Vec3 c = xf * sphere->m_center;
+		b3Vec3 p = xf * sphere->m_center;
-		m_debugDraw->DrawPoint(c, wireColor);
+		m_debugDraw->DrawPoint(p, 4.0f, wireColor);
 		break;
 	}
 	case e_capsuleShape:
 	{
 		const b3CapsuleShape* capsule = (b3CapsuleShape*)shape;
-		b3Vec3 c1 = xf * capsule->m_centers[0];
+		b3Vec3 p1 = xf * capsule->m_centers[0];
-		b3Vec3 c2 = xf * capsule->m_centers[1];
+		b3Vec3 p2 = xf * capsule->m_centers[1];
-		
+		m_debugDraw->DrawPoint(p1, 4.0f, wireColor);
-		m_debugDraw->DrawPoint(c1, wireColor);
+		m_debugDraw->DrawPoint(p2, 4.0f, wireColor);
-		m_debugDraw->DrawPoint(c2, wireColor);
+		m_debugDraw->DrawSegment(p1, p2, wireColor);
-		m_debugDraw->DrawSegment(c1, c2, wireColor);
 		break;
 	}
 	case e_hullShape:
 	{
 		const b3HullShape* hs = (b3HullShape*)shape;
 		const b3Hull* hull = hs->m_hull;
-
+		for (u32 i = 0; i < hull->edgeCount; i += 2)
-		for (u32 i = 0; i < hull->faceCount; ++i)
 		{
-			b3Vec3 polygon[B3_MAX_HULL_FEATURES];
+			const b3HalfEdge* edge = hull->GetEdge(i);
-			u32 vCount = 0;
+			const b3HalfEdge* twin = hull->GetEdge(i + 1);
+			
+			b3Vec3 p1 = xf * hull->vertices[edge->origin];
+			b3Vec3 p2 = xf * hull->vertices[twin->origin];
 
-			// Build convex polygon for loop
+			m_debugDraw->DrawSegment(p1, p2, wireColor);
-			const b3Face* face = hull->GetFace(i);
-			const b3HalfEdge* begin = hull->GetEdge(face->edge);
-			const b3HalfEdge* edge = begin;
-			do
-			{
-				polygon[vCount++] = xf * hull->GetVertex(edge->origin);
-				edge = hull->GetEdge(edge->next);
-			} while (edge != begin);
-
-			m_debugDraw->DrawPolygon(polygon, vCount, wireColor);
 		}
-
 		break;
 	}
 	case e_meshShape:
@@ -214,14 +214,13 @@ void b3World::DrawShape(const b3Transform& xf, const b3Shape* shape) const
 		const b3Mesh* mesh = ms->m_mesh;
 		for (u32 i = 0; i < mesh->triangleCount; ++i)
 		{
-			const b3Triangle* triangle = mesh->triangles + i;
+			const b3Triangle* t = mesh->triangles + i;
 
-			b3Vec3 vs[3];
+			b3Vec3 p1 = xf * mesh->vertices[t->v1];
-			vs[0] = xf * mesh->vertices[triangle->v1];
+			b3Vec3 p2 = xf * mesh->vertices[t->v2];
-			vs[1] = xf * mesh->vertices[triangle->v2];
+			b3Vec3 p3 = xf * mesh->vertices[t->v3];
-			vs[2] = xf * mesh->vertices[triangle->v3];
 
-			m_debugDraw->DrawPolygon(vs, 3, wireColor);
+			m_debugDraw->DrawTriangle(p1, p2, p3, wireColor);
 		}
 		break;
 	}

src/bounce/dynamics/joints/mouse_joint.cpp

@@ -116,7 +116,7 @@ void b3MouseJoint::Draw(b3Draw* draw) const
 	b3Vec3 a = GetAnchorA();
 	b3Vec3 b = GetAnchorB();
 
-	draw->DrawPoint(a, green);
+	draw->DrawPoint(a, 4.0f, green);
-	draw->DrawPoint(b, red);
+	draw->DrawPoint(b, 4.0f, red);
 	draw->DrawSegment(a, b, yellow);
 }

src/bounce/dynamics/joints/sphere_joint.cpp

@@ -162,7 +162,7 @@ void b3SphereJoint::Draw(b3Draw* draw) const
 	b3Vec3 a = GetAnchorA();
 	b3Vec3 b = GetAnchorB();
 	
-	draw->DrawPoint(a, b3Color(1.0f, 0.0f, 0.0f));
+	draw->DrawPoint(a, 4.0f, b3Color(1.0f, 0.0f, 0.0f));
-	draw->DrawPoint(b, b3Color(0.0f, 1.0f, 0.0f));
+	draw->DrawPoint(b, 4.0f, b3Color(0.0f, 1.0f, 0.0f));
 	draw->DrawSegment(a, b, b3Color(1.0f, 1.0f, 0.0f));
 }

src/bounce/dynamics/joints/spring_joint.cpp

@@ -261,7 +261,7 @@ void b3SpringJoint::Draw(b3Draw* draw) const
 	b3Vec3 a = GetBodyA()->GetWorldPoint(m_localAnchorA);
 	b3Vec3 b = GetBodyB()->GetWorldPoint(m_localAnchorB);
 	
-	draw->DrawPoint(a, green);
+	draw->DrawPoint(a, 4.0f, red);
-	draw->DrawPoint(b, green);
+	draw->DrawPoint(b, 4.0f, green);
 	draw->DrawSegment(a, b, blue);
 }

src/bounce/dynamics/shapes/capsule_shape.cpp

@@ -76,8 +76,8 @@ void b3CapsuleShape::ComputeMass(b3MassData* massData, float32 density) const
 		float32 volume = (2.0f / 3.0f) * B3_PI * r3;
 		float32 mass = density * volume;
 		
-		// Ic = Io + m * d^2
+		// I = Ic + m * d^2
-		// Io = Ic - m * d^2
+		// Ic = I - m * d^2
 
 		// Hemisphere inertia about the origin
 		float32 Io = (2.0f / 5.0f) * mass * r2;
@@ -126,6 +126,8 @@ void b3CapsuleShape::ComputeMass(b3MassData* massData, float32 density) const
 	massData->center = Ic_Capsule.center;
 	massData->mass = Ic_Capsule.mass;
 	massData->I = Ic;
+
+
 }
 
 void b3CapsuleShape::ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const 
@@ -205,6 +207,33 @@ bool b3CapsuleShape::RayCast(b3RayCastOutput* output, const b3RayCastInput& inpu
 	// Check for short segment.
 	if (dd < B3_EPSILON * B3_EPSILON)
 	{
+		float32 a = nn;
+		
+		b3Vec3 m = A - P;
+		float32 b = b3Dot(m, n);
+		float32 c = b3Dot(m, m) - m_radius * m_radius;
+
+		float32 disc = b * b - a * c;
+
+		// Check for negative discriminant.
+		if (disc < 0.0f)
+		{
+			return false;
+		}
+
+		// Find the minimum time of impact of the line with the sphere.
+		float32 t = -b - b3Sqrt(disc);
+
+		// Is the intersection point on the segment?
+		if (t > 0.0f && t <= input.maxFraction * a)
+		{
+			// Finish solution.
+			t /= a;
+			output->fraction = t;
+			output->normal = b3Normalize(m + t * n);
+			return true;
+		}
+
 		return false;
 	}
 

src/bounce/dynamics/shapes/hull_shape.cpp

@@ -18,7 +18,6 @@
 
 #include <bounce/dynamics/shapes/hull_shape.h>
 #include <bounce/collision/shapes/hull.h>
-#include <bounce/common/template/array.h>
 
 b3HullShape::b3HullShape()
 {

src/bounce/quickhull/qh_hull.cpp

@@ -747,26 +747,28 @@ void qhHull::Draw(b3Draw* draw) const
 		b3Vec3 c = face->center;
 		b3Vec3 n = face->plane.normal;
 
+		b3StackArray<b3Vec3, 32> vs;
+		
+		const qhHalfEdge* begin = face->edge;
+		const qhHalfEdge* edge = begin;
+		do
+		{
+			vs.PushBack(edge->tail->position);
+			edge = edge->next;
+		} while (edge != begin);
+
+		draw->DrawSolidPolygon(n, vs.Elements(), vs.Count(), b3Color(1.0f, 1.0f, 1.0f, 0.5f));
+
 		qhVertex* v = face->conflictList.head;
 		while (v)
 		{
-			draw->DrawPoint(v->position, b3Color(1.0f, 1.0f, 0.0f));
+			draw->DrawPoint(v->position, 4.0f, b3Color(1.0f, 1.0f, 0.0f));
 			draw->DrawSegment(c, v->position, b3Color(1.0f, 1.0f, 0.0f));
 			v = v->next;
 		}
 
 		draw->DrawSegment(c, c + n, b3Color(1.0f, 1.0f, 1.0f));
 
-		b3StackArray<b3Vec3, 32> polygon;
-		qhHalfEdge* edge = face->edge;
-		do
-		{
-			polygon.PushBack(edge->tail->position);
-			edge = edge->next;
-		} while (edge != face->edge);
-
-		draw->DrawSolidPolygon(polygon.Elements(), polygon.Count(), b3Color(0.0f, 0.0f, 1.0f, 1.0f));
-		
 		face = face->next;
 	}
 }

src/testbed/framework/main.cpp

@@ -255,19 +255,19 @@ void Interface()
 	{
 		glfwSetWindowShouldClose(g_window, true);
 	}
-	
+
 	ImGui::Separator();
 
 	ImGui::Text("Step");
-	
+
 	ImGui::Text("Hertz");
-	ImGui::SliderFloat("##Hertz", &g_settings.hertz, 0.0f, 240.0f, "%.4f");
+	ImGui::SliderFloat("##Hertz", &g_settings.hertz, 0.0f, 240.0f, "%.1f");
 	ImGui::Text("Velocity Iterations");
 	ImGui::SliderInt("##Velocity Iterations", &g_settings.velocityIterations, 0, 50);
 	ImGui::Text("Position Iterations");
 	ImGui::SliderInt("#Position Iterations", &g_settings.positionIterations, 0, 50);
-	ImGui::Checkbox("Warm Start", &g_settings.warmStart);
 	ImGui::Checkbox("Sleep", &g_settings.sleep);
+	ImGui::Checkbox("Warm Start", &g_settings.warmStart);
 	//ImGui::Checkbox("Convex Cache", &g_settings.convexCache);
 
 	if (ImGui::Button("Play/Pause", buttonSize))
@@ -280,13 +280,12 @@ void Interface()
 		g_settings.singleStep = true;
 	}
 
-	ImGui::PopItemWidth();
-
 	ImGui::Separator();
 
 	ImGui::Text("View");
 	ImGui::Checkbox("Grid", &g_settings.drawGrid);
-	ImGui::Checkbox("Polygons", &g_settings.drawShapes);
+	ImGui::Checkbox("Vertices and Edges", &g_settings.drawVerticesEdges);
+	ImGui::Checkbox("Faces", &g_settings.drawFaces);
 	ImGui::Checkbox("Center of Masses", &g_settings.drawCenterOfMasses);
 	ImGui::Checkbox("Bounding Boxes", &g_settings.drawBounds);
 	ImGui::Checkbox("Joints", &g_settings.drawJoints);
@@ -295,12 +294,42 @@ void Interface()
 	ImGui::Checkbox("Contact Tangents", &g_settings.drawContactTangents);
 	ImGui::Checkbox("Statistics", &g_settings.drawStats);
 	ImGui::Checkbox("Profile", &g_settings.drawProfile);
-	
+
 	ImGui::End();
 }
 
 void Step()
 {
+	if (g_settings.drawGrid)
+	{
+		int n = 20;
+
+		b3Vec3 t;
+		t.x = -0.5f * float32(n);
+		t.y = 0.0f;
+		t.z = -0.5f * float32(n);
+
+		b3Color color(0.5f, 0.5f, 0.5f, 1.0f);
+		for (int i = 0; i < n; i += 1)
+		{
+			for (int j = 0; j < n; j += 1)
+			{
+				b3Vec3 vs[4];
+				vs[0] = b3Vec3((float)i, 0.0f, (float)j);
+				vs[1] = b3Vec3((float)i, 0.0f, (float)j + 1);
+				vs[2] = b3Vec3((float)i + 1, 0.0f, (float)j + 1);
+				vs[3] = b3Vec3((float)i + 1, 0.0f, (float)j);
+
+				for (u32 k = 0; k < 4; ++k)
+				{
+					vs[k] += t;
+				}
+
+				g_debugDraw->DrawPolygon(vs, 4, color);
+			}
+		}
+	}
+
 	if (g_settings.testID != g_settings.lastTestID)
 	{
 		delete g_test;
@@ -308,7 +337,9 @@ void Step()
 		g_test = g_tests[g_settings.testID].create();
 		g_settings.pause = true;
 	}
-	g_test->Step();
+	
+	g_test->Step();	
+	g_debugDraw->Draw();
 }
 
 void Run()
@@ -320,9 +351,12 @@ void Run()
 	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
 	glEnable(GL_BLEND);
 
-	glClearColor(0.3f, 0.3f, 0.3f, 1.0f);
+	glClearColor(0.2f, 0.2f, 0.2f, 1.0f);
 	glClearDepth(1.0f);
 
+	double t1 = glfwGetTime();
+	double frameTime = 0.0;
+
 	while (glfwWindowShouldClose(g_window) == 0)
 	{
 		int width, height;
@@ -335,62 +369,19 @@ void Run()
 		
 		ImGui_ImplGlfwGL3_NewFrame();
 		
-		if (g_settings.drawGrid)
+		ImGui::SetNextWindowPos(ImVec2(0, 0));
-		{
+		ImGui::SetNextWindowSize(ImVec2((float)g_camera.m_width, (float)g_camera.m_height));
-			int n = 20;
+		ImGui::Begin("Overlay", NULL, ImVec2(0, 0), 0.0f, ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_NoInputs | ImGuiWindowFlags_AlwaysAutoResize | ImGuiWindowFlags_NoScrollbar);
+		ImGui::SetCursorPos(ImVec2(5, (float)g_camera.m_height - 20));
+		ImGui::Text("%.1f ms", 1000.0 * frameTime);
+		ImGui::End();
 
-			b3Vec3 t;
-			t.x = -0.5f * float32(n);
-			t.y = 0.0f;
-			t.z = -0.5f * float32(n);
-
-			b3Color color(1.0f, 1.0f, 1.0f, 1.0f);
-			for (int i = 0; i < n; i += 1)
-			{
-				for (int j = 0; j < n; j += 1)
-				{
-					b3Vec3 vs[4];
-					vs[0] = b3Vec3((float)i, 0.0f, (float)j);
-					vs[1] = b3Vec3((float)i, 0.0f, (float)j + 1);
-					vs[2] = b3Vec3((float)i + 1, 0.0f, (float)j + 1);
-					vs[3] = b3Vec3((float)i + 1, 0.0f, (float)j);
-
-					for (u32 k = 0; k < 4; ++k)
-					{
-						vs[k] += t;
-					}
-
-					g_debugDraw->DrawPolygon(vs, 4, color);
-				}
-			}
-
-			b3Color color2(0.0f, 0.0f, 0.0f);
-
-			{
-				b3Vec3 p1(t.x, 0.005f, 0.0f);
-				b3Vec3 p2(-t.x, 0.005f, 0.0f);
-
-				g_debugDraw->DrawSegment(p1, p2, color2);
-			}
-
-			{
-				b3Vec3 p1(0.0f, 0.005f, t.x);
-				b3Vec3 p2(0.0f, 0.005f, -t.x);
-
-				g_debugDraw->DrawSegment(p1, p2, color2);
-			}
-		}
-		
-		Step();
-		
-		g_debugDraw->Submit();
-
-		if (g_settings.drawShapes)
-		{
-			g_debugDraw->Submit(g_test->m_world);
-		}
-		
 		Interface();
+		Step();
+
+		double t = glfwGetTime();
+		frameTime = t - t1;
+		t1 = t;
 
 		ImGui::Render();
 
@@ -413,14 +404,18 @@ int main(int argc, char** args)
 		return -1;
 	}
 	
-	// Create g_window
+	// Create window
 	extern b3Version b3_version;
 	char title[256];
 	sprintf(title, "Bounce Testbed Version %d.%d.%d", b3_version.major, b3_version.minor, b3_version.revision);
-	
+
-	glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
+#if defined(__APPLE__)
-    glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 0);
+	glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 4);
-    	
+	glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 0);
+	glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE);
+	glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);
+#endif
+		
 	g_window = glfwCreateWindow(1024, 768, title, NULL, NULL);
 	if (g_window == NULL)
 	{		

src/testbed/framework/test.cpp

@@ -364,9 +364,27 @@ void Test::Step()
 	m_maxProfile.solver.solveVelocity = b3Max(m_maxProfile.solver.solveVelocity, m_profile.solver.solveVelocity);
 	m_maxProfile.solver.solvePosition = b3Max(m_maxProfile.solver.solvePosition, m_profile.solver.solvePosition);
 
+	// Draw World
+	u32 drawFlags = 0;
+	drawFlags += g_settings.drawBounds * b3Draw::e_aabbsFlag;
+	drawFlags += g_settings.drawVerticesEdges * b3Draw::e_shapesFlag;
+	drawFlags += g_settings.drawCenterOfMasses * b3Draw::e_centerOfMassesFlag;
+	drawFlags += g_settings.drawJoints * b3Draw::e_jointsFlag;
+	drawFlags += g_settings.drawContactPoints * b3Draw::e_contactPointsFlag;
+	drawFlags += g_settings.drawContactNormals * b3Draw::e_contactNormalsFlag;
+	drawFlags += g_settings.drawContactTangents * b3Draw::e_contactTangentsFlag;
+
+	g_debugDraw->SetFlags(drawFlags);
+	m_world.DebugDraw();
+	g_debugDraw->Draw();
+	
+	if (g_settings.drawFaces)
+	{
+		g_debugDraw->Draw(m_world);
+	}
+
 	// Draw Statistics
 	ImGui::SetNextWindowPos(ImVec2(0.0f, 0.0f));
-	//ImGui::SetNextWindowSize(ImVec2(250.0f, g_camera.m_height));
 	ImGui::Begin("Log", NULL, ImVec2(0, 0), 0.0f, ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_NoInputs | ImGuiWindowFlags_AlwaysAutoResize | ImGuiWindowFlags_NoScrollbar);
 
 	if (g_settings.drawStats)
@@ -383,7 +401,7 @@ void Test::Step()
 
 		ImGui::Text("GJK Calls %d", b3_gjkCalls);
 		ImGui::Text("GJK Iterations %d (%d) (%f)", b3_gjkIters, b3_gjkMaxIters, avgGjkIters);
-		
+
 		float32 convexCacheHitRatio = 0.0f;
 		if (b3_convexCalls > 0)
 		{
@@ -403,21 +421,8 @@ void Test::Step()
 		ImGui::Text(" Velocity Solver %.4f (%.4f)", m_profile.solver.solveVelocity, m_maxProfile.solver.solveVelocity);
 		ImGui::Text(" Position Solver %.4f (%.4f)", m_profile.solver.solvePosition, m_maxProfile.solver.solvePosition);
 	}
-	
+
 	ImGui::End();
-
-	u32 drawFlags = 0;
-	drawFlags += g_settings.drawBounds * b3Draw::e_aabbsFlag;
-	drawFlags += g_settings.drawShapes * b3Draw::e_shapesFlag;
-	drawFlags += g_settings.drawCenterOfMasses * b3Draw::e_centerOfMassesFlag;
-	drawFlags += g_settings.drawJoints * b3Draw::e_jointsFlag;
-	drawFlags += g_settings.drawContactPoints * b3Draw::e_contactPointsFlag;
-	drawFlags += g_settings.drawContactNormals * b3Draw::e_contactNormalsFlag;
-	drawFlags += g_settings.drawContactTangents * b3Draw::e_contactTangentsFlag;
-
-	g_debugDraw->SetFlags(drawFlags);
-
-	m_world.DebugDraw();
 }
 
 void Test::MouseMove(const Ray3& pw)