[代码|Code]四元数≒矩阵|Quaternions≒Matrix

Jerry小坏

by Akatsuki

1. 
   
2. // 四元数转矩阵[] Quaternions To Matrix
   
3. void QuatMatrix(const float in[4], float out[4][4])
   
4. {
   
5.     float xx, xy, xz, xw;
   
6.     float yy, yz, yw;
   
7.     float zz, zw;
   
8. 
   
9.     xx = in[0]*in[0];
   
10.     xy = in[0]*in[1];
    
11.     xz = in[0]*in[2];
    
12.     xw = in[0]*in[3];
    
13.     yy = in[1]*in[1];
    
14.     yz = in[1]*in[2];
    
15.     yw = in[1]*in[3];
    
16.     zz = in[2]*in[2];
    
17.     zw = in[2]*in[3];
    
18. 
    
19.     out[0][0] = 1-2*(yy+zz);
    
20.     out[0][1] =   2*(xy-zw);
    
21.     out[0][2] =   2*(xz+yw);
    
22.     out[0][3] =   0;
    
23.     out[1][0] =   2*(xy+zw);
    
24.     out[1][1] = 1-2*(xx+zz);
    
25.     out[1][2] =   2*(yz-xw);
    
26.     out[1][3] =   0;
    
27.     out[2][0] =   2*(xz-yw);
    
28.     out[2][1] =   2*(yz+xw);
    
29.     out[2][2] = 1-2*(xx+yy);
    
30.     out[2][3] =   0;
    
31.     out[3][0] =   0;
    
32.     out[3][1] =   0;
    
33.     out[3][2] =   0;
    
34.     out[3][3] =   1;
    
35. }
    
36. 
    
37. // 矩阵转四元数[] Matrix To Quaternions
    
38. void MatrixQuat(const float in[4][4], float out[4])
    
39. {
    
40.     float s;
    
41. 
    
42.     if (in[0][0]>in[1][1] && in[0][0]>in[2][2])
    
43.     {
    
44.         s = 2*sqrt(1.0+in[0][0]-in[1][1]-in[2][2]);
    
45.         out[0] = 0.25*s;
    
46.         out[1] = (in[1][0]+in[0][1])/s;
    
47.         out[2] = (in[0][2]+in[2][0])/s;
    
48.         out[3] = (in[2][1]-in[1][2])/s;
    
49.     }
    
50.     else if (in[1][1]>in[2][2])
    
51.     {
    
52.         s = 2*sqrt(1.0+in[1][1]-in[0][0]-in[2][2]);
    
53.         out[0] = (in[1][0]+in[0][1])/s;
    
54.         out[1] = 0.25*s;
    
55.         out[2] = (in[2][1]+in[1][2])/s;
    
56.         out[3] = (in[0][2]-in[2][0])/s;
    
57.     }
    
58.     else
    
59.     {
    
60.         s = 2*sqrt(1.0+in[2][2]-in[0][0]-in[1][1]);
    
61.         out[0] = (in[0][2]+in[2][0])/s;
    
62.         out[1] = (in[2][1]+in[1][2])/s;
    
63.         out[2] = 0.25*s;
    
64.         out[3] = (in[1][0]-in[0][1])/s;
    
65.     }
    
66. }
    

复制代码