四元数和欧拉角的相互转换

四元数转欧拉角&欧拉角转四元数程序：四元数转欧拉角&欧拉角转四元数程序。

?

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47

#include "stdafx.h" #include "math.h" #include<iostream> #include<string.h> using namespace std;   void EulerAnglesToQuaternion( double roll, double yaw, double pitch) {      double cosRoll,sinRoll,cosPitch,sinPitch,cosyaw,sinyaw,qw,qx,qy,qz;      cosRoll = cosf(roll * 0 .5f); //z      sinRoll = sinf(roll * 0 .5f); //z        cosyaw= cosf(yaw * 0 .5f); //y      sinyaw = sinf(yaw * 0 .5f); //y        cosPitch = cosf(pitch * 0 .5f); //x      sinPitch = sinf(pitch * 0 .5f); //x          qw = cosRoll * cosyaw* cosPitch + sinRoll * sinyaw * sinPitch;      qx = cosRoll * sinyaw * cosPitch + sinRoll * cosyaw * sinPitch;      qy = cosRoll * cosyaw * sinPitch - sinRoll * sinyaw * cosPitch;      qz = sinRoll * cosyaw * cosPitch - cosRoll * sinyaw * sinPitch;      cout << "qw = " << qw << endl;      cout << "qx = " << qx << endl;      cout << "qy = " << qy << endl;      cout << "qz = " << qz << endl;   } void QuaternionToEulerAngles( double qw, double qx, double qy, double qz) {      double roll, yaw, pitch;      roll = atan2f( 2 .f * (qw*qz + qx*qy), 1 - 2 *(qz*qz+qx*qx)); //Z      yaw =  asinf( 2 .f * (qw*qx - qy*qz)); //Y      pitch =atan2f( 2 .f * (qw*qy + qz*qx), 1 - 2 *(qy*qy+qx*qx)); //X        cout << "roll = " << roll << endl;      cout << "yaw = " << yaw << endl;      cout << "pitch = " << pitch << endl; }   int main() {      QuaternionToEulerAngles( 0 , 0.707106 , 0.707106 , 0 );      EulerAnglesToQuaternion( 1.5707 , 0 , 3.14159 );      return 0 ; }</string.h></iostream>

二、Ctrl+F5执行

三、结果