/***************************************************************/ /* Given a rotation matrix convert it into a quaternion */ /* Given a quaternion convert it into a rotation matrix */ /* By Robert Shuttleworth, illiMath2001, July 2001 */ /* */ /***************************************************************/ #include #include #include /* put this into the source code */ #pragma warning (disable:4305) /* double-to-float */ #pragma warning (disable:4101) /* unreferenced local variable */ #pragma warning (disable:4056) /* overflow in fp constant arithmetic */ #pragma warning (disable:4244) /* conversion from double to float */ float mat[16]={1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1}; float quat[4] ={1,0,0,0}; void normalize(float *vec){ int ii; float temp = vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2] + vec[3]*vec[3]; if (temp == 1){ printf(" The vector is of unit length.\n"); } else{ temp = sqrt(temp); for(ii=0;ii<4;ii++)vec[ii] /= temp; } } void printquat(float *quat){ printf("\n Quaternion: \n"); printf(" w = %f ", quat[0]); printf(" x = %f ", quat[1]); printf(" y = %f ", quat[2]); printf(" z = %f ", quat[3]); } void printmat(float *mat){ printf("\n Matrix: \n"); printf(" %f %f %f %f \n", mat[0], mat[4], mat[8], mat[12]); printf(" %f %f %f %f \n", mat[1], mat[5], mat[9], mat[13]); printf(" %f %f %f %f \n", mat[2], mat[6], mat[10], mat[14]); printf(" %f %f %f %f \n", mat[3], mat[7], mat[11], mat[15]); } void q2m(float *quat, float *mat){ float w = quat[0], x = quat[1], y = quat[2], z = quat[3]; float ww, xx, yy, zz, xy, xz, yz, wz, wy, wx; ww = w*w; xx = x*x; yy = y*y; zz = z*z; xy = x*y; xz = x*z; yz = y*z; wz = w*z; wy = w*y; wx = w*x; xz = x*z; mat[0]=ww+xx-yy-zz; mat[4]=2*xy - 2*wz; mat[8]=2*wy + 2*xz; mat[1]=2*wz + 2*xy; mat[5]=ww-xx+yy-zz; mat[9]=2*yz - 2*wx; mat[2]=2*xz - 2*wy; mat[6]=2*wx + 2*yz; mat[10]=ww-xx-yy+zz; mat[3] = mat[7] = mat[11] = mat[12] = mat[13] = mat[14] = 0; mat[15] = ww + xx + yy + zz; /* this is a check */ } void m2q(float *mat, float *quat){ /* the inverse operation */ float tmp; int ii, jj; float tr[4] ={ /* pseudo traces */ mat[0] + mat[5] + mat[10], mat[0] - mat[5] - mat[10], -mat[0] + mat[5] - mat[10], -mat[0] - mat[5] + mat[10]}; printf("trace %f %f %f %f \n" , tr[0],tr[1],tr[2],tr[3]); ii=1; jj=2; /* find the largest tr */ if(tr[0] > tr[1]) ii = 0; if(tr[3] > tr[2]) jj = 3; if(tr[jj] > tr[ii]) ii = jj; printf("\n %i is the largest trace. \n", ii); tmp = 4*sqrt(tr[ii]+1)/2; switch(ii){ case 0: quat[0] = tmp/4; quat[1] = (mat[6] - mat[9]) / tmp; quat[2] = (mat[8] - mat[2]) / tmp; quat[3] = (mat[1] - mat[4]) / tmp; break; case 1: quat[0] = (mat[6] - mat[9]) / tmp; quat[1] = tmp/4; quat[2] = (mat[1] + mat[4]) / tmp; quat[3] = (mat[8] + mat[2]) / tmp; break; case 2: quat[0] = (mat[8] - mat[2]) / tmp; quat[1] = (mat[4] + mat[1]) / tmp; quat[2] = tmp/4; quat[3] = (mat[9] + mat[6]) / tmp; break; case 3: quat[0] = (mat[1] - mat[4]) / tmp; quat[1] = (mat[8] + mat[2]) / tmp; quat[2] = (mat[9] + mat[6]) / tmp; quat[3] = tmp/4; break; } } /**********************************************************************/ void arguments(int argc,char **argv){ /* Pat Hanrahan 1989 */ while(--argc){++argv; if(argv[0][0]=='-')switch(argv[0][1]){ case 'q': quat[0] =atof(argv[1]); argv++; argc--; quat[1] =atof(argv[1]); argv++; argc--; quat[2] =atof(argv[1]); argv++; argc--; quat[3] =atof(argv[1]); argv++; argc--; break; case 'm': mat[0]=atof(argv[1]); argv++; argc--; break; }}} /**********************************************************************/ int main(int argc, char **argv){ int jj; quat[0] = 51.567898; quat[1] = 1123.1234; quat[2] = 23431.12346; quat[3] = 33314.746438; arguments(argc,argv); printquat(quat); normalize(quat); /* Ensures quaternion if of unit length */ printquat(quat); printmat(mat); q2m(quat,mat); printmat(mat); m2q(mat,quat); printquat(quat); q2m(quat,mat); printmat(mat); }