/// <summary> /// Calculates the inertia of the shape relative to the center of mass. /// </summary> /// <param name="shape"></param> /// <param name="centerOfMass"></param> /// <param name="inertia">Returns the inertia relative to the center of mass, not to the origin</param> /// <returns></returns> #region public static double CalculateMassInertia(Shape shape, out Vector3D centerOfMass, out MatrixD inertia) public static double CalculateMassInertia(Shape shape, out Vector3D centerOfMass, out MatrixD inertia) { double mass = 0.0f; centerOfMass = Vector3D.Zero; inertia = MatrixD.Zero; //if (shape is Multishape) throw new ArgumentException("Can't calculate inertia of multishapes.", "shape"); // build a triangle hull around the shape List <Vector3D> hullTriangles = new List <Vector3D>(); shape.MakeHull(ref hullTriangles, 3); // create inertia of tetrahedron with vertices at // (0,0,0) (1,0,0) (0,1,0) (0,0,1) double a = 1.0f / 60.0f, b = 1.0f / 120.0f; MatrixD C = new MatrixD(a, b, b, b, a, b, b, b, a); for (int i = 0; i < hullTriangles.Count; i += 3) { Vector3D column0 = hullTriangles[i + 0]; Vector3D column1 = hullTriangles[i + 1]; Vector3D column2 = hullTriangles[i + 2]; MatrixD A = new MatrixD(column0.X, column1.X, column2.X, column0.Y, column1.Y, column2.Y, column0.Z, column1.Z, column2.Z); double detA = A.Determinant(); // now transform this canonical tetrahedron to the target tetrahedron // inertia by a linear transformation A MatrixD tetrahedronInertia = MatrixD.Multiply(A * C * MatrixD.Transpose(A), detA); Vector3D tetrahedronCOM = (1.0f / 4.0f) * (hullTriangles[i + 0] + hullTriangles[i + 1] + hullTriangles[i + 2]); double tetrahedronMass = (1.0f / 6.0f) * detA; inertia += tetrahedronInertia; centerOfMass += tetrahedronMass * tetrahedronCOM; mass += tetrahedronMass; } inertia = MatrixD.Multiply(MatrixD.Identity, Trace(inertia.M11, inertia.M22, inertia.M33)) - inertia; //test was inertia.Trace() centerOfMass = centerOfMass * (1.0f / mass); double x = centerOfMass.X; double y = centerOfMass.Y; double z = centerOfMass.Z; // now translate the inertia by the center of mass MatrixD t = new MatrixD( -mass * (y * y + z * z), mass * x * y, mass * x * z, mass * y * x, -mass * (z * z + x * x), mass * y * z, mass * z * x, mass * z * y, -mass * (x * x + y * y)); MatrixD.Add(ref inertia, ref t, out inertia); return(mass); }