fix hull mass data calculation and make it more robust, bugfixes

include/testbed/tests/jenga.h

@@ -96,7 +96,7 @@ public:
 				b3ShapeDef sd;
 				sd.shape = &hs;
 				sd.density = 0.1f;
-				sd.friction = 0.1f;
+				sd.friction = 0.3f;
 
 				body->CreateShape(sd);
 			}

include/testbed/tests/multiple_shapes.h

@@ -0,0 +1,121 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* 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 MULTIPLE_SHAPES_H
+#define MULTIPLE_SHAPES_H
+
+class MultipleShapes : public Test
+{
+public:
+	MultipleShapes()
+	{
+		g_camera.m_center.Set(2.0f, -2.0f, 0.0f);
+		g_camera.m_zoom = 50.0f;
+		g_settings.drawCenterOfMasses = true;
+
+		{
+			b3BodyDef bd;
+			b3Body* body = m_world.CreateBody(bd);
+
+			b3HullShape hs;
+			hs.m_hull = &m_groundHull;
+
+			b3ShapeDef sd;
+			sd.shape = &hs;
+
+			body->CreateShape(sd);
+		}
+
+		{
+			b3Transform xf;
+			xf.SetIdentity();
+			xf.position.Set(-5.0f, 10.0f, 0.0f);
+			m_box1.SetTransform(xf);
+		}
+		
+		{
+			b3Transform xf;
+			xf.SetIdentity();
+			xf.position.Set(5.0f, 10.0f, 0.0f);
+			m_box2.SetTransform(xf);
+		}
+		
+		{
+			b3Transform xf;
+			xf.SetIdentity();
+			xf.position.Set(0.0f, 2.0f, 0.0f);
+			m_box3.SetTransform(xf);
+		}
+		 
+		{
+			b3Transform xf;
+			xf.SetIdentity();
+			xf.position.Set(0.0f, 6.0f, 0.0f);
+			m_box4.SetTransform(xf);
+		}
+		
+		{
+			b3Transform xf;
+			xf.SetIdentity();
+			xf.position.Set(0.0f, 10.0f, 0.0f);
+			m_box5.SetTransform(xf);
+		}
+		
+		{
+			b3BodyDef bd;
+			bd.type = e_dynamicBody;
+			bd.angularVelocity.Set(0.0f, B3_PI, 0.0f);
+
+			b3Body* body = m_world.CreateBody(bd);
+
+			b3HullShape hs;
+
+			b3ShapeDef sd;
+			sd.shape = &hs;
+			sd.density = 0.1f;
+
+			hs.m_hull = &m_box1;
+			body->CreateShape(sd);
+			
+			hs.m_hull = &m_box2;
+			body->CreateShape(sd);
+
+			hs.m_hull = &m_box3;
+			body->CreateShape(sd); 
+
+			hs.m_hull = &m_box4;
+			body->CreateShape(sd); 
+
+			hs.m_hull = &m_box5;
+			body->CreateShape(sd); 
+		}
+	}
+
+	static Test* Create()
+	{
+		return new MultipleShapes();
+	}
+
+	b3BoxHull m_box1;
+	b3BoxHull m_box2;
+	b3BoxHull m_box3;
+	b3BoxHull m_box4;
+	b3BoxHull m_box5;
+};
+
+#endif

include/testbed/tests/quadric_shapes.h

@@ -16,8 +16,8 @@
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
-#ifndef QUADRIC_H
-#define QUADRIC_H
+#ifndef QUADRIC_SHAPES_H
+#define QUADRIC_SHAPES_H
 
 #include <testbed/tests/quickhull_test.h>
 
@@ -25,39 +25,41 @@ extern DebugDraw* g_debugDraw;
 extern Camera g_camera;
 extern Settings g_settings;
 
-class Quadric : public Test
+class QuadricShapes : public Test
 {
 public:
-	Quadric()
+	QuadricShapes()
 	{
+		g_camera.m_center.Set(2.0f, -2.0f, 0.0f);
 		g_camera.m_zoom = 20.0f;
-		g_camera.m_q = b3Quat(b3Vec3(0.0f, 1.0f, 0.0f), 0.15f * B3_PI);
-		g_camera.m_q = g_camera.m_q * b3Quat(b3Vec3(1.0f, 0.0f, 0.0f), -0.15f * B3_PI);
-		g_camera.m_center.SetZero();
+		g_settings.drawCenterOfMasses = true;
 
 		{
-			qhHull hull;
-
 			b3StackArray<b3Vec3, 32> points;
 			ConstructCone(points);
 
 			u32 size = qhGetMemorySize(points.Count());
 			void* p = b3Alloc(size);
+			
+			qhHull hull;
 			hull.Construct(p, points);
 			m_coneHull = ConvertHull(hull);
+			
 			b3Free(p);
 		}
 		
 		{
-			qhHull hull;
 
 			b3StackArray<b3Vec3, 32> points;
 			ConstructCylinder(points);
 
-			u32 size = qhGetMemorySize(points.Count());
+			const u32 size = qhGetMemorySize(points.Count());
 			void* p = b3Alloc(size);
+			
+			qhHull hull;
 			hull.Construct(p, points);
 			m_cylinderHull = ConvertHull(hull);
+			
 			b3Free(p);
 		}
 
@@ -85,7 +87,7 @@ public:
 			hull.m_hull = &m_coneHull;
 
 			b3ShapeDef sdef;
-			sdef.density = 0.2f;
+			sdef.density = 0.1f;
 			sdef.friction = 0.3f;
 			sdef.shape = &hull;
 
@@ -103,7 +105,7 @@ public:
 			hull.m_hull = &m_cylinderHull;
 
 			b3ShapeDef sdef;
-			sdef.density = 1.0f;
+			sdef.density = 0.1f;
 			sdef.friction = 0.3f;
 			sdef.shape = &hull;
 
@@ -111,7 +113,7 @@ public:
 		}
 	}
 
-	~Quadric()
+	~QuadricShapes()
 	{
 		{
 			b3Free(m_coneHull.vertices);
@@ -130,7 +132,7 @@ public:
 
 	static Test* Create()
 	{
-		return new Quadric();
+		return new QuadricShapes();
 	}
 
 	b3Hull m_coneHull;

include/testbed/tests/quickhull_test.h

@@ -25,6 +25,70 @@ extern DebugDraw* g_debugDraw;
 extern Camera g_camera;
 extern Settings g_settings;
 
+inline b3Vec3 ComputeCentroid(const b3Hull& h)
+{
+	b3Vec3 c(0.0f, 0.0f, 0.0f);
+	float32 volume = 0.0f;
+	
+	// Pick reference point not too away from the origin 
+	// to minimize floating point rounding errors.
+	b3Vec3 p1(0.0f, 0.0f, 0.0f);
+	// Put it inside the hull.
+	for (u32 i = 0; i < h.vertexCount; ++i)
+	{
+		p1 += h.vertices[i];
+	}
+	p1 *= 1.0f / float32(h.vertexCount);
+
+	const float32 inv4 = 0.25f;
+	const float32 inv6 = 1.0f / 6.0f;
+	const float32 inv60 = 1.0f / 60.0f;
+	const float32 inv120 = 1.0f / 120.0f;
+
+	b3Vec3 diag(0.0f, 0.0f, 0.0f);
+	b3Vec3 offDiag(0.0f, 0.0f, 0.0f);
+
+	// Triangulate convex polygons
+	for (u32 i = 0; i < h.faceCount; ++i)
+	{
+		const b3Face* face = h.GetFace(i);
+		const b3HalfEdge* begin = h.GetEdge(face->edge);
+
+		const b3HalfEdge* edge = h.GetEdge(begin->next);
+		do
+		{
+			u32 i1 = begin->origin;
+			u32 i2 = edge->origin;
+			const b3HalfEdge* next = h.GetEdge(edge->next);
+			u32 i3 = next->origin;
+
+			b3Vec3 p2 = h.vertices[i1];
+			b3Vec3 p3 = h.vertices[i2];
+			b3Vec3 p4 = h.vertices[i3];
+
+			b3Vec3 e1 = p2 - p1;
+			b3Vec3 e2 = p3 - p1;
+			b3Vec3 e3 = p4 - p1;
+
+			float32 D = b3Det(e1, e2, e3);
+			
+			float32 tetraVolume = inv6 * D;
+			volume += tetraVolume;
+			
+			// Volume weighted centroid
+			c += tetraVolume * inv4 * (e1 + e2 + e3);
+
+			edge = next;
+		} while (h.GetEdge(edge->next) != begin);
+	}
+
+	// Centroid
+	B3_ASSERT(volume > B3_EPSILON);
+	c *= 1.0f / volume;
+	c += p1;
+	return c;
+}
+
 struct Pair
 {
 	void* key;
@@ -195,14 +259,7 @@ inline b3Hull ConvertHull(const qhHull& hull)
 	out.faceCount = faceCount;
 	out.faces = faces;
 	out.planes = planes;
-	out.centroid.SetZero();
-
-	for (u32 i = 0; i < vertexCount; ++i)
-	{
-		out.centroid += vertices[i];
-	}
-	out.centroid /= float32(vertexCount);
-
+	out.centroid = ComputeCentroid(out);
 	out.Validate();
 	return out;
 }
@@ -351,23 +408,6 @@ public:
 		m_qhull.Draw(g_debugDraw);
 	}
 
-	void KeyDown(int button)
-	{
-		if (button == GLFW_KEY_LEFT)
-		{
-			//m_index = b3Max(m_index - 1, 0);
-		}
-
-		if (button == GLFW_KEY_RIGHT)
-		{
-			//m_index = b3Min(m_index + 1, i32(horizon.Count()) - 1);
-		}
-
-		if (button == GLFW_KEY_I)
-		{
-		}
-	}
-
 	static Test* Create()
 	{
 		return new QuickhullTest();