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]; }
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); }