///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.
public void CalculatePrincipalAxisTransform(IList<float> masses, ref IndexedMatrix principal, out IndexedVector3 inertia)
{
int n = m_children.Count;
float totalMass = 0;
IndexedVector3 center = IndexedVector3.Zero;
for (int k = 0; k < n; k++)
{
Debug.Assert(masses[k] > 0f);
center += m_children[k].m_transform._origin * masses[k];
totalMass += masses[k];
}
Debug.Assert(totalMass > 0f);
center /= totalMass;
principal._origin = center;
IndexedBasisMatrix tensor = new IndexedBasisMatrix();
for (int k = 0; k < n; k++)
{
IndexedVector3 i;
m_children[k].m_childShape.CalculateLocalInertia(masses[k], out i);
IndexedMatrix t = m_children[k].m_transform;
IndexedVector3 o = t._origin - center;
//compute inertia tensor in coordinate system of compound shape
IndexedBasisMatrix j = t._basis.Transpose();
j._el0 *= i.X;
j._el1 *= i.Y;
j._el2 *= i.Z;
j = t._basis * j;
//add inertia tensor
tensor._el0 += j._el0;
tensor._el1 += j._el1;
tensor._el2 += j._el2;
//tensor += j;
//compute inertia tensor of pointmass at o
float o2 = o.LengthSquared();
j._el0 = new IndexedVector3(o2, 0, 0);
j._el1 = new IndexedVector3(0, o2, 0);
j._el2 = new IndexedVector3(0, 0, o2);
j._el0 += o * -o.X;
j._el1 += o * -o.Y;
j._el2 += o * -o.Z;
//add inertia tensor of pointmass
tensor._el0 += masses[k] * j._el0;
tensor._el1 += masses[k] * j._el1;
tensor._el2 += masses[k] * j._el2;
}
tensor.Diagonalize(out principal, 0.00001f, 20);
inertia = new IndexedVector3(tensor._el0.X, tensor._el1.Y, tensor._el2.Z);
}
