frodo2975..

7

我一直在尋找類似解決方案的幾天,我終于遇到了這個(gè)網(wǎng)站,它有一個(gè)將四元數(shù)轉(zhuǎn)換為任意Euler和Tait-Bryan旋轉(zhuǎn)的算法!

這是代碼:

///

// Quaternion to Euler

///

enum RotSeq{zyx, zyz, zxy, zxz, yxz, yxy, yzx, yzy, xyz, xyx, xzy,xzx};

void twoaxisrot(double r11, double r12, double r21, double r31, double r32, double res[]){

res[0] = atan2( r11, r12 );

res[1] = acos ( r21 );

res[2] = atan2( r31, r32 );

}

void threeaxisrot(double r11, double r12, double r21, double r31, double r32, double res[]){

res[0] = atan2( r31, r32 );

res[1] = asin ( r21 );

res[2] = atan2( r11, r12 );

}

void quaternion2Euler(const Quaternion& q, double res[], RotSeq rotSeq)

{

switch(rotSeq){

case zyx:

threeaxisrot( 2*(q.x*q.y + q.w*q.z),

q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z,

-2*(q.x*q.z - q.w*q.y),

2*(q.y*q.z + q.w*q.x),

q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z,

res);

break;

case zyz:

twoaxisrot( 2*(q.y*q.z - q.w*q.x),

2*(q.x*q.z + q.w*q.y),

q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z,

2*(q.y*q.z + q.w*q.x),

-2*(q.x*q.z - q.w*q.y),

res);

break;

case zxy:

threeaxisrot( -2*(q.x*q.y - q.w*q.z),

q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z,

2*(q.y*q.z + q.w*q.x),

-2*(q.x*q.z - q.w*q.y),

q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z,

res);

break;

case zxz:

twoaxisrot( 2*(q.x*q.z + q.w*q.y),

-2*(q.y*q.z - q.w*q.x),

q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z,

2*(q.x*q.z - q.w*q.y),

2*(q.y*q.z + q.w*q.x),

res);

break;

case yxz:

threeaxisrot( 2*(q.x*q.z + q.w*q.y),

q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z,

-2*(q.y*q.z - q.w*q.x),

2*(q.x*q.y + q.w*q.z),

q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z,

res);

break;

case yxy:

twoaxisrot( 2*(q.x*q.y - q.w*q.z),

2*(q.y*q.z + q.w*q.x),

q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z,

2*(q.x*q.y + q.w*q.z),

-2*(q.y*q.z - q.w*q.x),

res);

break;

case yzx:

threeaxisrot( -2*(q.x*q.z - q.w*q.y),

q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z,

2*(q.x*q.y + q.w*q.z),

-2*(q.y*q.z - q.w*q.x),

q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z,

res);

break;

case yzy:

twoaxisrot( 2*(q.y*q.z + q.w*q.x),

-2*(q.x*q.y - q.w*q.z),

q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z,

2*(q.y*q.z - q.w*q.x),

2*(q.x*q.y + q.w*q.z),

res);

break;

case xyz:

threeaxisrot( -2*(q.y*q.z - q.w*q.x),

q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z,

2*(q.x*q.z + q.w*q.y),

-2*(q.x*q.y - q.w*q.z),

q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z,

res);

break;

case xyx:

twoaxisrot( 2*(q.x*q.y + q.w*q.z),

-2*(q.x*q.z - q.w*q.y),

q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z,

2*(q.x*q.y - q.w*q.z),

2*(q.x*q.z + q.w*q.y),

res);

break;

case xzy:

threeaxisrot( 2*(q.y*q.z + q.w*q.x),

q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z,

-2*(q.x*q.y - q.w*q.z),

2*(q.x*q.z + q.w*q.y),

q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z,

res);

break;

case xzx:

twoaxisrot( 2*(q.x*q.z - q.w*q.y),

2*(q.x*q.y + q.w*q.z),

q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z,

2*(q.x*q.z + q.w*q.y),

-2*(q.x*q.y - q.w*q.z),

res);

break;

default:

std::cout << "Unknown rotation sequence" << std::endl;

break;

}

}

總結(jié)

以上是生活随笔為你收集整理的如何将四元数方向转化为旋转举证_是否有将四元数旋转转换为欧拉角旋转的算法？...的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。