#define PAI 3.141592653589793
#define RADIAN(PAI / 180.0 ) //弧度 = 角度 * π / 180
#define ANGLE (180.0 / PAI ) //角度 = 弧度 * 180 / π
* @param arr : 旋转矩阵数组 N O A
* @retval 欧拉角(弧度值) 偏航角,俯仰角,滚动角
float *Matrix_Eulerian_Angle(float *arr)
E[0] = atan2(T[1],T[0]);//Ny/Nx
E[1] = atan2(-1*T[2],(T[0]*cos(E[0])+T[1]*sin(E[0])));//Nz/(Nx+Ny)
E[2] = atan2((T[6]*sin(E[0])-T[7]*cos(E[0])),(-1*T[3]*sin(E[0])+T[4]*cos(E[0])));//Ax-Ay/-Ox+Oy
* @param arr : 欧拉角(弧度值) 偏航角,俯仰角,滚动角
Eulerian_Angle_Matrix(float *arr)
Q_D(FitControlRobotWidget);
T[0] = 1*(cos(E[0])*cos(E[1]));
T[1] = 1*(sin(E[0])*cos(E[1]));
/*O*/
T[3] = 1*(cos(E[0])*sin(E[1])*sin(E[2])-sin(E[0])*cos(E[2]));
T[4] = 1*(sin(E[0])*sin(E[1])*sin(E[2])+cos(E[0])*cos(E[2]));
T[5] = 1*(cos(E[1])*sin(E[2]));
T[6] = 1*(cos(E[0])*sin(E[1])*cos(E[2])+sin(E[0])*sin(E[2]));
T[7] = 1*(sin(E[0])*sin(E[1])*cos(E[2])-cos(E[0])*sin(E[2]));
T[8] = 1*(cos(E[1])*cos(E[2]));