void Quaternion_normalize(Quaternion *quaternion) { int32 magnitude; magnitude = sqrtf32(mulf32(quaternion->x, quaternion->x) + mulf32(quaternion->y, quaternion->y) + mulf32(quaternion->z, quaternion->z) + mulf32(quaternion->w, quaternion->w)); quaternion->x = divf32(quaternion->x, magnitude); quaternion->y = divf32(quaternion->y, magnitude); quaternion->z = divf32(quaternion->z, magnitude); quaternion->w = divf32(quaternion->w, magnitude); }
void RubiksCube::Grab(VECTOR touchVector) { tvf32[0]+=floattof32(touchVector.X); tvf32[1]+=floattof32(touchVector.Y); tvf32[2]=0; if(f32toint(sqrtf32(mulf32(tvf32[0],tvf32[0])+mulf32(tvf32[1],tvf32[1])+mulf32(tvf32[2],tvf32[2])))>5) { VECTOR upv, rightv, rotduv, rotdrv; int32 uvf32[3];//up vector as f32 int32 rvf32[3];//right vector int32 magup, magright; m4x4 grabMatrix;//container for the Position Matrix vectorFromSide(upv, rightv, clicked[0]); //printf ("Initial Vector:\n %f, ", tmpv.X); //printf("%f, ", tmpv.Y); //printf("%f\n", tmpv.Z); glGetFixed(GL_GET_MATRIX_POSITION, (int32*)&grabMatrix); glMatrixMode(GL_MODELVIEW); //rotate the up vector thru the projection matrix //and cast it to f32 RotateVector(grabMatrix, upv, rotduv); uvf32[0]=floattof32(rotduv.X); uvf32[1]=floattof32(rotduv.Y); uvf32[2]=floattof32(rotduv.Z); //rinse and repeat with the right vector RotateVector(grabMatrix, rightv, rotdrv); rvf32[0]=floattof32(rotdrv.X); rvf32[1]=floattof32(rotdrv.Y); rvf32[2]=floattof32(rotdrv.Z); if(controlStyle) { int32 suvf32[3]; int32 srvf32[3]; suvf32[0]=0; suvf32[1]=inttof32(1); suvf32[2]=0; srvf32[0]=inttof32(1); srvf32[1]=0; srvf32[2]=0; magup=dotf32(uvf32, suvf32); magright=dotf32(uvf32, srvf32); if(abs(magup)>abs(magright)) { for(int i=0; i<3; i++) { if(magup>0) { rvf32[i]=srvf32[i]; uvf32[i]=suvf32[i]; } else { rvf32[i]=-srvf32[i]; uvf32[i]=-suvf32[i]; } } } else { for(int i=0; i<3; i++) { if(magright>0) { uvf32[i]=srvf32[i]; rvf32[i]=-suvf32[i]; } else { uvf32[i]=-srvf32[i]; rvf32[i]=suvf32[i]; } } } } magup=dotf32(uvf32, tvf32); magright=dotf32(rvf32, tvf32); if(magup || magright) { int32 tmp[2]; if(abs(magup)>abs(magright)) { tmp[0]=uvf32[0]; tmp[1]=uvf32[1]; unitVector((int32*)tmp); InitTwist(true, tmp); }else{ tmp[0]=rvf32[0]; tmp[1]=rvf32[1]; unitVector((int32*)tmp); InitTwist(false, tmp); } Twisting=true; Grabbing=false; tvf32[0]=0; tvf32[1]=0; } } }
void unitVector (int32* vector) { int32 magnitude = sqrtf32(mulf32(vector[0], vector[0]) + mulf32(vector[1], vector[1])); vector[0]=divf32(vector[0], magnitude); vector[1]=divf32(vector[1], magnitude); }