public override Matrix3x3 ComputeVolumeDistribution(out float volume)
{
Vector3 center = ComputeCenter();
volume = ComputeVolume(); //Just approximate.
//Calculate distribution of mass.
const float massPerPoint = .333333333f;
//Subtract the position from the distribution, moving into a 'body space' relative to itself.
// [ (j * j + z * z) (-j * j) (-j * z) ]
//I = I + [ (-j * j) (j * j + z * z) (-j * z) ]
// [ (-j * z) (-j * z) (j * j + j * j) ]
float i = vA.X - center.X;
float j = vA.Y - center.Y;
float k = vA.Z - center.Z;
//localInertiaTensor += new Matrix(j * j + k * k, -j * j, -j * k, 0, -j * j, j * j + k * k, -j * k, 0, -j * k, -j * k, j * j + j * j, 0, 0, 0, 0, 0); //No mass per point.
var volumeDistribution = new Matrix3x3(massPerPoint * (j * j + k * k), massPerPoint * (-i * j), massPerPoint * (-i * k),
massPerPoint * (-i * j), massPerPoint * (i * i + k * k), massPerPoint * (-j * k),
massPerPoint * (-i * k), massPerPoint * (-j * k), massPerPoint * (i * i + j * j));
i = vB.X - center.X;
j = vB.Y - center.Y;
k = vB.Z - center.Z;
var pointContribution = new Matrix3x3(massPerPoint * (j * j + k * k), massPerPoint * (-i * j), massPerPoint * (-i * k),
massPerPoint * (-i * j), massPerPoint * (i * i + k * k), massPerPoint * (-j * k),
massPerPoint * (-i * k), massPerPoint * (-j * k), massPerPoint * (i * i + j * j));
Matrix3x3.Add(ref volumeDistribution, ref pointContribution, out volumeDistribution);
i = vC.X - center.X;
j = vC.Y - center.Y;
k = vC.Z - center.Z;
pointContribution = new Matrix3x3(massPerPoint * (j * j + k * k), massPerPoint * (-i * j), massPerPoint * (-i * k),
massPerPoint * (-i * j), massPerPoint * (i * i + k * k), massPerPoint * (-j * k),
massPerPoint * (-i * k), massPerPoint * (-j * k), massPerPoint * (i * i + j * j));
Matrix3x3.Add(ref volumeDistribution, ref pointContribution, out volumeDistribution);
return volumeDistribution;
}
