Esempio n. 1
0
        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);
        }