/***************************************************************/ /* 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; float mat[16]; float quat[4]; void main(void) { int jj; quat[0] = 51.567898; quat[1] = 1123.1234; quat[2] = 23431.12346; quat[3] = 33314.746438; printf("Given quaternion is: \n"); printf(" %f %f %f %f \n", quat[0], quat[1], quat[2], quat[3]); normalize(); /* Ensures quaternion if of unit length */ for(jj=0;jj<5;jj++){ q2m(); m2q(); } } normalize() { float norm, unit; int ii; norm = quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2] + quat[3]*quat[3]; if (norm == 1) { printf(" The quaternion is of unit length.\n"); } else { printf(" The quaternion is not of unit length.\n"); printf(" The values of the unit quaternion are: \n"); unit = sqrt(norm); for(ii=0;ii<4;ii++) { quat[ii] = quat[ii]/unit; printf(" %f \n", quat[ii]); } } } q2m() { w = quat[0]; x = quat[1]; y = quat[2]; z = quat[3]; 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; 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]); } m2q() { float tr[4], str; int ii, jj; tr[0] = mat[0] + mat[5] + mat[10]; tr[1] = mat[0] - mat[5] - mat[10]; tr[2] = -mat[0] + mat[5] - mat[10]; tr[3] = -mat[0] - mat[5] + mat[10]; ii = 1; jj = 2; if(tr[0] > tr[1]) { ii = 0; } if(tr[3] > tr[2]) { jj = 3; } if(tr[jj] > tr[ii]) { ii = jj; } if(ii == 0) { printf(" %i is the largest trace. \n", ii); str = sqrt(tr[ii]+1)/2; quat[0] = str; quat[1] = 0.25*(mat[6] - mat[9]) / str; quat[2] = 0.25*(mat[8] - mat[2]) / str; quat[3] = 0.25*(mat[1] - mat[4]) / str; } if(ii == 1) { printf(" %i is the largest \n", ii); str = sqrt(tr[ii]+1)/2; quat[0] = 0.25*(mat[6] - mat[9]) / str; quat[1] = str; quat[2] = 0.25*(mat[1] + mat[4]) / str; quat[3] = 0.25*(mat[8] + mat[2]) / str; } if(ii == 2) { printf(" %i is the largest \n", ii); str = sqrt(tr[ii]+1)/2; quat[0] = 0.25*(mat[8] - mat[2]) / str; quat[1] = 0.25*(mat[4] + mat[1]) / str; quat[2] = str; quat[3] = 0.25*(mat[9] + mat[6]) / str; } if(ii == 3) { printf(" %i is the largest \n", ii); str = sqrt(tr[ii]+1)/2; quat[0] = 0.25*(mat[1] - mat[4]) / str; quat[1] = 0.25*(mat[8] + mat[2]) / str; quat[2] = 0.25*(mat[9] + mat[6]) / str; quat[3] = str; } printf(" The quaternions are: \n"); printf(" %f %f %f %f \n", quat[0], quat[1], quat[2], quat[3]); }