歐拉角與四元數(shù)

四元數(shù)與旋轉(zhuǎn)一.四元組基礎(chǔ)Q(x,y,z,w)，其中x,y,z用來確定旋轉(zhuǎn)軸，w為旋轉(zhuǎn)的角度Q=w+xi+yj+zk,i,j,k為三個虛軸的單位分量I*j=kJ*k=i;K*i=j;叉乘：c=axb=Iijk||a1blc1||a2b2c2|=(b1c2-b2c1,c1a2-a1c2,a1b2-a2b1)c也為一個向量，且c的長度為|a||b|sin(theta),垂直于a和b所在的平面，方向由右手法則來判定，用右手的四指先表示向量a的方向，然后手指朝著手心的方向擺動到向量b的方向，大拇指所指的方向就是向量c的方向四元組相乘：Q1=w1+x1i+y1j+z1k=(w1,v1)Q2=w2+x2i+y2j+z2k=(w2,v2)Q1*Q2=(w1*w2-<v1,v2>,w1*v2+w2*v1+v1xv2)(w1+x1i+y1j+z1k)*(w2+x2i+y2j+z2k)=w1*w2-x1*x2-y1*y2-z1*z2+(W1*x2+x1*w2+y1*z2-z1-y2)i+(y1*w2+w1*y2+z1*x2-x1*z2)j+(w1*z2+z1*w2+x1*y2-y1*x2)k對于其中的軸部分，假如v1//v2,則有v1xv2=0(平行向量的叉乘結(jié)果為0)四元組的點乘，點乘積為數(shù)值：Q1.*Q2=w1*w2+<v1,v2>=w1*w2+x1*x2+y1*y2+z1*z2;數(shù)乘s為一實數(shù)，q為四元組，則有sq=qs共軛p=(w,v)，則p*=(w,-v)(pq)*=q*p*N(q)=w2+x2+y2+z2

q-i=q*/N(q)q-i=q*/N(q)顯然可得qq-i=(1,0)假如有一表示向量的四元組q=(w,v),對其應(yīng)用旋轉(zhuǎn)量p后的結(jié)果為：q’=pqp-i=(w,v’)從上可以看出，計算的結(jié)果q’的實部和q的實部是相等的，并且有N(v)=N(v’)如果N(q)=1,則可以令q=(cosa,usina),u也為一個單位向量，則q’是q繞u旋轉(zhuǎn)2a個弧度的結(jié)果假如S(q)表示q的實部，則有2S(q)=q+q*2S(pqp-i)=pqp-i+(pqp-i)*=pqp*+(pqp*)*=pqp*+pq*p*=p(q+q*)p*=2S(q)(這里由于p是單位四元數(shù)，所以有p-1等于p*)歐拉角到四元數(shù)的轉(zhuǎn)換定義pitch,yaw,roll分別為繞X軸、Y軸、Z軸的旋轉(zhuǎn)弧度floatp=pitch*PIOVER180/2.0;floaty=yaw*PIOVER180/2.0;floatr=roll*PIOVER180/2.0;floatsinp=sin(p);floatsiny=sin(y);floatsinr=sin(r);floatcosp=cos(p);floatcosy=cos(y);floatcosr=cos(r);this->x=sinr*cosp*cosy-cosr*sinp*siny;this->y=cosr*sinp*cosy+sinr*cosp*siny;this->z=cosr*cosp*siny-sinr*sinp*cosy;this->w=cosr*cosp*cosy+sinr*sinp*siny;normalise();三.使用matlab進行相關(guān)計算計算兩個向量V1和v2之間的旋轉(zhuǎn)量四元數(shù)p,使得v1應(yīng)用p后到達v2假如v1轉(zhuǎn)到v2的旋轉(zhuǎn)軸為v,旋轉(zhuǎn)角為theta，則q=[v*cos(theta/2)sin(theta/2)]Matlab代碼：functionq=vector2q(v1,v2)%..normalize....len1=sqrt(v1*v1');len2=sqrt(v2火v2');v1=v1/len1;v2=v2/len2;angle=v1*v2';axis=cross(v1,v2);alen=sqrt(axis*axis');axis=axis/alen;t=acos(angle);t=t/2;q(1)=axis(1)*sin(t);q(2)=axis(2)*sin(t);q(3)=axis(3)*sin(t);q(4)=cos(t);end計算出了q之后，可以獲得對應(yīng)的旋轉(zhuǎn)矩陣，旋轉(zhuǎn)矩陣的計算Matlab里面的矩陣是以列為主順序的functionr=q2rot(q)w=q(4);x=q(1);y=q(2);z=q(3);r=zeros(3,3);r(1,1)=1-2火y*y-2火z*z;r(1,2)=2火x*y+2火w*z;r(1,3)=2*x*z-2*w*y;r(2,1)=2火x*y-2火w*z;r(2,2)=1-2*x*x-2火z*z;r(2,3)=2火z*y+2火w*x;r(3,1)=2火x*z+2火w*y;r(3,2)=2火y*z-2火w*x;r(3,3)=1-2*x*x-2*y*y;r=r'；end同時，也可以根據(jù)四元數(shù)來計算歐拉角functionR=q2euler(q)w=q(4);x=q(1);y=q(2);z=q(3);t11=2*(w*x+y*z);t12=1-2*(x*x+y*y);R(1)=atan2(t11,t12);t2=2*(w*y-z*x);R(2)=asin(t2);t31=2*(w*z+x*y);t32=1-2*(y*y+z*z);R(3)=atan2(t31,t32);end計算出來的歐拉角rx，ry，rz，分別為繞X軸、Y軸和Z軸的旋轉(zhuǎn)角，假如有：Rotq=q2rot(q)R=q2euler(q)[rotxrotyrotz]=Rotation(R)可以發(fā)現(xiàn)Rotq==rotz*roty*rotx從這里可以看出，上面使用四元數(shù)這樣計算出來的旋轉(zhuǎn)矩陣的旋轉(zhuǎn)順序分別是X軸、Y軸和Z軸的ra=pi/4;qz=[00-sin(ra)cos(ra)]%繞z旋轉(zhuǎn)-90度qy=[0sin(ra)0cos(ra)]%繞y旋轉(zhuǎn)90度qyz=qmult(qy,qz)r=q2euler(qyz)上面的r得出的結(jié)果為r=-1.57080.0000-1.5708也就是說其幾何意義變成先繞X軸旋轉(zhuǎn)-90度，再繞Z軸旋轉(zhuǎn)-90度，而根據(jù)qy和qz的相乘我們實際進行的操作卻是先繞Z軸旋轉(zhuǎn)-90度，再繞Y軸旋轉(zhuǎn)90度，但是結(jié)果卻是這兩種操作等價，這說明由四元數(shù)到歐拉角可以有多個解兩個四元數(shù)，假如它們的方向是相反的，用它們作用于向量得到的新向量的值仍然相等q1=[0.024666-0.0239540.5047270.862594];arm=[-8.8817196.037597-2.36776];q2=-q1;rot1=q2rot(q1);rot2=q2rot(q2);v1=rot1*arm'v2=rot2火arm'上面計算出來的v1等于v2四元數(shù)的余弦值為它們的內(nèi)積假如余弦值小于0，則需要將其中的一個取反，因為上面我們知道一個四元數(shù)和它的反方向的四元數(shù)對一個向量起相同的作用四元數(shù)的相乘，代表旋轉(zhuǎn)的累積pq=p*q;rotp=q2rot(p);rotq=q2rot(q);rotpq=q2rot(pq);rotmul=rotp*rotq;這里rotpq與rotmul相等四.OGRE中Quaternion類的幾個函數(shù)四元數(shù)到旋轉(zhuǎn)向量voidQuaternion::ToRotationMatrix(Matrix3&kRot)const1-2*qy2-2*qz22*qx*qy-2*qz*qw2*qx*qz+2*qy*qw2*qx*qy+2*qz*qw1-2*qx2-2*qz22*qy*qz-2*qx*qw2*qx*qz-2*qy*qw2*qy*qz+2*qx*qw1-2*qx2-2*qy2旋轉(zhuǎn)量到四元數(shù)根據(jù)1中的表格，有：4*(1-qx2-qy2-qz2)=1+m00+m11+m22又qw2=1-qx2-qy2-qz2，可得4*qw2=1+m00+mil+m22這里解qw必須保證1+m00+mil+m22>=0,如果不是的話，就構(gòu)造其他的等式來計算，OGRE中分成兩種情況，一種是m00+mil+m22>=0，就可以直接先解出qw，否則的采用另外的等式計算LocalaxisVector3xAixs(void)