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

include/bounce/common/math/quat.h

@@ -118,15 +118,12 @@ struct b3Quat
 			*axis = s * v;
 		}
 		
-		*angle = 0.0f;
 		// cosine check
-		if (w >= -1.0f && w <= 1.0f)
-		{
-			// half angle
-			float32 theta = acos(w);
-			// full angle
-			*angle = 2.0f * theta;
-		}
+		float32 cosine = b3Clamp(w, -1.0f, 1.0f);
+		// half angle
+		float32 theta = acos(cosine);
+		// full angle
+		*angle = 2.0f * theta;
 	}
 
 	float32 x, y, z, w;

include/bounce/dynamics/body.h

@@ -112,6 +112,7 @@ public:
 	
 	// Set the body world transform from a position, axis of rotation and an angle 
 	// of rotation about the axis.
+	// The transform defines a reference frame for this body world center of mass.
 	// However, manipulating a body transform during the simulation may cause non-physical behaviour.
 	void SetTransform(const b3Vec3& position, const b3Vec3& axis, float32 angle);
 	
@@ -185,13 +186,16 @@ public:
 	// Set this body mass data.
 	void SetMassData(const b3MassData* data);
 
-	// Get the linear kinetic energy of the body in Joules (kilogram-meters squared per second squared).
+	// Recalculate this body mass data based on all of its shapes.
+	void ResetMass();
+
+	// Get the linear kinetic energy of the body in Joules (kg m^2/s^2).
 	float32 GetLinearEnergy() const;
 
-	// Get the angular kinetic energy of the body in Joules (kilogram-meters squared per second squared).
+	// Get the angular kinetic energy of the body in Joules (kg m^2/s^2).
 	float32 GetAngularEnergy() const;
 
-	// Get the total kinetic energy of the body in Joules (kilogram-meters squared per second squared).
+	// Get the total kinetic energy of the body in Joules (kg m^2/s^2).
 	float32 GetEnergy() const;
 	
 	// Transform a vector to the local space of this body.
@@ -257,16 +261,8 @@ private:
 	// Destroy all joints connected to the body.
 	void DestroyJoints();
 
-	// Recalculate the mass of the body based on the shapes associated
-	// with it.
-	void ResetMass();
-
-	// Synchronize this body transform with its world 
-	// center of mass and orientation.
-	void SynchronizeTransform();
-
-	// Synchronize this body shape AABBs with the synchronized transform. 	
 	void SynchronizeShapes();
+	void SynchronizeTransform();
 
 	// Check if this body should collide with another.
 	bool ShouldCollide(const b3Body* other) const;
@@ -306,7 +302,10 @@ private:
 	b3Vec3 m_linearVelocity;
 	b3Vec3 m_angularVelocity;
 	
+	// Motion proxy for CCD.
 	b3Sweep m_sweep;
+
+	// The body origin transform. 
 	b3Transform m_xf;
 		
 	// The parent world of this body.

include/bounce/dynamics/shapes/hull_shape.h

@@ -30,8 +30,8 @@ public :
 	~b3HullShape();
 
 	void Swap(const b3HullShape& other);
-
-	void ComputeMass(b3MassData* data, float32 density) const;	
+	
+	void ComputeMass(b3MassData* data, float32 density) const;
 
 	void ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const;	
 

include/bounce/dynamics/time_step.h

@@ -51,22 +51,42 @@ enum b3LimitState
 	e_equalLimits
 };
 
-// Move an inertia tensor from the its current center
-// to another.
-inline b3Mat33 b3MoveToCOM(const b3Mat33& inertia, float32 mass, const b3Vec3& center)
+// Return the Steiner's matrix given the displacement vector from the old 
+// center of rotation to the new center of rotation.
+// The result equals to transpose( skew(v) ) * skew(v) or diagonal(v^2) - outer(v)
+inline b3Mat33 b3Steiner(const b3Vec3& v)
 {
-	// Paralell Axis Theorem
-	// J = I + m * dot(r, r) * E - outer(r, r)
-	// where
-	// I - inertia about the center of mass
-	// m - mass
-	// E - identity 3x3
-	// r - displacement vector from the current com to the new com
-	// J - inertia tensor at the new center of rotation
-	float32 dd = b3Dot(center, center);
-	b3Mat33 A = b3Diagonal(mass * dd);
-	b3Mat33 B = b3Outer(center, center);
-	return inertia + A - B;
+	float32 xx = v.x * v.x;
+	float32 yy = v.y * v.y;
+	float32 zz = v.z * v.z;
+
+	b3Mat33 S;
+	
+	S.x.x = yy + zz;
+	S.x.y = -v.x * v.y;
+	S.x.z = -v.x * v.z;
+
+	S.y.x = S.x.y;
+	S.y.y = xx + zz;
+	S.y.z = -v.y * v.z;
+
+	S.z.x = S.x.z;
+	S.z.y = S.y.z;
+	S.z.z = xx + yy;
+
+	return S;
+}
+
+// Move an inertia tensor given the displacement vector from the center of mass to the translated origin.
+inline b3Mat33 b3MoveToOrigin(const b3Mat33& I, const b3Vec3& v)
+{
+	return I + b3Steiner(v);
+}
+
+// Move an inertia tensor given the displacement vector from the origin to the translated center of mass.
+inline b3Mat33 b3MoveToCOM(const b3Mat33& I, const b3Vec3& v)
+{
+	return I - b3Steiner(v);
 }
 
 // Compute the inertia matrix of a body measured in 

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();