110,499
社区成员
发帖
与我相关
我的任务
分享
double x_rad = Math.Atan2(System.Math.Sin(x_angle), System.Math.Cos(x_angle));
double y_rad = Math.Atan2(System.Math.Sin(y_angle), System.Math.Cos(y_angle));
double z_rad = Math.Atan2(System.Math.Sin(z_angle), System.Math.Cos(z_angle));
double r11 = Math.Cos(z_rad) * Math.Cos(y_rad);
double r12 = Math.Cos(z_rad) * Math.Sin(y_rad) * Math.Sin(x_rad) -
Math.Sin(z_rad) * Math.Cos(x_rad);
double r13 = Math.Cos(z_rad) * Math.Sin(y_rad) * Math.Cos(x_rad) +
Math.Sin(z_rad) * Math.Sin(x_rad);
double r21 = Math.Sin(z_rad) * Math.Cos(y_rad);
double r22 = Math.Sin(z_rad) * Math.Sin(y_rad) * Math.Sin(x_rad) -
Math.Cos(z_rad) * Math.Cos(x_rad);
double r23 = Math.Sin(z_rad) * Math.Sin(y_rad) * Math.Cos(x_rad) -
Math.Cos(z_rad) * Math.Sin(x_rad);
double r31 = -(Math.Sin(y_rad));
double r32 = Math.Cos(y_rad) * Math.Sin(x_rad);
double r33 = Math.Cos(y_rad) * Math.Cos(x_rad);
Matrix R_ab = new double[,] { { r11, r12, r13 }, { r21, r22, r23 }, { r31, r32, r33 } };
Matrix A = new double[,] { { x_a }, { y_a }, { z_a } };
Matrix R = R_ab.Inverse();
Matrix result = R * A;