コード例 #1
0
ファイル: TSMatrix.cs プロジェクト: GodTaeja/CodePackage
        public static void Invert(ref TSMatrix matrix, out TSMatrix result)
        {
            FP determinantInverse = 1 / matrix.Determinant();
            FP m11 = (matrix.M22 * matrix.M33 - matrix.M23 * matrix.M32) * determinantInverse;
            FP m12 = (matrix.M13 * matrix.M32 - matrix.M33 * matrix.M12) * determinantInverse;
            FP m13 = (matrix.M12 * matrix.M23 - matrix.M22 * matrix.M13) * determinantInverse;

            FP m21 = (matrix.M23 * matrix.M31 - matrix.M21 * matrix.M33) * determinantInverse;
            FP m22 = (matrix.M11 * matrix.M33 - matrix.M13 * matrix.M31) * determinantInverse;
            FP m23 = (matrix.M13 * matrix.M21 - matrix.M11 * matrix.M23) * determinantInverse;

            FP m31 = (matrix.M21 * matrix.M32 - matrix.M22 * matrix.M31) * determinantInverse;
            FP m32 = (matrix.M12 * matrix.M31 - matrix.M11 * matrix.M32) * determinantInverse;
            FP m33 = (matrix.M11 * matrix.M22 - matrix.M12 * matrix.M21) * determinantInverse;

            result.M11 = m11;
            result.M12 = m12;
            result.M13 = m13;

            result.M21 = m21;
            result.M22 = m22;
            result.M23 = m23;

            result.M31 = m31;
            result.M32 = m32;
            result.M33 = m33;
        }
コード例 #2
0
ファイル: Shape.cs プロジェクト: yuruyigit/TrueSync
        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);
        }
コード例 #3
0
ファイル: TSMatrix.cs プロジェクト: yuruyigit/TrueSync
        public static void Invert(ref TSMatrix matrix, out TSMatrix result)
        {
            FP y  = 1 / matrix.Determinant();
            FP m  = (matrix.M22 * matrix.M33 - matrix.M23 * matrix.M32) * y;
            FP m2 = (matrix.M13 * matrix.M32 - matrix.M33 * matrix.M12) * y;
            FP m3 = (matrix.M12 * matrix.M23 - matrix.M22 * matrix.M13) * y;
            FP m4 = (matrix.M23 * matrix.M31 - matrix.M21 * matrix.M33) * y;
            FP m5 = (matrix.M11 * matrix.M33 - matrix.M13 * matrix.M31) * y;
            FP m6 = (matrix.M13 * matrix.M21 - matrix.M11 * matrix.M23) * y;
            FP m7 = (matrix.M21 * matrix.M32 - matrix.M22 * matrix.M31) * y;
            FP m8 = (matrix.M12 * matrix.M31 - matrix.M11 * matrix.M32) * y;
            FP m9 = (matrix.M11 * matrix.M22 - matrix.M12 * matrix.M21) * y;

            result.M11 = m;
            result.M12 = m2;
            result.M13 = m3;
            result.M21 = m4;
            result.M22 = m5;
            result.M23 = m6;
            result.M31 = m7;
            result.M32 = m8;
            result.M33 = m9;
        }