inputdevice.cpp

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

/*
 * InputDevice.cpp 
 * Copyright (c) 1996-1999 Vojtech Pavlik, 2001-2002 J. "MUFTI" Scheurich 
 */

/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */

#include "InputDevice.h"
#include <string.h>
#include <math.h>

#ifdef WIN32
double copysign (double x, double y)
   {
   if (y>=0)
      return fabs(x);
   else
      return -fabs(x);
   }
#endif

float InputDevice::accelerate(float value,int num_axis)
   {
   float result=0;
   if (get_acceleration(num_axis)!=1.0)
      {
#  ifdef HAVE_POWF
      result=copysign(powf(get_acceleration(num_axis),fabs(value))-1,value);
#  else
      result=copysign(pow((double)get_acceleration(num_axis),fabs(value))-1,
                      value);
#  endif
      }
   result+=value*get_factor(num_axis);
//printf("result %f num_axis %d\n",result*get_sign(num_axis),num_axis);
   return result*get_sign(num_axis);
   }

static int max_position(float f1,float f2,float f3)
   {
   if (fabs(f1) >= fabs(f2)) 
      if (fabs(f1) >= fabs(f3))
         return 0;
   if (fabs(f2) >= fabs(f3)) 
      if (fabs(f2) >= fabs(f1))
         return 1;
   if (fabs(f3) >= fabs(f2))
      if (fabs(f3) >= fabs(f1))
         return 2;
   }

static int min_position(float f1,float f2,float f3)
   {
   if (fabs(f1) <= fabs(f2) )
      if (fabs(f1) <= fabs(f3))
         return 0;
   if (fabs(f2) <= fabs(f3)) 
      if (fabs(f2) <= fabs(f1))
         return 1;
   if (fabs(f3) <= fabs(f2)) 
      if (fabs(f3) <= fabs(f1))
         return 2;
   }

// test if value belongs to the greater values in 1, 2 (or 3) dimensions

bool InputDevice::max_value(int index,TransformMode* tm)
   {
   if (tm->tdimension==TM_3D)
      return(true);
   if (tm->tdimension==TM_2D)
      if (index<3)
         {
         if (index!=min_position(value[0],value[1],value[2]))
            return true;
         }
      else
         if (index!=(3+min_position(value[3],value[4],value[5])))
            return true;
   if (tm->tdimension==TM_1D)
      if (index<3)
         {
         if (index==max_position(value[0],value[1],value[2]))
            return true;
         }
      else
         if (index==(3+max_position(value[3],value[4],value[5])))
            return true;
   return false;
   }   

void InputDevice::set_firstflag(void)
   {
   for (int i=0;i<7;i++)
      if (!get_zero_on_release(i))
         firstflag[i]=true;
   }


// return value, if device deliver zero if you release it,
// otherwise return difference to last value

float InputDevice::get_value(int index,bool check_only,int num_axis)
   {
   // value > zerosize ?
   float zerosize=get_zero_size_fraction(num_axis)*maxvalue(num_axis);
   if (fabs(value[index]) < zerosize)
      return 0;
   float val;
   // subtract nullsize from value
   if (value[index]>0)
      val=value[index]-zerosize;
   else
      val=value[index]+zerosize;
   if (get_zero_on_release(num_axis))
      return(accelerate(val,num_axis));
   else
      {
      float ret;
      if (firstflag[index])
         {
         ret=0.0;
         if (check_only)
            {
            if (val!=0)
               {
               firstflag[index]=false;
               oldvalue[index]=val;
               }
            return(0);
            }
         firstflag[index]=false;
         }
      else
         ret=accelerate(val,num_axis)-
             accelerate(oldvalue[index],num_axis);
      if (!check_only)
         oldvalue[index]=val;
      return ret;
      }    
   }

#define INPUT_DEVICE_GET(functionname,index,num_axis) \
float functionname (TransformMode* tm,bool check_only) \
   { \
   if (tm==NULL) \
      return get_value(index,check_only,num_axis); \
   if (max_value(index,tm)) \
      return get_value(index,check_only,num_axis); \
   else \
      return 0; \
   }

//printf("no zero on release numaxis %d ret %f\n",num_axis,ret); 

INPUT_DEVICE_GET(InputDevice::get_x,0,num_axis_x)
INPUT_DEVICE_GET(InputDevice::get_y,1,num_axis_y)
INPUT_DEVICE_GET(InputDevice::get_z,2,num_axis_z)
INPUT_DEVICE_GET(InputDevice::get_xrot,3,num_axis_xrot)
INPUT_DEVICE_GET(InputDevice::get_yrot,4,num_axis_yrot)
INPUT_DEVICE_GET(InputDevice::get_zrot,5,num_axis_zrot)
INPUT_DEVICE_GET(InputDevice::get_angle,6,num_axis_angle)

Vec3f& InputDevice::get_vector(TransformMode* tm)
   {
   static Vec3f v;
   v.x=get_x(tm);
   v.y=get_y(tm);
   v.z=get_z(tm);
   if (get_number_axes()==4)
      v[2]=-v[2];
   TMode mode=tm->tmode;
   if (mode==TM_TRANSLATE) 
      {
      if (get_number_axes()==2)
         if (tm->t2axes==TM_NEAR_FAR) 
            {
            float temp=v[2];
            v[2]=-v[1];
            v[1]=temp;
            } 
      } 
   else if (mode==TM_ROCKET) 
      {
      v[0]=0;
      v[1]=0;
      } 
   else if (mode==TM_HOVER) 
      {
      if (get_number_axes()==3) 
         {
         v[0]=0;
         } 
      else if (get_number_axes()==2) 
         if (tm->t2axes==TM_NEAR_FAR) 
            {
            v[2]=-v[1];
            v[0]=0;
            v[1]=0;
            } 
      else if (tm->t2axes==TM_UP_DOWN) 
            {
            v[1]=v[1];
            v[0]=0;
            v[2]=0;
            }             
      } 
   else if (mode==TM_SCALE)
      {
      v[0]=-v[0];
      v[2]=-v[2];
      if (get_number_axes()==2) 
         if (tm->t2axes==TM_NEAR_FAR) 
            {
            float temp=v[2];
            v[2]=-v[1];
            v[1]=temp;
            }             
      } 
   else if (mode==TM_CENTER)
      {
      if (get_number_axes()==2) 
         if (tm->t2axes==TM_NEAR_FAR) 
            {
            float temp=v[2];
            v[2]=-v[1];
            v[1]=temp;
            }             
      } 
   return v;
   }

EulerAngles& InputDevice::get_eulerAngles(TransformMode* tm, bool check_only)
   {
   bool rotationOnly=false;
   bool rocketOnly=false;
   bool hoverOnly=false;
   static EulerAngles euler;   
   // check for constraints on 2 axes or 3 axes or 4 axes devices
   if (get_number_axes()<=4)
      if (tm!=NULL)
         if (tm->tmode==TM_ROTATE)
            rotationOnly=true;
         else if (tm->tmode==TM_ROCKET)
            rocketOnly=true;
   if (tm!=NULL)
      if (tm->tmode==TM_HOVER)
         hoverOnly=true;
   if (rotationOnly)
      { 
      if (get_number_axes()==4)
         {
         euler.x=/*RAD2DEG*/(-get_y(tm, check_only));
         euler.y=/*RAD2DEG*/( get_xrot(tm, check_only));
         euler.z=/*RAD2DEG*/(-get_x(tm, check_only));
         }
      else if (get_number_axes()==3)
         {
         euler.x=/*RAD2DEG*/(-get_y(tm, check_only));
         euler.y=/*RAD2DEG*/( get_z(tm, check_only));
         euler.z=/*RAD2DEG*/(-get_x(tm, check_only));
         }
      else if (get_number_axes()==2)
         {
         if (tm->t2axes==TM_NEAR_FAR)
            {
            euler.x=/*RAD2DEG*/(-get_y(tm, check_only));
            euler.y=/*RAD2DEG*/(get_x(tm, check_only));
            euler.z=0;
            }
         else 
            {
            euler.x=/*RAD2DEG*/(-get_y(tm, check_only));
            euler.y=0;
            euler.z=/*RAD2DEG*/(-get_x(tm, check_only));
            }
         }
      }
   else if (rocketOnly)
      {
      if (get_number_axes()==4)
         {
         euler.x=/*RAD2DEG*/(-get_y(tm, check_only));
         euler.y=/*RAD2DEG*/(-get_xrot(tm, check_only));
         euler.z=/*RAD2DEG*/(-get_x(tm, check_only));
         }
      else if (get_number_axes()==3)
         {
         if (tm->t2axes==TM_NEAR_FAR)
            { 
            euler.x=/*RAD2DEG*/(-get_y(tm, check_only));
            euler.y=/*RAD2DEG*/(-get_x(tm, check_only));
            euler.z=0;
            }
         else
            { 
            euler.x=/*RAD2DEG*/(-get_y(tm, check_only));
            euler.y=0;
            euler.z=/*RAD2DEG*/(-get_x(tm, check_only));
            }
         }
      }
   else if (hoverOnly)
      {
      if (get_number_axes()==4)
         {
         euler.x=0;
         euler.y=/*RAD2DEG*/(-get_xrot(tm, check_only));
         euler.z=0;
         }
      else if (get_number_axes()==3)
         {
         euler.x=0;
         euler.y=/*RAD2DEG*/(-get_x(tm, check_only));
         euler.z=0;
         }
      else if (get_number_axes()==2)
         {
         euler.x=0;
         euler.y=/*RAD2DEG*/(-get_x(tm, check_only));
         euler.z=0;
         }
      else
         {
         euler.x=0;
         euler.y=/*RAD2DEG*/( get_yrot(tm, check_only));
         euler.z=0;
         }
      }
   else
      {
      euler.x=/*RAD2DEG*/( get_xrot(tm, check_only));
      euler.y=/*RAD2DEG*/( get_yrot(tm, check_only));
      euler.z=/*RAD2DEG*/(-get_zrot(tm, check_only));
      }
   
   euler.w=EulOrdXYZs;
   return euler;
   }

Quaternion& InputDevice::get_quaternion(TransformMode* tm, bool check_only)
   {
   static Quaternion inputrot;
   EulerAngles euler=get_eulerAngles(tm,check_only);   
   inputrot=Eul_ToQuat(euler);
   inputrot.normalize();
   return inputrot;
   }

Quaternion& InputDevice::get_localQuaternion(TransformMode* tm, bool check_only)
   {
   static Quaternion inputrot;
   EulerAngles euler=get_eulerAngles(tm,check_only);
   euler.x=-euler.x;   
   euler.z=-euler.z;   
   inputrot=Eul_ToQuat(euler);
   inputrot.normalize();
   return inputrot;
   }

bool InputDevice::allzero(void)
   {
   if ((get_x(NULL,true)==0) && (get_y(NULL,true)==0) && (get_z(NULL,true)==0)
       && (get_xrot(NULL,true)==0) 
       && (get_yrot(NULL,true)==0) 
       && (get_zrot(NULL,true)==0)
      )
      return true;
   else
      return false;
   }

// signswap of value from axis

int InputDevice::get_sign(int axis_number)
   {
   if ((sign_swap==NULL) || (axis_number>=number_axes))
      return 1;
   else
      if (sign_swap[axis_number])
         return -1;
      else
         return 1;
   }

void InputDevice::set_sign_swap(int axis_number,bool value)
   {
   if (axis_number<number_axes)
      {
      if (sign_swap==NULL)
         {
         sign_swap=new bool[number_axes];
         for (int i=0;i<number_axes;i++)
            sign_swap[i]=false;
         }
      sign_swap[axis_number]=value;
      }
   else
      fprintf(stderr,"axis number %d do not exist \n",axis_number);
   }

// factor: multiplied to the axis value returned from device
// deliver 1.0 if factor is not set

float InputDevice::get_factor(int axis_number)
   {
   if ((factor==NULL) || (axis_number>=number_axes))
      return 1.0;
   else
      return factor[axis_number];
   }

void InputDevice::set_factor(int axis_number,float value)
   {
   if (axis_number<number_axes)
      {
      if (factor==NULL)
         {
         factor=new float[number_axes];
         for (int i=0;i<number_axes;i++)
            factor[i]=1.0;
         }
      factor[axis_number]=value;
      }
   else
      fprintf(stderr,"axis number %d do not exist \n",axis_number);
   }

void InputDevice::set_factor(char* string,float value)
   {
   for (int i=0;i<sizeof(axesinfo)/sizeof(char*);i++)
      if (stringncmp(string,axesinfo[i])==0)
         {
         set_factor(getAxisFromInformation(string),value);
         break;
         }
   if (stringncmp(string,"all")==0)
      {
      if (stringncmp(string,"allrot")!=0)
         {
         set_factor(num_axis_x,value);
         set_factor(num_axis_y,value);
         set_factor(num_axis_z,value);
         }
      if (stringncmp(string,"allxyz")!=0)
         {
         set_factor(num_axis_xrot,value);
         set_factor(num_axis_yrot,value);
         set_factor(num_axis_zrot,value);    
         }
      }
   }

// acceleration: additional acceleration for devices without acceleration
// deliver 1.0 (no acceleration) if acceleration is not set

float InputDevice::get_acceleration(int axis_number)
   {
   if ((acceleration==NULL) || (axis_number>=number_axes))
      return 1.0;
   else
      return acceleration[axis_number];
   }

void InputDevice::set_acceleration(int axis_number,float value)
   {
   if (axis_number<number_axes)
      {
      if (acceleration==NULL)
         {
         acceleration=new float[number_axes];
         for (int i=0;i<number_axes;i++)
            acceleration[i]=1.0;
         }
      acceleration[axis_number]=value;
      }
   else
      fprintf(stderr,"axis number %d do not exist \n",axis_number);
   }

void InputDevice::set_acceleration(char* string,float value)
   {
   for (int i=0;i<sizeof(axesinfo)/sizeof(char*);i++)
      if (stringncmp(string,axesinfo[i])==0)
         {
         set_acceleration(getAxisFromInformation(string),value);
         break;
         }
   if (stringncmp(string,"all")==0)
      {
      if (stringncmp(string,"allrot")!=0)
         {
         set_acceleration(num_axis_x,value);
         set_acceleration(num_axis_y,value);
         set_acceleration(num_axis_z,value);
         }
      if (stringncmp(string,"allxyz")!=0)
         {
         set_acceleration(num_axis_xrot,value);
         set_acceleration(num_axis_yrot,value);
         set_acceleration(num_axis_zrot,value);    
         }
      }
   }

// return true if axis number deliver zero when device is released

bool InputDevice::get_zero_on_release(int axis_number)
   {
   if ((zero_on_release==NULL) || (axis_number>=number_axes))
      return true;
   else
      return zero_on_release[axis_number];
   }

void InputDevice::set_zero_on_release(int axis_number,bool value)
   {
   if (axis_number<number_axes)
      {
      if (zero_on_release==NULL)
         {
         zero_on_release=new bool[number_axes];
         for (int i=0;i<number_axes;i++)
            zero_on_release[i]=true;
         }
      zero_on_release[axis_number]=value;
      }
   else
      fprintf(stderr,"axis number %d do not exist \n",axis_number);
   }

void InputDevice::set_zero_on_release(char* string,bool value)
   {
   for (int i=0;i<sizeof(axesinfo)/sizeof(char*);i++)
      if (stringncmp(string,axesinfo[i])==0)
         {
         set_zero_on_release(getAxisFromInformation(string),value);
         break;
         }
   if (stringncmp(string,"all")==0)
      {