[代码|Code]欧拉角≒矩阵|EulerAngles≒Matrix

Jerry小坏

by Akatsuki

1. 
   
2. // 欧拉角转矩阵[] EulerAngles To Matrix
   
3. void AngleMatrix(const float in[3], float out[4][4])
   
4. {
   
5.     float cx, sx, cy, sy, cz, sz;
   
6.     float yx, yy;
   
7. 
   
8.     cx = cos(in[0]);
   
9.     sx = sin(in[0]);
   
10.     cy = cos(in[1]);
    
11.     sy = sin(in[1]);
    
12.     cz = cos(in[2]);
    
13.     sz = sin(in[2]);
    
14.    
    
15.     yx = sy*cx;
    
16.     yy = sy*sx;
    
17. 
    
18.     out[0][0] =  cy*cz;
    
19.     out[0][1] = -cy*sz;
    
20.     out[0][2] =     sy;
    
21.     out[0][3] =  0;
    
22.     out[1][0] =  yy*cz+cx*sz;
    
23.     out[1][1] = -yy*sz+cx*cz;
    
24.     out[1][2] = -sx;
    
25.     out[1][3] =  0;
    
26.     out[2][0] = -yx*cz+sx*sz;
    
27.     out[2][1] =  yx*sz+sx*cz;
    
28.     out[2][2] =  cx*cy;
    
29.     out[2][3] =  0;
    
30.     out[3][0] =  0;
    
31.     out[3][1] =  0;
    
32.     out[3][2] =  0;
    
33.     out[3][3] =  1;
    
34. }
    
35. 
    
36. // 矩阵转欧拉角[] Matrix To EulerAngles
    
37. void MatrixAngle(const float in[4][4], float out[3])
    
38. {
    
39.     float c;
    
40.     float tx, ty;
    
41. 
    
42.     out[1] = asin(in[0][2]);
    
43.     c = cos(out[1]);
    
44.     if (fabs(c)>0.005)
    
45.     {
    
46.         tx =  in[2][2]/c;
    
47.         ty = -in[1][2]/c;
    
48.         out[0] = atan2(ty, tx);
    
49.         tx =  in[0][0]/c;
    
50.         ty = -in[0][1]/c;
    
51.         out[2] = atan2(ty, tx);
    
52.     }
    
53.     else
    
54.     {
    
55.         out[0] = 0;
    
56.         tx = in[1][1];
    
57.         ty = in[1][0];
    
58.         out[2] = atan2(ty, tx);
    
59.     }
    
60. }
    

复制代码