///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);
}
///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.
public void CalculatePrincipalAxisTransform(ref IndexedMatrix principal, out IndexedVector3 inertia, float volume)
{
CenterCallback centerCallback = new CenterCallback();
IndexedVector3 aabbMax = MathUtil.MAX_VECTOR;
IndexedVector3 aabbMin = MathUtil.MIN_VECTOR;
m_stridingMesh.InternalProcessAllTriangles(centerCallback, ref aabbMin, ref aabbMax);
IndexedVector3 center = centerCallback.GetCenter();
principal._origin = center;
volume = centerCallback.GetVolume();
InertiaCallback inertiaCallback = new InertiaCallback(ref center);
m_stridingMesh.InternalProcessAllTriangles(inertiaCallback, ref aabbMax, ref aabbMax);
IndexedBasisMatrix i = inertiaCallback.GetInertia();
i.Diagonalize(out principal, 0.00001f, 20);
//i.diagonalize(principal.getBasis(), 0.00001f, 20);
inertia = new IndexedVector3(i[0, 0], i[1, 1], i[2, 2]);
inertia /= volume;
}