Example #1
0
        /// <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);
        }