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