예제 #1
0
 public void tranformRotate()
 {
     if (parent == null) // Root
     {
         double[,] matrixT = { { 1, 0, 0, tx }, { 0, 1, 0, ty }, { 0, 0, 1, tz }, { 0, 0, 0, 1 } };
         matrixM           = TheTool.Matrix_Multiply(matrixT, rotationMatrix);
     }
     else
     {
         double[,] matrixT = { { 1, 0, 0, offsetX }, { 0, 1, 0, offsetY }, { 0, 0, 1, offsetZ }, { 0, 0, 0, 1 } };
         matrixM           = TheTool.Matrix_Multiply(parent.matrixM, TheTool.Matrix_Multiply(matrixT, rotationMatrix));
     }
     x = matrixM[0, 3];
     y = matrixM[1, 3];
     z = matrixM[2, 3];
 }
예제 #2
0
        public static double[,] createRotationMatrix(double a_x_deg, double a_y_deg, double a_z_deg, string rotationOrder)
        {
            double[,] matrix_r = new double[4, 4];
            double a_x = a_x_deg * 0.01745329252;//to radian
            double a_y = a_y_deg * 0.01745329252;
            double a_z = a_z_deg * 0.01745329252;

            if (rotationOrder == "zyx")
            {
                return(new double[, ] {
                    { Math.Cos(a_y) * Math.Cos(a_z),
                      Math.Cos(a_z) * Math.Sin(a_x) * Math.Sin(a_y) - Math.Sin(a_z) * Math.Cos(a_x),
                      Math.Cos(a_z) * Math.Cos(a_x) * Math.Sin(a_y) + Math.Sin(a_z) * Math.Sin(a_x),
                      0 },
                    { Math.Cos(a_y) * Math.Sin(a_z),
                      Math.Sin(a_x) * Math.Sin(a_y) * Math.Sin(a_z) + Math.Cos(a_x) * Math.Cos(a_z),
                      Math.Cos(a_x) * Math.Sin(a_y) * Math.Sin(a_z) - Math.Sin(a_x) * Math.Cos(a_z),
                      0 },
                    { -Math.Sin(a_y),
                      Math.Cos(a_y) * Math.Sin(a_x),
                      Math.Cos(a_x) * Math.Cos(a_y),
                      0 },
                    { 0, 0, 0, 1 }
                });
            }
            else
            {
                //---------------------------------------------------------------
                double[,] matrix_X = new double[, ] {
                    { 1, 0, 0, 0 },
                    { 0, Math.Cos(a_x), -Math.Sin(a_x), 0 },
                    { 0, Math.Sin(a_x), Math.Cos(a_x), 0 },
                    { 0, 0, 0, 1 }
                };
                double[,] matrix_Y = new double[, ] {
                    { Math.Cos(a_y), 0, Math.Sin(a_y), 0 },
                    { 0, 1, 0, 0 },
                    { -Math.Sin(a_y), 0, Math.Cos(a_y), 0 },
                    { 0, 0, 0, 1 }
                };
                double[,] matrix_Z = new double[, ] {
                    { Math.Cos(a_z), -Math.Sin(a_z), 0, 0 },
                    { Math.Sin(a_z), Math.Cos(a_z), 0, 0 },
                    { 0, 0, 1, 0 },
                    { 0, 0, 0, 1 }
                };
                if (rotationOrder == "zyx")
                {
                    matrix_r = TheTool.Matrix_Multiply(matrix_Z, TheTool.Matrix_Multiply(matrix_Y, matrix_X));
                }
                else if (rotationOrder == "xyz")
                {
                    matrix_r = TheTool.Matrix_Multiply(matrix_X, TheTool.Matrix_Multiply(matrix_Y, matrix_Z));
                }
                else if (rotationOrder == "xzy")
                {
                    matrix_r = TheTool.Matrix_Multiply(matrix_X, TheTool.Matrix_Multiply(matrix_Z, matrix_Y));
                }
                else if (rotationOrder == "yxz")
                {
                    matrix_r = TheTool.Matrix_Multiply(matrix_Y, TheTool.Matrix_Multiply(matrix_X, matrix_Z));
                }
                else if (rotationOrder == "yzx")
                {
                    matrix_r = TheTool.Matrix_Multiply(matrix_Y, TheTool.Matrix_Multiply(matrix_Z, matrix_X));
                }
                else
                {
                    matrix_r = TheTool.Matrix_Multiply(matrix_Z, TheTool.Matrix_Multiply(matrix_X, matrix_Y));
                }
                //TheTool.Matrix_print(matrix_r, 3);
            }
            return(matrix_r);
        }