飛行器-四軸關于四元數(shù)

qcos(Q/2sin(Q2)vv是旋轉軸矢量，Q歐拉角轉四元數(shù) =[cos(y/2),(sin(y/2),0, =[cos(q/2),(0,sin(q/2), =[cos(f/2),(0,0,sin(f/2)]|1-2y2- 2yz+ 2xz-2wyRm=|2xy-1-2x2-2yz-2wx|2xz+2yz-1-2x2-MatToQuat(floatm[4][4],QUAT*{floattr,s, i,j,intnxt[3]={1,2,tr=m[0][0]+m[1][1]+//checkthediagonalif(tr>0.0){s=sqrt(tr+1.0);quat->w=s/2.0;s=0.5/s;quat->x=(m[1][2]-m[2][1])*quat->y=(m[2][0]-m[0][2])*quat->z=(m[0][1]-m[1][0])*}{//diagonalisnegativei=0;if(m[1][1]>m[0][0])i=if(m[2][2]>m[i][i])i=j=k=s=sqrt((m[i][i]-(m[j][j]+m[k][k]))+q[i]=s*if(s!=0.0)s=0.5/q[3]=(m[j][k]-m[k][j])*q[j]=(m[i][j]+m[j][i])*q[k]=(m[i][k]+m[k][i])*quat->x=quat->y=quat->z=quat->w=}}QuatToMatrix(QUAT*quat,floatfloatwx,wy,wz,xx,yy,yz,xy,xz,zz,x2,y2,//calculatecoefficientsx2=quat->x+quat->x;y2=quat->y+quat->y;z2=quat->z+quat->z;xx=quat->x*x2;xy=quat->x*y2;xz=quat->x*z2;yy=quat->y*y2;yz=quat->y*z2;zz=quat->z*z2;wx=quat->w*x2;wy=quat->w*y2;wz=quat->w*m[0][0]=1.0-(yy+m[1][0]=xy-m[2][0]=xz+m[3][0]=m[0][1]=xy+wz;m[1][1]=1.0-(xx+zz);m[2][1]=yz-m[3][1]=m[0][2]=xz-m[1][2]=yz+wx;m[2][2]=1.0-(xx+yy);m[3][2]=0.0;m[0][3]=m[1][3]=m[2][3]=m[3][3]=}EulerToQuat(floatroll,floatpitch,floatyaw,QUAT*{floatcr,cp,cy,sr,sp,sy,cpcy,//calculatetrigidentitiescr=cos(roll/2);cp=cos(pitch/2);cy=cos(yaw/2);sr=sin(roll/2);sy=sin(yaw/2);cpcy=cp*cy;spsy=sp*quat->w=cr*cpcy+sr*spsy;quat->x=sr*cpcy-cr*spsy;quat->y=cr*sp*cy+sr*cp*sy;quat->z=cr*cp*sy-sr*sp*cy;}QuatMul(QUAT*q1,QUAT*q2,QUATfloatA,B,C,D,E,F,G,A=(q1->w+q1->x)*(q2->w+q2-B=(q1->z-q1->y)*(q2->y-q2-C=(q1->w-q1->x)*(q2->y+q2-D=(q1->y+q1->z)*(q2->w-q2-E=(q1->x+q1->z)*(q2->x+q2-F=(q1->x-q1->z)*(q2->x-q2-G=(q1->w+q1->y)*(q2->w-q2-H=(q1->w-q1->y)*(q2->w+q2-res->w=B+(-E-F+G+H)/2;res->x=A-(E+F+G+H)/2;res->y=C+(E-F+G-H)/2;res->z=D+(E-F-G+H)/2;}QuatSlerp(QUAT*from,QUAT*to,floatt,QUAT*{ omega,cosom,sinom,scale0,//calccosom=from->x*to->x+from->y*to->y+from->z*to-+from->w*to-//adjustsigns(ifif(cosom<0.0){cosom=-cosom;to1[0]=-to->x;to1[1]=-to->y;to1[2]=-to-to1[3]=-to-}elseto1[0]=to-to1[1]=to-to1[2]=to-to1[3]=to-}//calculateif((1.0-cosom)>DELTA)//standardcase(slerp)omega=acos(cosom);sinom=sin(omega);scale0=sin((1.0-t)*omega)/sinom;scale1=sin(t*omega)/sinom;}else//"from"and"to"quaternionsarevery//...sowecandoalinearinterpolationscale0=1.0-t;scale1=}//calculatefinalres->x=scale0*from->x+scale1*