Methods to compute more accurate inertia tensor for btCompoundShape and btConvexTriangleMeshShape.

Thanks to Ole K. for the fixes, see http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=2562

src/BulletCollision/CollisionShapes/btCompoundShape.cpp

@@ -147,5 +147,62 @@ void	btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) co
 
 }
 
-	
+
+
+
+void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const
+{
+   int n = m_children.size();
+
+   btScalar totalMass = 0;
+   btVector3 center(0, 0, 0);
+   for (int k = 0; k < n; k++)
+   {
+      center += m_children[k].m_transform.getOrigin() * masses[k];
+      totalMass += masses[k];
+   }
+   center /= totalMass;
+   principal.setOrigin(center);
+
+   btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0);
+   for (int k = 0; k < n; k++)
+   {
+      btVector3 i;
+      m_children[k].m_childShape->calculateLocalInertia(masses[k], i);
+
+      const btTransform& t = m_children[k].m_transform;
+      btVector3 o = t.getOrigin() - center;
+      
+      //compute inertia tensor in coordinate system of compound shape
+      btMatrix3x3 j = t.getBasis().transpose();
+      j[0] *= i[0];
+      j[1] *= i[1];
+      j[2] *= i[2];
+      j = t.getBasis() * j;
+      
+      //add inertia tensor
+      tensor[0] += j[0];
+      tensor[1] += j[1];
+      tensor[2] += j[2];
+
+      //compute inertia tensor of pointmass at o
+      btScalar o2 = o.length2();
+      j[0].setValue(o2, 0, 0);
+      j[1].setValue(0, o2, 0);
+      j[2].setValue(0, 0, o2);
+      j[0] += o * -o.x(); 
+      j[1] += o * -o.y(); 
+      j[2] += o * -o.z();
+
+      //add inertia tensor of pointmass
+      tensor[0] += masses[k] * j[0];
+      tensor[1] += masses[k] * j[1];
+      tensor[2] += masses[k] * j[2];
+   }
+
+   tensor.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
+   inertia.setValue(tensor[0][0], tensor[1][1], tensor[2][2]);
+}
+
+
 	

src/BulletCollision/CollisionShapes/btCompoundShape.h

@@ -142,6 +142,14 @@ public:
 		return m_aabbTree;
 	}
 
+	///computes the exact moment of inertia and the transform from the coordinate system defined by the principal axes of the moment of inertia
+	///and the center of mass to the current coordinate system. "masses" points to an array of masses of the children. The resulting transform
+	///"principal" has to be applied inversely to all children transforms in order for the local coordinate system of the compound
+	///shape to be centered at the center of mass and to coincide with the principal axes. This also necessitates a correction of the world transform
+	///of the collision object by the principal transform.
+	void calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const;
+
+
 private:
 	btScalar	m_collisionMargin;
 protected:

src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp

@@ -204,3 +204,113 @@ const btVector3& btConvexTriangleMeshShape::getLocalScaling() const
 {
 	return m_stridingMesh->getScaling();
 }
+
+void btConvexTriangleMeshShape::calculatePrincipalAxisTransform(btTransform& principal, btVector3& inertia, btScalar& volume) const
+{
+   class CenterCallback: public btInternalTriangleIndexCallback
+   {
+      bool first;
+      btVector3 ref;
+      btVector3 sum;
+      btScalar volume;
+
+   public:
+
+      CenterCallback() : first(true), ref(0, 0, 0), sum(0, 0, 0), volume(0)
+      {
+      }
+
+      virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
+      {
+         (void) triangleIndex;
+         (void) partId;
+         if (first)
+         {
+            ref = triangle[0];
+            first = false;
+         }
+         else
+         {
+            btScalar vol = btFabs((triangle[0] - ref).triple(triangle[1] - ref, triangle[2] - ref));
+            sum += (btScalar(0.25) * vol) * ((triangle[0] + triangle[1] + triangle[2] + ref));
+            volume += vol;
+         }
+      }
+      
+      btVector3 getCenter()
+      {
+         return (volume > 0) ? sum / volume : ref;
+      }
+
+      btScalar getVolume()
+      {
+         return volume * btScalar(1. / 6);
+      }
+
+   };
+
+   class InertiaCallback: public btInternalTriangleIndexCallback
+   {
+      btMatrix3x3 sum;
+      btVector3 center;
+
+   public:
+
+      InertiaCallback(btVector3& center) : sum(0, 0, 0, 0, 0, 0, 0, 0, 0), center(center)
+      {
+      }
+
+      virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
+      {
+         (void) triangleIndex;
+         (void) partId;
+         btMatrix3x3 i;
+         btVector3 a = triangle[0] - center;
+         btVector3 b = triangle[1] - center;
+         btVector3 c = triangle[2] - center;
+         btVector3 abc = a + b + c;
+         btScalar volNeg = -btFabs(a.triple(b, c)) * btScalar(1. / 6);
+         for (int j = 0; j < 3; j++)
+         {
+            for (int k = 0; k <= j; k++)
+            {
+               i[j][k] = i[k][j] = volNeg * (center[j] * center[k]
+                 + btScalar(0.25) * (center[j] * abc[k] + center[k] * abc[j])
+                  + btScalar(0.1) * (a[j] * a[k] + b[j] * b[k] + c[j] * c[k])
+                  + btScalar(0.05) * (a[j] * b[k] + a[k] * b[j] + a[j] * c[k] + a[k] * c[j] + b[j] * c[k] + b[k] * c[j]));
+            }
+         }
+         btScalar i00 = -i[0][0];
+         btScalar i11 = -i[1][1];
+         btScalar i22 = -i[2][2];
+         i[0][0] = i11 + i22; 
+         i[1][1] = i22 + i00; 
+         i[2][2] = i00 + i11;
+         sum[0] += i[0];
+         sum[1] += i[1];
+         sum[2] += i[2];
+      }
+      
+      btMatrix3x3& getInertia()
+      {
+         return sum;
+      }
+
+   };
+
+   CenterCallback centerCallback;
+   btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+   m_stridingMesh->InternalProcessAllTriangles(&centerCallback, -aabbMax, aabbMax);
+   btVector3 center = centerCallback.getCenter();
+   principal.setOrigin(center);
+   volume = centerCallback.getVolume();
+
+   InertiaCallback inertiaCallback(center);
+   m_stridingMesh->InternalProcessAllTriangles(&inertiaCallback, -aabbMax, aabbMax);
+
+   btMatrix3x3& i = inertiaCallback.getInertia();
+   i.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
+   inertia.setValue(i[0][0], i[1][1], i[2][2]);
+   inertia /= volume;
+}
+

src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h

@@ -46,6 +46,13 @@ public:
 	virtual void	setLocalScaling(const btVector3& scaling);
 	virtual const btVector3& getLocalScaling() const;
 
+	///computes the exact moment of inertia and the transform from the coordinate system defined by the principal axes of the moment of inertia
+	///and the center of mass to the current coordinate system. A mass of 1 is assumed, for other masses just multiply the computed "inertia"
+	///by the mass. The resulting transform "principal" has to be applied inversely to the mesh in order for the local coordinate system of the
+	///shape to be centered at the center of mass and to coincide with the principal axes. This also necessitates a correction of the world transform
+	///of the collision object by the principal transform. This method also computes the volume of the convex mesh.
+	void btConvexTriangleMeshShape::calculatePrincipalAxisTransform(btTransform& principal, btVector3& inertia, btScalar& volume) const;
+
 };
 
 

src/LinearMath/btMatrix3x3.h

@@ -284,6 +284,91 @@ class btMatrix3x3 {
 		}
 		
 
+		///diagonalizes this matrix by the Jacobi method. rot stores the rotation
+		///from the coordinate system in which the matrix is diagonal to the original
+		///coordinate system, i.e., old_this = rot * new_this * rot^T. The iteration
+		///stops when all off-diagonal elements are less than the threshold multiplied
+		///by the sum of the absolute values of the diagonal, or when maxSteps have
+		///been executed. Note that this matrix is assumed to be symmetric.
+		void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps)
+		{
+		 rot.setIdentity();
+		 for (int step = maxSteps; step > 0; step--)
+		 {
+			// find off-diagonal element [p][q] with largest magnitude
+			int p = 0;
+			int q = 1;
+			int r = 2;
+			btScalar max = btFabs(m_el[0][1]);
+			btScalar v = btFabs(m_el[0][2]);
+			if (v > max)
+			{
+			   q = 2;
+			   r = 1;
+			   max = v;
+			}
+			v = btFabs(m_el[1][2]);
+			if (v > max)
+			{
+			   p = 1;
+			   q = 2;
+			   r = 0;
+			   max = v;
+			}
+
+			btScalar t = threshold * (btFabs(m_el[0][0]) + btFabs(m_el[1][1]) + btFabs(m_el[2][2]));
+			if (max <= t)
+			{
+			   if (max <= SIMD_EPSILON * t)
+			   {
+				  return;
+			   }
+			   step = 1;
+			}
+
+			// compute Jacobi rotation J which leads to a zero for element [p][q] 
+			btScalar mpq = m_el[p][q];
+			btScalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq);
+			btScalar theta2 = theta * theta;
+			btScalar cos;
+			btScalar sin;
+			if (theta2 * theta2 < btScalar(10 / SIMD_EPSILON))
+			{
+			   t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2))
+										: 1 / (theta - btSqrt(1 + theta2));
+			   cos = 1 / btSqrt(1 + t * t);
+			   sin = cos * t;
+			}
+			else
+			{
+			   // approximation for large theta-value, i.e., a nearly diagonal matrix
+			   t = 1 / (theta * (2 + btScalar(0.5) / theta2));
+			   cos = 1 - btScalar(0.5) * t * t;
+			   sin = cos * t;
+			}
+
+			// apply rotation to matrix (this = J^T * this * J)
+			m_el[p][q] = m_el[q][p] = 0;
+			m_el[p][p] -= t * mpq;
+			m_el[q][q] += t * mpq;
+			btScalar mrp = m_el[r][p];
+			btScalar mrq = m_el[r][q];
+			m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq;
+			m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp;
+
+			// apply rotation to rot (rot = rot * J)
+			for (int i = 0; i < 3; i++)
+			{
+			   btVector3& row = rot[i];
+			   mrp = row[p];
+			   mrq = row[q];
+			   row[p] = cos * mrp - sin * mrq;
+			   row[q] = cos * mrq + sin * mrp;
+			}
+		 }
+		}
+
+
 		
 	protected:
 		btScalar cofac(int r1, int c1, int r2, int c2) const