/* gkf condensation */ /***************************************************************/ /* Given a rotation matrix and convert it into a quaternion */ /* Notation: Quaternion will be expressed as: */ /* */ /***************************************************************/ #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] ={51.567898, 1123.1234, 23431.12346, 33314.746438 }; 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 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; printf(" The coordinates 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 entries 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]); } 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] }; ii=1; jj=1; /* 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; if(ii == 0) { printf(" %i is the largest trace. \n", ii); tmp = sqrt(tr[ii]+1)/2; quat[0] = tmp; quat[1] = 0.25*(mat[6] - mat[9]) / tmp; quat[2] = 0.25*(mat[8] - mat[2]) / tmp; quat[3] = 0.25*(mat[1] - mat[4]) / tmp; } if(ii == 1) { printf(" %i is the largest \n", ii); tmp = sqrt(tr[ii]+1)/2; quat[0] = 0.25*(mat[6] - mat[9]) / tmp; quat[1] = tmp; quat[2] = 0.25*(mat[1] + mat[4]) / tmp; quat[3] = 0.25*(mat[8] + mat[2]) / tmp; } if(ii == 2) { printf(" %i is the largest \n", ii); tmp = sqrt(tr[ii]+1)/2; quat[0] = 0.25*(mat[8] - mat[2]) / tmp; quat[1] = 0.25*(mat[4] + mat[1]) / tmp; quat[2] = tmp; quat[3] = 0.25*(mat[9] + mat[6]) / tmp; } if(ii == 3) { printf(" %i is the largest \n", ii); tmp = sqrt(tr[ii]+1)/2; quat[0] = 0.25*(mat[1] - mat[4]) / tmp; quat[1] = 0.25*(mat[8] + mat[2]) / tmp; quat[2] = 0.25*(mat[9] + mat[6]) / tmp; quat[3] = tmp; } printf(" The quaternions are: \n"); printf(" %f %f %f %f \n", quat[0], quat[1], quat[2], quat[3]); } void main(void){ int jj; printf("Given quaternion is: \n"); printf(" %f %f %f %f \n", quat[0], quat[1], quat[2], quat[3]); normalize(quat); /* Ensures quaternion if of unit length */ for(jj=0;jj<5;jj++){ q2m(quat,mat); m2q(mat,quat); } }