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