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

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

#include "stdafx.h"
#include "math.h"
#include
#include
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 ;
}

二、Ctrl+F5执行

三、结果