[代码|Code]欧拉角≒四元数|EulerAngles≒Quaternions

Jerry小坏

by Akatsuki

1. 
   
2. // 欧拉角转四元数[] EulerAngles To Quaternions
   
3. void AngleQuat(const float in[3], float out[4])
   
4. {
   
5.     out[3] = cos(in[0]/2.0)*cos(in[1]/2.0)*cos(in[2]/2.0) + sin(in[0]/2.0)*sin(in[1]/2.0)*sin(in[2]/2.0);
   
6.     out[0] = sin(in[0]/2.0)*cos(in[1]/2.0)*cos(in[2]/2.0) - cos(in[0]/2.0)*sin(in[1]/2.0)*sin(in[2]/2.0);
   
7.     out[1] = cos(in[0]/2.0)*sin(in[1]/2.0)*cos(in[2]/2.0) + sin(in[0]/2.0)*cos(in[1]/2.0)*sin(in[2]/2.0);
   
8.     out[2] = cos(in[0]/2.0)*cos(in[1]/2.0)*sin(in[2]/2.0) - sin(in[0]/2.0)*sin(in[1]/2.0)*cos(in[2]/2.0);
   
9. }
   
10. 
    
11. // 四元数转欧拉角[] Quaternions To EulerAngles
    
12. void QuatAngle(const float in[4], float out[3])
    
13. {
    
14.     out[0] = atan2(2*(in[3]*in[0]+in[1]*in[2]), 1-2*(in[0]*in[0]+in[1]*in[1]));
    
15.     out[1] = atan(2*(in[3]*in[1]-in[2]*in[0]));
    
16.     out[2] = atan2(2*(in[3]*in[2]+in[0]*in[1]), 1-2*(in[1]*in[1]+in[2]*in[2]));
    
17. }
    

复制代码