inputdevice.cpp

FreeWRLduneInputDevice和FreeWRL一起可以让用户用带有6DoF的输入设备检索3D VRML/X3D数据。它基于FreeWRL的"/tmp/inpdev"扩展传感器输入接口和w

      fprintf(stderr,"failed to initialize xinput device %s\n",device);
      free(swxinput);
      swxinput=NULL;
      return;
      }

   number_buttons=swxinput->number_buttons;

   if (number_buttons>0)
      { 
      button=new bool[number_buttons];
      for (i=0;i<number_buttons;i++)
         button[i]=false;
      }

   number_axes=swxinput->number_axes;

   if (number_axes>0)
      { 
      max_value=new float[number_buttons];
      for (i=0;i<number_buttons;i++)
         max_value[i]=swxinput->maxvalue[i];
      }

   name="Unknown";
  
#  ifdef DEBUG
   fprintf(stderr,"Xinputdevice %s has %d axes and %d buttons.\n",
          device, number_axes, number_buttons);  
#  endif
   }
 
xinput::~xinput(void)
   {
   swCloseXinputDevice(swxinput);
   free(swxinput);
   }

bool xinput::readInputDevice(void)
   {
   static int lastbutton=-1;
   button_pressed=-1;
   button_released=-1;
   int buttons;

   if (swxinput==NULL)
      return false;
   for (int i=0;i<number_buttons;i++)
      button[i]=false;
    
   swQueryXinputDevice(swxinput);
   if (swxinput->xinput==NULL)
      return false;
   value[0]= factor*float(swxinput->axes[num_axis_x])/
                    swxinput->maxvalue[num_axis_x];
   value[1]= factor*float(swxinput->axes[num_axis_y])/
                    swxinput->maxvalue[num_axis_y];
   value[2]=-factor*float(swxinput->axes[num_axis_z])/
                   swxinput->maxvalue[num_axis_z];

   value[3]=rotfactor*float(swxinput->axes[num_axis_xrot])/
                      swxinput->maxvalue[num_axis_xrot];
   value[4]=rotfactor*float(swxinput->axes[num_axis_yrot])/
                      swxinput->maxvalue[num_axis_yrot];
   value[5]=rotfactor*float(swxinput->axes[num_axis_zrot])/
                      swxinput->maxvalue[num_axis_zrot];
   value[6]=rotfactor*float(swxinput->axes[num_axis_angle])/
                      swxinput->maxvalue[num_axis_angle];

   return true;
   }
 
#endif

// Driver for SpaceTec/LabTec Spaceball and clones.

#ifdef HAVE_LIBSBALL

/*
 * initialise spaceball 
 * argument "device" is the spaceballdevice, eg. "/dev/ttyd2" or "/dev/ttyS0" 
 */
 
spaceball::spaceball(char *device) : InputDevice()
   {
   sball=NULL;
   if ((sball = sball_open(device)) == NULL)
      {
      fprintf(stderr,"spaceballdevice %s ",device);
      fprintf(stderr,"initialisation failed\n");
      return;
      }
 
   sball_init(sball);
   nullsize=0;
   max_value=32767.0;
   factor=1.0;
   rotfactor=1.0;

   sball_set_nullregion(sball, nullsize, nullsize, nullsize, 
                                nullsize, nullsize, nullsize);

   number_axes = 6;
   number_buttons = 9;
   name="Unknown Spaceball";

   if (number_buttons>0)
      { 
      button=new bool[number_buttons];
      for (int i=0;i<number_buttons;i++)
         button[i]=false;
      }
 
# ifdef DEBUG
   fprintf(stderr,"InputDevice (%s) has %d axes and %d buttons. Driver ???\n",
           name, number_axes, number_buttons);  
# endif
   }
 
spaceball::~spaceball(void)
   {
   sball_close(sball);
   }

bool spaceball::readInputDevice(void)
   {
   bool ret=false;
   static int lastbutton=-1;
   button_pressed=-1;
   button_released=-1;
   int buttons;
   int sx,sy,sz;
   int sxrot,syrot,szrot;
 
   if (sball==NULL)
      return false;
   for (int i=0;i<number_buttons;i++)
      button[i]=false;
   if (sball_getstatus(sball, &sx, &sy, &sz, &sxrot, &syrot, &szrot, &buttons))
      {
      value[0]= ((float)sx)/maxvalue(0)*factor;
      value[1]= ((float)sy)/maxvalue(1)*factor;
      value[2]=-((float)sz)/maxvalue(2)*factor;
      value[3]= ((float)sxrot)/maxvalue(3)*rotfactor;
      value[4]= ((float)syrot)/maxvalue(4)*rotfactor;
      value[5]= ((float)szrot)/maxvalue(5)*rotfactor;
      ret=true;
      }
   return ret;
   }
 
# ifdef TEST_SPACEBALL
int main(int argc,char** argv)
   {
   spaceball device=spaceball("/dev/ttyd2");
   while(1)
      if (device.readInputDevice())
          printf("%f %f %f\n",device.get_x(),device.get_y(),device.get_z());
   }
# endif
#endif


// Driver for Ascention Flock of birds devices

#ifdef HAVE_AFLOCK
#include <unistd.h>

/*
 * initialise Aflock 
 */

AflockDevice::AflockDevice(char* dev)
   {
   /* insert your default configuration here */
   device = dev;
   baudrate = 38400;     // baudrate of device
   sync = 0;          
   block = 0;         
   numBrds = 2;          // number of birds (without transmitter)
   transmit = 1;         // number of bird transmitter unit
   hemi = RIGHT_HEM;     // hemisphere (sit on the antenna block to see, 
                         //             what is left or right 8-)
   filt = FILTER_DEFAULT;// use default filter set by flock autoconfig
   sudden_change_lock=true;
   report = 'Q';
   calfile = "";         // calibration file
   /* end of configuratable data */

   opened=false;
   flock=NULL;
   } 

AflockDevice::~AflockDevice(void)
   {
   if (flock!=NULL)
      if (opened)
         {
         flock->stop();
         opened=false;
         delete flock;
         flock=NULL;
         }
   }

void AflockDevice::setBaud(char* baudrate_string)
   {
   if (!string2int(baudrate,baudrate_string))
      fprintf(stderr,"argument %s is not a integer, ignored\n",baudrate_string);
   }

void AflockDevice::setSync(char* sync_string)
   {
   if (!string2int(sync,sync_string))
      fprintf(stderr,"argument %s is not a integer, ignored\n",sync_string);
   }

void AflockDevice::setBlock(char* block_string)
   {
   if (!string2int(block,block_string))
      fprintf(stderr,"argument %s is not a integer, ignored\n",block_string);
   }

void AflockDevice::setNumBrds(char* numBrds_string)
   {
   if (!string2int(numBrds,numBrds_string))
      fprintf(stderr,"argument %s is not a integer, ignored\n",numBrds_string);
   }

void AflockDevice::setTransmit(char* transmit_string)
   {
   if (!string2int(transmit,transmit_string))
      fprintf(stderr,"argument %s is not a integer, ignored\n",transmit_string);
   }

void AflockDevice::setHemi(char* hemi_string)
   {
   if (strcmp(hemi_string,"FRONT_HEM")==0)
      hemi=FRONT_HEM;
   else if (strcmp(hemi_string,"AFT_HEM")==0)
      hemi=AFT_HEM;
   else if (strcmp(hemi_string,"UPPER_HEM")==0)
      hemi=UPPER_HEM;
   else if (strcmp(hemi_string,"LOWER_HEM")==0)
      hemi=LOWER_HEM;
   else if (strcmp(hemi_string,"LEFT_HEM")==0)
      hemi=LEFT_HEM;
   else if (strcmp(hemi_string,"RIGHT_HEM")==0)
      hemi=RIGHT_HEM;
   else
      {
      fprintf(stderr,"argument %s is no of ",hemi_string);
      fprintf(stderr,"FRONT_HEM, AFT_HEM, UPPER_HEM, LOWER_HEM,");
      fprintf(stderr," LEFT_HEM, RIGHT_HEM, ignored\n");
      }
   }

void AflockDevice::setFilt(char* filt_string)
   {
   if (strcmp(filt_string,"AC_NARROW")==0)
      filt=filt | AC_NARROW;
   else if (strcmp(filt_string,"AC_WIDE")==0)
      filt=filt | AC_WIDE;
   else if (strcmp(filt_string,"DC_FILTER")==0)
      filt=filt | DC_FILTER;
   else 
      {
      fprintf(stderr,"argument %s is no of ",filt_string);
      fprintf(stderr,"AC_NARROW, AC_WIDE, DC_FILTER, ignored\n");
      }  
   }

void AflockDevice::setSuddenChangeLock(char* sudden_string)
   {
   int sudden=0;
   if (!string2int(sudden,sudden_string))
      fprintf(stderr,"argument %s is not a integer, ignored\n",sudden_string);
   if (sudden==0)
      sudden_change_lock=false;
   else
      sudden_change_lock=true;
   }

void AflockDevice::setReport(char* report_string)
   {
   report=report_string[0];
   }

void AflockDevice::setCalfile(char* calfile_string)
   {
   calfile=calfile_string;
   }


/* set device and initialise flock of birds */

Aflock* AflockDevice::getAflock(void)
   {
   if (!opened)
      {
      flock=NULL;
      flock = new Aflock(device,baudrate,sync,block,
                         numBrds,transmit,hemi,filt,sudden_change_lock,
                         report,calfile);
      if (!flock->start())
         {
         flock=NULL;
         fprintf(stderr,"Aflock %s ",device);
         fprintf(stderr,"initialisation failed\n");
         return flock;
         }
      sleep(1);
      opened=true;
      }
   return flock;      
   }
 
aflock::aflock(AflockDevice* device,char* receiver_string,bool headflag) 
              : InputDevice()
   {
   head=false;
   wand=false;
   headNavigation=false;

   if (headflag)
      head=true;
   else
      wand=true;

   if (!string2int(receiver,receiver_string))
      {
      fprintf(stderr,"argument %s is not a integer",receiver_string);
      flock=NULL;
      return;
      }

   if (device==NULL)
      {
      fprintf(stderr,"internal error, initialition failed");
      flock=NULL;
      return;
      }

   flock=device->getAflock();

   if (receiver==flock->getTransmitter())
      flock->setMasterUseReceiver();      


   number_axes = 6;
   number_buttons = 3;
   name="Ascention flock of birds";
   max_value=1.0;
   factor=1.0;
   rotfactor=M_PI/180.0;
   firstflag=true;

   for (int i=0;i<number_axes;i++)
      set_zero_on_release(i,false);

   if (number_buttons>0)
      { 
      button=new bool[number_buttons];
      for (int i=0;i<number_buttons;i++)
         button[i]=false;
      }

#ifndef USE_AFLOCK_QUATERNION
   old_xrot=old_yrot=old_zrot=0;
#endif
 
   for (int i=0;i<6;i++)
      set_zero_on_release(i,false);
# ifdef DEBUG
   fprintf(stderr,"InputDevice (%s) has %d axes and %d buttons. Driver ???\n",
           name, number_axes, number_buttons);  
# endif
   }

aflock::~aflock(void)
   {
   }

void aflock::prepareRead(void)
   {
   bool ret=false;
   static int lastbutton=-1;
   button_pressed=-1;
   button_released=-1;
   int buttons;

   if (flock==NULL)
      return;
   if (!flock->isActive())
      {
      fprintf(stderr,"Bird not active\n");
      return;
      }
   flock->prepareSample();
   }

bool aflock::readInputDevice(void)
   {
   bool ret=false;
   static int lastbutton=-1;
   button_pressed=-1;
   button_released=-1;
   int buttons;

   if (flock==NULL)
      return false;
   if (!flock->isActive())
      {
      fprintf(stderr,"Bird not active\n");
      return false;
      }
   flock->sample();
   for (int i=0;i<number_buttons;i++)
      button[i]=false;

   value[0]=-flock->yPos(receiver)/maxvalue(2)*factor;
   value[1]=-flock->zPos(receiver)/maxvalue(1)*factor;
   value[2]= flock->xPos(receiver)/maxvalue(0)*factor;

#ifdef USE_AFLOCK_QUATERNION
   value[3]=flock->xQuat(receiver);
   value[4]=flock->yQuat(receiver);
   value[5]=flock->zQuat(receiver);
   value[6]=flock->wQuat(receiver);
#else
   value[3]= flock->yRot(receiver)/maxvalue(3)*rotfactor;
   value[4]=-flock->zRot(receiver)/maxvalue(4)*rotfactor;
   value[5]= flock->xRot(receiver)/maxvalue(5)*rotfactor;

   if ((value[4]>89) && (value[4]<-89))
      return false;
   value[3]=fix2pi(value[3],old_xrot);
   old_xrot=value[3];
   value[4]=fix2pi(value[4],old_yrot);
   old_yrot=value[4];
   value[5]=fix2pi(value[5],old_zrot);
   old_zrot=value[5];
#endif

//   firstflag=true;

   return true;
   }

#ifdef USE_AFLOCK_QUATERNION

Quaternion& aflock::get_quaternion(TransformMode* tm=NULL,bool check_only=false)
   {
   static Quaternion ret;
   Quaternion inputrot;
   inputrot.x=value[3];   
   inputrot.y=value[4];   
   inputrot.z=value[5];   
   inputrot.w=value[6];   
   inputrot.normalize();
printf("inputrot %f %f %f %f\n",inputrot.x,inputrot.y,inputrot.z,inputrot.w);
   if (firstflag)
      {
      // unit 1 quaternion
      ret.x=0;   
      ret.y=0;   
      ret.z=0;   
      ret.w=1;
      firstflag=false;
      old_quat=inputrot;
      }
   else
      ret=inputrot*old_quat.conj();
   if (!check_only)
      old_quat=inputrot;
   return ret; 
   }

Quaternion& aflock::get_localQuaternion(TransformMode* tm=NULL,bool check_only=false)
   {
   static Quaternion ret;
   Quaternion inputrot;
   inputrot.x=-value[3];   
   inputrot.y=value[4];   
   inputrot.z=-value[5];   
   inputrot.w=value[6];   
   inputrot.normalize();
   if (firstflag)
      {
      // unit 1 quaternion
      ret.x=0;   
      ret.y=0;   
      ret.z=0;   
      ret.w=1;
      firstflag=false;
      old_quat=inputrot;
      }
   else
      ret=inputrot*old_quat.conj();
   if (!check_only)
      old_quat=inputrot;
   return ret; 
   }

bool aflock::allzero(void)
   {
/*
   if ((get_x(NULL,true)==0) && (get_y(NULL,true)==0) && (get_z(NULL,true)==0))
      {
      Quaternion testquat=get_quaternion(NULL,true);
      if ((testquat.x==0) && (testquat.y==0) && (testquat.z==0) && 
          (testquat.x==1))
         return true;
      }
*/
   return false;
   }


#else

float aflock::fix2pi(float rot,float oldrot)
   {
   return rot;
   if (firstflag)
      return rot;
   if ((rot-oldrot)>M_PI)
      return rot-2*M_PI;
   else if ((oldrot-rot)<-M_PI)
      return rot+2*M_PI;
   else
      return rot;
   }

#endif

# ifdef TEST_AFLOCK
int main(int argc,char** argv)
   {
   AflockDevice* device=new AflockDevice();
   aflock device1(device,argv[1],true);
   for (int i=1;i<3000;i++)
      {
      if (device1.readInputDevice())
         printf("1 %g %g %g\n",device1.get_x(),device1.get_y(),device1.get_z());
         printf("1 rot %g %g %g\n",device1.get_xrot(),device1.get_yrot(),device1.get_zrot());
      }
   delete device;
   }
# endif
#endif