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

include/bounce/common/math/quat.h

@@ -118,16 +118,13 @@ struct b3Quat
 			*axis = s * v;
 		}
 		
-		*angle = 0.0f;
 		// cosine check
-		if (w >= -1.0f && w <= 1.0f)
+		float32 cosine = b3Clamp(w, -1.0f, 1.0f);
-		{
 		// half angle
-			float32 theta = acos(w);
+		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/time_step.h

@@ -51,22 +51,42 @@ enum b3LimitState
 	e_equalLimits
 };
 
-// Move an inertia tensor from the its current center
+// Return the Steiner's matrix given the displacement vector from the old 
-// to another.
+// center of rotation to the new center of rotation.
-inline b3Mat33 b3MoveToCOM(const b3Mat33& inertia, float32 mass, const b3Vec3& center)
+// The result equals to transpose( skew(v) ) * skew(v) or diagonal(v^2) - outer(v)
+inline b3Mat33 b3Steiner(const b3Vec3& v)
 {
-	// Paralell Axis Theorem
+	float32 xx = v.x * v.x;
-	// J = I + m * dot(r, r) * E - outer(r, r)
+	float32 yy = v.y * v.y;
-	// where
+	float32 zz = v.z * v.z;
-	// I - inertia about the center of mass
+
-	// m - mass
+	b3Mat33 S;
-	// E - identity 3x3
+	
-	// r - displacement vector from the current com to the new com
+	S.x.x = yy + zz;
-	// J - inertia tensor at the new center of rotation
+	S.x.y = -v.x * v.y;
-	float32 dd = b3Dot(center, center);
+	S.x.z = -v.x * v.z;
-	b3Mat33 A = b3Diagonal(mass * dd);
+
-	b3Mat33 B = b3Outer(center, center);
+	S.y.x = S.x.y;
-	return inertia + A - B;
+	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
+#ifndef QUADRIC_SHAPES_H
-#define QUADRIC_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_settings.drawCenterOfMasses = true;
-		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();
 
 		{
-			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();
+	out.centroid = ComputeCentroid(out);
-
-	for (u32 i = 0; i < vertexCount; ++i)
-	{
-		out.centroid += vertices[i];
-	}
-	out.centroid /= float32(vertexCount);
-
 	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();

src/bounce/dynamics/body.cpp

@@ -220,8 +220,7 @@ void b3Body::ResetMass()
 		return;
 	}
 
-	// Compute mass data for each shape. Each shape contributes to 
+	// Accumulate the mass about the body origin of all shapes.
-	// this body mass data.
 	b3Vec3 localCenter;
 	localCenter.SetZero();
 	for (b3Shape* s = m_shapeList.m_head; s; s = s->m_next)
@@ -233,6 +232,7 @@ void b3Body::ResetMass()
 		
 		b3MassData massData;
 		s->ComputeMass(&massData, s->m_density);
+		
 		localCenter += massData.mass * massData.center;
 		m_mass += massData.mass;
 		m_I += massData.I;
@@ -240,11 +240,17 @@ void b3Body::ResetMass()
 
 	if (m_mass > 0.0f) 
 	{
+		// Compute local center of mass.
 		m_invMass = 1.0f / m_mass;
 		localCenter *= m_invMass;
-		// Center inertia about the center of mass.
+
-		m_I = b3MoveToCOM(m_I, m_mass, localCenter);
+		// Shift inertia about the body origin into the body local center of mass.
+		m_I = m_I - m_mass * b3Steiner(localCenter);
+		
+		// Compute inverse inertia about the body local center of mass.
 		m_invI = b3Inverse(m_I);
+
+		// Align the inverse inertia with the world frame of the body.
 		m_worldInvI = b3RotateToFrame(m_invI, m_xf.rotation);
 
 		// Fix rotation.
@@ -353,8 +359,7 @@ void b3Body::SetMassData(const b3MassData* massData)
 	if (m_mass > 0.0f)
 	{
 		m_invMass = 1.0f / m_mass;
-
+		m_I = massData->I - m_mass * b3Steiner(massData->center);
-		m_I = b3MoveToCOM(massData->I, m_mass, massData->center);
 		m_invI = b3Inverse(m_I);
 		m_worldInvI = b3RotateToFrame(m_invI, m_xf.rotation);
 

src/bounce/dynamics/contacts/collide/collide_capsules.cpp

@@ -60,34 +60,6 @@ void b3CollideCapsuleAndCapsule(b3Manifold& manifold,
 
 	float32 totalRadius = hullA.radius + hullB.radius;
 	
-	// todo return small distance output struct?
-	b3Vec3 pointA, pointB;
-	b3ClosestPointsOnSegments(&pointA, &pointB, hullA.vertices[0], hullA.vertices[1], hullB.vertices[0], hullB.vertices[1]);
-	float32 distance = b3Distance(pointA, pointB);
-	if (distance > totalRadius)
-	{
-		return;
-	}
-
-	if (distance > B3_EPSILON)
-	{
-		b3Vec3 normal = (pointB - pointA) / distance;
-		b3Vec3 center = 0.5f * (pointA + hullA.radius * normal + pointB - hullB.radius * normal);
-
-		manifold.pointCount = 1;
-		manifold.points[0].triangleKey = B3_NULL_TRIANGLE;
-		manifold.points[0].key = 0;
-		manifold.points[0].localNormal = b3MulT(xfA.rotation, normal);
-		manifold.points[0].localPoint = b3MulT(xfA, pointA);
-		manifold.points[0].localPoint2 = b3MulT(xfB, pointB);
-
-		manifold.center = center;
-		manifold.normal = normal;
-		manifold.tangent1 = b3Perp(normal);
-		manifold.tangent2 = b3Cross(manifold.tangent1, normal);
-		return;
-	}
-
 	if (b3AreParalell(hullA, hullB))
 	{
 		// Clip edge A against the side planes of edge B.
@@ -97,8 +69,6 @@ void b3CollideCapsuleAndCapsule(b3Manifold& manifold,
 		b3ClipVertex clipEdgeA[2];
 		u32 clipCount = b3ClipEdgeToFace(clipEdgeA, edgeA, &hullB);
 
-		float32 totalRadius = hullA.radius + hullB.radius;
-
 		if (clipCount == 2)
 		{
 			b3Vec3 cp1 = b3ClosestPointOnSegment(clipEdgeA[0].position, hullB.vertices[0], hullB.vertices[1]);
@@ -136,7 +106,35 @@ void b3CollideCapsuleAndCapsule(b3Manifold& manifold,
 				manifold.normal = normal;
 				manifold.tangent1 = b3Perp(normal);
 				manifold.tangent2 = b3Cross(manifold.tangent1, normal);
+
+				return;
 			}
 		}
 	}
+
+	b3Vec3 pointA, pointB;
+	b3ClosestPointsOnSegments(&pointA, &pointB, hullA.vertices[0], hullA.vertices[1], hullB.vertices[0], hullB.vertices[1]);
+	float32 distance = b3Distance(pointA, pointB);
+	if (distance > totalRadius)
+	{
+		return;
+	}
+
+	if (distance > B3_EPSILON)
+	{
+		b3Vec3 normal = (pointB - pointA) / distance;
+		b3Vec3 center = 0.5f * (pointA + hullA.radius * normal + pointB - hullB.radius * normal);
+
+		manifold.pointCount = 1;
+		manifold.points[0].triangleKey = B3_NULL_TRIANGLE;
+		manifold.points[0].key = 0;
+		manifold.points[0].localNormal = b3MulT(xfA.rotation, normal);
+		manifold.points[0].localPoint = b3MulT(xfA, pointA);
+		manifold.points[0].localPoint2 = b3MulT(xfB, pointB);
+
+		manifold.center = center;
+		manifold.normal = normal;
+		manifold.tangent1 = b3Perp(normal);
+		manifold.tangent2 = b3Cross(manifold.tangent1, normal);
+	}
 }

src/bounce/dynamics/island.cpp

@@ -87,14 +87,13 @@ void b3Island::Solve(const b3Vec3& gravity, float32 dt, u32 velocityIterations,
 {
 	float32 h = dt;
 
-	// Measure coefficient of damping.
+	// Apply some damping.
-	// Box2D.
 	// ODE: dv/dt + c * v = 0
 	// Solution: v(t) = v0 * exp(-c * t)
 	// Step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
 	// v2 = exp(-c * dt) * v1
 	const float32 k_d = 0.1f;
-	float32 d = exp(-h * k_d);
+	float32 d = exp(-k_d * h);
 
 	// 1. Integrate velocities
 	for (u32 i = 0; i < m_bodyCount; ++i) 
@@ -121,7 +120,7 @@ void b3Island::Solve(const b3Vec3& gravity, float32 dt, u32 velocityIterations,
 			
 			// Integrate torques
 			// @todo add gyroscopic term
-			w += h * b3Mul(b->m_worldInvI, b->m_torque);
+			w += h * b->m_worldInvI * b->m_torque;
 			// Clear torques
 			b->m_torque.SetZero();
 			
@@ -211,9 +210,9 @@ void b3Island::Solve(const b3Vec3& gravity, float32 dt, u32 velocityIterations,
 			w *= ratio;
 		}
 
-		// Integrate position 
+		// Integrate linear velocity
 		x += h * v;
-		// Integrate orientation
+		// Integrate angular velocity
 		q = b3Integrate(q, w, h);
 
 		m_positions[i].x = x;

src/bounce/dynamics/shapes/capsule_shape.cpp

@@ -126,8 +126,6 @@ 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 

src/bounce/dynamics/shapes/hull_shape.cpp

@@ -18,6 +18,7 @@
 
 #include <bounce/dynamics/shapes/hull_shape.h>
 #include <bounce/collision/shapes/hull.h>
+#include <bounce/dynamics/time_step.h>
 
 b3HullShape::b3HullShape()
 {
@@ -46,7 +47,14 @@ void b3HullShape::ComputeMass(b3MassData* massData, float32 density) const
 
 	// Pick reference point not too away from the origin 
 	// to minimize floating point rounding errors.
-	b3Vec3 v1(0.0f, 0.0f, 0.0f);
+	b3Vec3 p1(0.0f, 0.0f, 0.0f);
+	
+	// Put it inside the hull.
+	for (u32 i = 0; i < m_hull->vertexCount; ++i)
+	{
+		p1 += m_hull->vertices[i];
+	}
+	p1 *= 1.0f / float32(m_hull->vertexCount);
 
 	const float32 inv4 = 0.25f;
 	const float32 inv6 = 1.0f / 6.0f;
@@ -54,7 +62,7 @@ void b3HullShape::ComputeMass(b3MassData* massData, float32 density) const
 	const float32 inv120 = 1.0f / 120.0f;
 
 	b3Vec3 diag(0.0f, 0.0f, 0.0f);
-	b3Vec3 offDiag(0.0f, 0.0f, 0.0f);
+	b3Vec3 off_diag(0.0f, 0.0f, 0.0f);
 
 	// Triangulate convex polygons
 	for (u32 i = 0; i < m_hull->faceCount; ++i)
@@ -70,23 +78,22 @@ void b3HullShape::ComputeMass(b3MassData* massData, float32 density) const
 			const b3HalfEdge* next = m_hull->GetEdge(edge->next);
 			u32 i3 = next->origin;
 
-			b3Vec3 v2 = m_hull->vertices[i1];
+			b3Vec3 p2 = m_hull->vertices[i1];
-			b3Vec3 v3 = m_hull->vertices[i2];
+			b3Vec3 p3 = m_hull->vertices[i2];
-			b3Vec3 v4 = m_hull->vertices[i3];
+			b3Vec3 p4 = m_hull->vertices[i3];
 			
-			//
+			b3Vec3 e1 = p2 - p1;
-			b3Vec3 tetraCenter = inv4 * (v1 + v2 + v3 + v4);
+			b3Vec3 e2 = p3 - p1;
+			b3Vec3 e3 = p4 - p1;
 			
-			b3Vec3 e1 = v2 - v1;
+			float32 D = b3Det(e1, e2, e3);
-			b3Vec3 e2 = v3 - v1;
-			b3Vec3 e3 = v4 - v1;
-			float32 det = b3Det(e1, e2, e3);
-			float32 tetraVolume = inv6 * det;
 
-			// Volume weighted center of mass
+			float32 tetraVolume = inv6 * D;
-			center += tetraVolume * tetraCenter;
 			volume += tetraVolume;
 			
+			// Volume weighted centroid
+			center += tetraVolume * inv4 * (e1 + e2 + e3);
+
 			// Volume weighted inertia tensor
 			// https://github.com/melax/sandbox
 			for (u32 j = 0; j < 3; ++j)
@@ -94,11 +101,11 @@ void b3HullShape::ComputeMass(b3MassData* massData, float32 density) const
 				u32 j1 = (j + 1) % 3;
 				u32 j2 = (j + 2) % 3;
 
-				diag[j] += inv60 * det *
+				diag[j] += inv60 * D *
 					(e1[j] * e2[j] + e2[j] * e3[j] + e3[j] * e1[j] +
 					 e1[j] * e1[j] + e2[j] * e2[j] + e3[j] * e3[j]);
 
-				offDiag[j] += inv120 * det  *
+				off_diag[j] += inv120 * D  *
 					(e1[j1] * e2[j2] + e2[j1] * e3[j2] + e3[j1] * e1[j2] +
 					 e1[j1] * e3[j2] + e2[j1] * e1[j2] + e3[j1] * e2[j2] +
 					 e1[j1] * e1[j2] * 2.0f + e2[j1] * e2[j2] * 2.0f + e3[j1] * e3[j2] * 2.0f);
@@ -108,23 +115,25 @@ void b3HullShape::ComputeMass(b3MassData* massData, float32 density) const
 		} while (m_hull->GetEdge(edge->next) != begin);
 	}
 
-	B3_ASSERT(volume > 0.0f);
+	// Total mass
-	float32 invVolume = 0.0f;
-	if (volume != 0.0f)
-	{
-		invVolume = 1.0f / volume;
-	}
-	
-	diag = invVolume * diag;
-	offDiag = invVolume * offDiag;
-
-	I.x.Set(diag.y + diag.z, -offDiag.z, -offDiag.y);
-	I.y.Set(-offDiag.z, diag.x + diag.z, -offDiag.x);
-	I.z.Set(-offDiag.y, -offDiag.x, diag.x + diag.y);
-
-	massData->center = invVolume * center;
 	massData->mass = density * volume;
+	
+	// Center of mass
+	B3_ASSERT(volume > B3_EPSILON);
+	float32 inv_volume = 1.0f / volume;
+	center *= inv_volume;
+	massData->center = center + p1;
+
+	// Inertia tensor relative to the local origin (p1)
+	diag = inv_volume * diag;
+	off_diag = inv_volume * off_diag;
+	
+	I.x.Set(diag.y + diag.z, -off_diag.z, -off_diag.y);
+	I.y.Set(-off_diag.z, diag.x + diag.z, -off_diag.x);
+	I.z.Set(-off_diag.y, -off_diag.x, diag.x + diag.y);
+
 	massData->I = massData->mass * I;
+	massData->I = massData->I + massData->mass * b3Steiner(p1);
 }
 
 void b3HullShape::ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const

src/bounce/dynamics/shapes/sphere_shape.cpp

@@ -38,14 +38,16 @@ void b3SphereShape::Swap(const b3SphereShape& other)
 
 void b3SphereShape::ComputeMass(b3MassData* massData, float32 density) const 
 {
-	// Compute inertia about the origin
 	float32 volume = (4.0f / 3.0f) * B3_PI * m_radius * m_radius * m_radius;
 	float32 mass = density * volume;
-	b3Mat33 Io = b3Diagonal(mass * (2.0f / 5.0f) * m_radius * m_radius);
+	
-	// Move inertia to the sphere center
+	// Inertia about the local shape center of mass 
+	// Then shift to the local body origin
+	float32 I = mass * (0.4f * m_radius * m_radius + b3Dot(m_center, m_center));
+	
 	massData->center = m_center;
 	massData->mass = mass;
-	massData->I = b3MoveToCOM(Io, mass, m_center);
+	massData->I = b3Diagonal(I);
 }
 
 void b3SphereShape::ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const 

src/testbed/framework/test_entries.cpp

@@ -27,12 +27,13 @@
 #include <testbed/tests/hull_collision.h>
 #include <testbed/tests/linear_motion.h>
 #include <testbed/tests/angular_motion.h>
+#include <testbed/tests/multiple_shapes.h>
+#include <testbed/tests/quadric_shapes.h>
 #include <testbed/tests/spring.h>
 #include <testbed/tests/newton_cradle.h>
 #include <testbed/tests/hinge_motor.h>
 #include <testbed/tests/hinge_chain.h>
 #include <testbed/tests/ragdoll.h>
-#include <testbed/tests/quadrics.h>
 #include <testbed/tests/mesh_contact_test.h>
 #include <testbed/tests/sphere_stack.h>
 #include <testbed/tests/capsule_stack.h>
@@ -61,21 +62,22 @@ TestEntry g_tests[] =
 	{ "Hull Collision", &HullAndHull::Create },
 	{ "Linear Motion", &LinearMotion::Create },
 	{ "Angular Motion", &AngularMotion::Create },
+	{ "Multiple Shapes", &MultipleShapes::Create },
+	{ "Quadric Shapes", &QuadricShapes::Create },
 	{ "Springs", &Spring::Create },
 	{ "Newton's Cradle", &NewtonCradle::Create },
 	{ "Hinge Motor", &HingeMotor::Create },
 	{ "Hinge Chain", &HingeChain::Create },
 	{ "Ragdoll", &Ragdoll::Create },
-	{ "Quadrics", &Quadric::Create },
+	{ "Thin Boxes", &Thin::Create },
 	{ "Mesh Contact Test", &MeshContactTest::Create },
 	{ "Sphere Stack", &SphereStack::Create },
 	{ "Capsule Stack", &CapsuleStack::Create },
 	{ "Box Stack", &BoxStack::Create },
 	{ "Shape Stack", &ShapeStack::Create },
 	{ "Jenga", &Jenga::Create },
-	{ "Thin Plates", &Thin::Create },
+	{ "Box Pyramid", &Pyramid::Create },
-	{ "Pyramid", &Pyramid::Create },
+	{ "Box Pyramid Rows", &Pyramids::Create },
-	{ "Pyramid Rows", &Pyramids::Create },
 	{ "Ray Cast", &RayCast::Create },
 	{ "Sensor Test", &SensorTest::Create },
 	{ "Character Test", &Character::Create },