/***************************************************************/ /* 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; printf("I have been performed once \n"); quat[0] = 5.0; /* /sqrt(30.0); */ quat[1] = 123.0; /* /sqrt(30.0); */ quat[2] = 31.0; /* /sqrt(30.0); */ quat[3] = 14.0; /* /sqrt(30.0); */ printf(" Checking quaternion to ensure is a unit vector \n"); normalize(); for(jj=0;jj<3;jj++){ printf("Loading quat2mat \n"); quat2mat(); mat2quat(); } } 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"); unit = sqrt(norm); for(ii=0;ii<4;ii++) { quat[ii] = quat[ii]/unit; } } } quat2mat() { 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]); } mat2quat() { float tr[4], str; int max, dir; 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]; if(tr[0] > tr[1]) { if(tr[0] > tr[2]) { if(tr[0] > tr[3]) { ii = 0; printf(" %i is the largest \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; } else { 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; } } else { if(tr[2] > tr[3]) { 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; } else { 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; } } } else { if(tr[1] > tr[2]) { if(tr[1] > tr[3]) { 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; } else { ii = 3; 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; } } else { if(tr[2] > tr[3]) { 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; } else { 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]); }