//at the top int sizz; //from siz, and caveyes in CAVEskels enum {console, cave, cube} eyes; int gesture; float eps; float del //in deFault gesture=0; eps=0.002; //hand rotation gesture attenuation del=0.02; //hand translation gesture attenuation eyes=cube; //because the getoptery doesn't work yet resethand=0; //gesturetrack gkf: 2jun04 /***************************************************************/ /* Given a rotation matrix convert it into a quaternion */ /* Given a quaternion convert it into a rotation matrix */ /* By Robert Shuttleworth, illiMath2001, July 2001 */ /* */ /***************************************************************/ //gkf float mat[16]={1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1}; //gkf 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){ /* quaternion yields a rotation */ 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){ /* isometry yields a quaternion */ 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 loadIdentity(float* mat) {int ii; for(ii=0;ii<16;ii++) mat[ii]=float(ii/4 == ii%4);} void getmat(float *target, float *source){ memcpy(target,source, 16*sizeof(float)); } void multmatinv(float *T){ // of an Euclidean isometry float N[16]; getmat(N,T); N[1] = T[4]; N[2] = T[8]; N[6] = T[9]; N[4] = T[1]; N[8] = T[2]; N[9] = T[6]; N[12]= -T[0]*T[12]-T[1]*T[13]-T[2]*T[14]; N[13]= -T[4]*T[12]-T[5]*T[13]-T[6]*T[14]; N[14]= -T[8]*T[12]-T[9]*T[13]-T[10]*T[14]; glMultMatrixf(N); } // uses eps to shrink rotation and del to shring translation void updategeomat(float *oldhand, float *newhand){ float dx=0, dy=0, dz=0 ; float dhand[16]; float quat[4]; glMatrixMode(GL_TEXTURE); //do some arithmetic on an unused stack glPushMatrix(); glLoadIdentity(); multmatinv(newhand); glMultMatrixf(oldhand); glGetFloatv(GL_TEXTURE_MATRIX, dhand); glPopMatrix(); //dhand= newhand^-1 * oldhand dx=del*dhand[12]; dhand[12]=0; //attenuation is about right dy=del*dhand[13]; dhand[13]=0; dz=del*dhand[14]; dhand[14]=0; //use a bit of the translation in dhand and then make it the rotation if(gesture){ //shrink dhand m2q(dhand,quat); quat[0]= 1-eps + eps*quat[0]; quat[1] *= eps; quat[2] *= eps; quat[3] *= eps; normalize(quat); q2m(quat,dhand); } glMatrixMode(GL_TEXTURE); glPushMatrix(); glLoadIdentity(); glMultMatrixf(dhand); glTranslatef(dx,dy,dz); glMultMatrixf(affmat); glGetFloatv(GL_TEXTURE_MATRIX,affmat); } void readbuttons(arMasterSlaveFramework& fw){ const int butn[6]= { fw.getButton(0), fw.getButton(1), fw.getButton(2), fw.getButton(3), fw.getButton(4), fw.getButton(5) }; static int obutn[6]={0,0,0,0,0,0}; if(obutn[3]==0 & butn[3] == 1) morph=1-morph; //{X}button if(obutn[2]==0 & butn[2] == 1) gesture=1-gesture; //{C}button if(obutn[1]==0 & butn[1] == 1) resethand=1; //{B}button if(obutn[4]==0 & butn[4] == 1) ; if(obutn[5]==0 & butn[5] == 1) deFault(); //{Z}button if(morph)autotymer(0); memcpy(obutn,butn,6*sizeof(int)); } void gesturetrack(arMasterSlaveFramework& fw){ //the present logic is legacy stuff from zyspace, rationalize it! if (!fw.getMaster())return; //only the master should listen //this is the gesture navigator static float oldhand[16]; float handmat[16]; arMatrix4 arhand=fw.getMatrix(1); getmat(handmat,arhand.v); //geotrack(arhand.v) gets arhand.v and used as float* handmat static bool reshand=true; if(resethand){reshand=true; resethand=false;} //do we really need two flags? if(gesture){ updategeomat(oldhand,handmat); } if(reshand){reshand=false; getmat(oldhand, handmat);} //this is the Pape navigator //this should really be done on the arithmetic (Texture) stack glMatrixMode(GL_MODELVIEW); glPushMatrix(); glLoadIdentity(); const float joy0 = fw.getAxis(0); //joystick right-left const float joy1 = fw.getAxis(1); //joystick fore-aft const float xx = arhand.v[8]; const float yy = arhand.v[9]; const float zz = arhand.v[10]; // printf(" xx %g yy %g zz %g \n",xx,yy,zz); const float da = .5*joy0; const float ds = .1*joy1; //joystick translates in the wand direction glTranslatef(xx*ds, yy*ds, zz*ds); glRotatef(da, 0.,1.,0.); glMultMatrixf(affmat); glGetFloatv(GL_MODELVIEW_MATRIX,affmat); glPopMatrix(); //this is an illogical place for this but maybe it works again readbuttons(fw); } /**********************************************************************/