public static FP CalculateMassInertia(Shape shape, out TSVector centerOfMass, out TSMatrix inertia)
{
FP fP = FP.Zero;
centerOfMass = TSVector.zero;
inertia = TSMatrix.Zero;
bool flag = shape is Multishape;
if (flag)
{
throw new ArgumentException("Can't calculate inertia of multishapes.", "shape");
}
List <TSVector> list = new List <TSVector>();
shape.MakeHull(ref list, 3);
FP fP2 = FP.One / (60 * FP.One);
FP fP3 = FP.One / (120 * FP.One);
TSMatrix value = new TSMatrix(fP2, fP3, fP3, fP3, fP2, fP3, fP3, fP3, fP2);
for (int i = 0; i < list.Count; i += 3)
{
TSVector tSVector = list[i];
TSVector tSVector2 = list[i + 1];
TSVector tSVector3 = list[i + 2];
TSMatrix tSMatrix = new TSMatrix(tSVector.x, tSVector2.x, tSVector3.x, tSVector.y, tSVector2.y, tSVector3.y, tSVector.z, tSVector2.z, tSVector3.z);
FP fP4 = tSMatrix.Determinant();
TSMatrix value2 = TSMatrix.Multiply(tSMatrix * value * TSMatrix.Transpose(tSMatrix), fP4);
TSVector value3 = FP.One / (4 * FP.One) * (list[i] + list[i + 1] + list[i + 2]);
FP fP5 = FP.One / (6 * FP.One) * fP4;
inertia += value2;
centerOfMass += fP5 * value3;
fP += fP5;
}
inertia = TSMatrix.Multiply(TSMatrix.Identity, inertia.Trace()) - inertia;
centerOfMass *= FP.One / fP;
FP x = centerOfMass.x;
FP y = centerOfMass.y;
FP z = centerOfMass.z;
TSMatrix tSMatrix2 = new TSMatrix(-fP * (y * y + z * z), fP * x * y, fP * x * z, fP * y * x, -fP * (z * z + x * x), fP * y * z, fP * z * x, fP * z * y, -fP * (x * x + y * y));
TSMatrix.Add(ref inertia, ref tSMatrix2, out inertia);
return(fP);
}