/***************************************************************/ /* Given a rotation matrix and convert it into a quaternion */ /* Notation: Quaternion will be expressed as: */ /* */ /***************************************************************/ #include #include #include float w,x,y,z; float ww, xx, yy, zz, xy, xz, yz, wz, wy, wx; void main(void) { float mat[16]; float quat[4]; quat[0] = 1/sqrt(30.0); quat[1] = 2/sqrt(30.0); quat[2] = 3/sqrt(30.0); quat[3] = 4/sqrt(30.0); #if 0 quat2mat(float *quat, float *mat) { #endif #if 0 float quat[4]; float mat[16]; #endif w = quat[3]; x = quat[0]; y = quat[1]; z = quat[2]; printf(" The values of the quaternion are: \n"); printf(" w = %f \n", w); printf(" x = %f \n", x); printf(" y = %f \n", y); printf(" z = %f \n", z); 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[1] = 2*wz + 2*xy; mat[2] = 2*xz - 2*wy; mat[3] = mat[7] = mat[11] = mat[12] = mat[13] = mat[14] = 0; mat[4] = 2*xy - 2*wz; mat[5] = ww - xx + yy - zz; mat[6] = 2*wx + 2*yz; mat[8] = 2*wy + 2*xz; mat[9] = 2*yz - 2*wx; mat[10] = ww - xx - yy + zz; mat[15] = ww + xx + yy + zz; #if 0 #define HEAD sprintf(phrase," % .4f % .4f % .4f % .4f ", #define TAIL ); cmov2(horz,vert); charstr(phrase); LF; HEAD mat[0],mat[4],mat[8] ,mat[12] TAIL HEAD mat[1],mat[5],mat[9] ,mat[13] TAIL HEAD mat[2],mat[6],mat[10],mat[14] TAIL HEAD mat[3],mat[7],mat[11],mat[15] TAIL #endif printf(" The values of the matrix are: \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]); }