src/events/SDL_gesture.c
author Jim Grandpre <jim.tla@gmail.com>
Fri, 09 Jul 2010 00:50:40 -0700
changeset 4658 454385d76845
parent 4657 eed063a0bf5b
child 4659 063b9455bd1a
permissions -rw-r--r--
Moved $1 Gestures into the SDL Library
     1 /*
     2     SDL - Simple DirectMedia Layer
     3     Copyright (C) 1997-2010 Sam Lantinga
     4 
     5     This library is free software; you can redistribute it and/or
     6     modify it under the terms of the GNU Lesser General Public
     7     License as published by the Free Software Foundation; either
     8     version 2.1 of the License, or (at your option) any later version.
     9 
    10     This library is distributed in the hope that it will be useful,
    11     but WITHOUT ANY WARRANTY; without even the implied warranty of
    12     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
    13     Lesser General Public License for more details.
    14 
    15     You should have received a copy of the GNU Lesser General Public
    16     License along with this library; if not, write to the Free Software
    17     Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
    18 
    19     Sam Lantinga
    20     slouken@libsdl.org
    21 */
    22 #include "SDL_config.h"
    23 
    24 /* General mouse handling code for SDL */
    25 
    26 #include "SDL_events.h"
    27 #include "SDL_events_c.h"
    28 #include "SDL_gesture_c.h"
    29 
    30 //TODO: Replace with malloc
    31 #define MAXFINGERS 3
    32 #define MAXTOUCHES 2
    33 #define MAXTEMPLATES 4
    34 #define MAXPATHSIZE 1024
    35 
    36 #define DOLLARNPOINTS 64
    37 #define DOLLARSIZE 256
    38 
    39 //PHI = ((sqrt(5)-1)/2)
    40 #define PHI 0.618033989 
    41 
    42 typedef struct {
    43   float x,y;
    44 } Point;
    45 
    46 
    47 typedef struct {
    48   Point p;
    49   float pressure;
    50   int id;
    51 } Finger;
    52 
    53 
    54 typedef struct {
    55   float length;
    56   
    57   int numPoints;
    58   Point p[MAXPATHSIZE];
    59 } DollarPath;
    60 
    61 
    62 typedef struct {
    63   Finger f;
    64   Point cv;
    65   float dtheta,dDist;
    66   DollarPath dollarPath;
    67 } TouchPoint;
    68 
    69 
    70 typedef struct {
    71   int id;
    72   Point res;
    73   Point centroid;
    74   TouchPoint gestureLast[MAXFINGERS];
    75   int numDownFingers;
    76 
    77   int numDollarTemplates;
    78   Point dollarTemplate[MAXTEMPLATES][DOLLARNPOINTS];
    79 } GestureTouch;
    80 
    81 GestureTouch gestureTouch[MAXTOUCHES];
    82 int numGestureTouches = 0;
    83 
    84 float dollarDifference(Point* points,Point* templ,float ang) {
    85   //  Point p[DOLLARNPOINTS];
    86   float dist = 0;
    87   Point p;
    88   int i;
    89   for(i = 0; i < DOLLARNPOINTS; i++) {
    90     p.x = points[i].x * cos(ang) - points[i].y * sin(ang);
    91     p.y = points[i].x * sin(ang) + points[i].y * cos(ang);
    92     dist += sqrt((p.x-templ[i].x)*(p.x-templ[i].x)+
    93 		 (p.y-templ[i].y)*(p.y-templ[i].y));
    94   }
    95   return dist/DOLLARNPOINTS;
    96   
    97 }
    98 
    99 float bestDollarDifference(Point* points,Point* templ) {
   100   //------------BEGIN DOLLAR BLACKBOX----------------//
   101   //-TRANSLATED DIRECTLY FROM PSUDEO-CODE AVAILABLE AT-//
   102   //-"http://depts.washington.edu/aimgroup/proj/dollar/"-//
   103   float ta = -M_PI/4;
   104   float tb = M_PI/4;
   105   float dt = M_PI/90;
   106   float x1 = PHI*ta + (1-PHI)*tb;
   107   float f1 = dollarDifference(points,templ,x1);
   108   float x2 = (1-PHI)*ta + PHI*tb;
   109   float f2 = dollarDifference(points,templ,x2);
   110   while(abs(ta-tb) > dt) {
   111     if(f1 < f2) {
   112       tb = x2;
   113       x2 = x1;
   114       f2 = f1;
   115       x1 = PHI*ta + (1-PHI)*tb;
   116       f1 = dollarDifference(points,templ,x1);
   117     }
   118     else {
   119       ta = x1;
   120       x1 = x2;
   121       f1 = f2;
   122       x2 = (1-PHI)*ta + PHI*tb;
   123       f2 = dollarDifference(points,templ,x2);
   124     }
   125   }
   126   /*
   127   if(f1 <= f2)
   128     printf("Min angle (x1): %f\n",x1);
   129   else if(f1 >  f2)
   130     printf("Min angle (x2): %f\n",x2);
   131   */
   132   return SDL_min(f1,f2);  
   133 }
   134 
   135 float dollarRecognize(DollarPath path,int *bestTempl,GestureTouch* touch) {
   136 
   137   Point points[DOLLARNPOINTS];
   138   int numPoints = dollarNormalize(path,points);
   139   int i;
   140  
   141   int bestDiff = 10000;
   142   *bestTempl = -1;
   143   for(i = 0;i < touch->numDollarTemplates;i++) {
   144     int diff = bestDollarDifference(points,touch->dollarTemplate[i]);
   145     if(diff < bestDiff) {bestDiff = diff; *bestTempl = i;}
   146   }
   147   return bestDiff;
   148 }
   149 
   150 //DollarPath contains raw points, plus (possibly) the calculated length
   151 int dollarNormalize(DollarPath path,Point *points) {
   152   int i;
   153   //Calculate length if it hasn't already been done
   154   if(path.length <= 0) {
   155     for(i=1;i<path.numPoints;i++) {
   156       float dx = path.p[i  ].x - 
   157 	         path.p[i-1].x;
   158       float dy = path.p[i  ].y - 
   159 	         path.p[i-1].y;
   160       path.length += sqrt(dx*dx+dy*dy);
   161     }
   162   }
   163 
   164 
   165   //Resample
   166   float interval = path.length/(DOLLARNPOINTS - 1);
   167   float dist = 0;
   168 
   169   int numPoints = 0;
   170   Point centroid; centroid.x = 0;centroid.y = 0;
   171   //printf("(%f,%f)\n",path.p[path.numPoints-1].x,path.p[path.numPoints-1].y);
   172   for(i = 1;i < path.numPoints;i++) {
   173     float d = sqrt((path.p[i-1].x-path.p[i].x)*(path.p[i-1].x-path.p[i].x)+
   174 		   (path.p[i-1].y-path.p[i].y)*(path.p[i-1].y-path.p[i].y));
   175     //printf("d = %f dist = %f/%f\n",d,dist,interval);
   176     while(dist + d > interval) {
   177       points[numPoints].x = path.p[i-1].x + 
   178 	((interval-dist)/d)*(path.p[i].x-path.p[i-1].x);
   179       points[numPoints].y = path.p[i-1].y + 
   180 	((interval-dist)/d)*(path.p[i].y-path.p[i-1].y);
   181       centroid.x += points[numPoints].x;
   182       centroid.y += points[numPoints].y;
   183       numPoints++;
   184 
   185       dist -= interval;
   186     }
   187     dist += d;
   188   }
   189   if(numPoints < 1) return 0;
   190   centroid.x /= numPoints;
   191   centroid.y /= numPoints;
   192  
   193   //printf("Centroid (%f,%f)",centroid.x,centroid.y);
   194   //Rotate Points so point 0 is left of centroid and solve for the bounding box
   195   float xmin,xmax,ymin,ymax;
   196   xmin = centroid.x;
   197   xmax = centroid.x;
   198   ymin = centroid.y;
   199   ymax = centroid.y;
   200   
   201   float ang = atan2(centroid.y - points[0].y,
   202 		    centroid.x - points[0].x);
   203 
   204   for(i = 0;i<numPoints;i++) {					       
   205     float px = points[i].x;
   206     float py = points[i].y;
   207     points[i].x = (px - centroid.x)*cos(ang) - 
   208                   (py - centroid.y)*sin(ang) + centroid.x;
   209     points[i].y = (px - centroid.x)*sin(ang) + 
   210                   (py - centroid.y)*cos(ang) + centroid.y;
   211 
   212 
   213     if(points[i].x < xmin) xmin = points[i].x;
   214     if(points[i].x > xmax) xmax = points[i].x; 
   215     if(points[i].y < ymin) ymin = points[i].y;
   216     if(points[i].y > ymax) ymax = points[i].y;
   217   }
   218 
   219   //Scale points to DOLLARSIZE, and translate to the origin
   220   float w = xmax-xmin;
   221   float h = ymax-ymin;
   222 
   223   for(i=0;i<numPoints;i++) {
   224     points[i].x = (points[i].x - centroid.x)*DOLLARSIZE/w;
   225     points[i].y = (points[i].y - centroid.y)*DOLLARSIZE/h;
   226   }  
   227   return numPoints;
   228 }
   229 
   230 int SDL_GestureAddTouch(SDL_Touch* touch) { 
   231   if(numGestureTouches >= MAXTOUCHES) return -1;
   232   
   233   gestureTouch[numGestureTouches].res.x = touch->xres;
   234   gestureTouch[numGestureTouches].res.y = touch->yres;
   235   gestureTouch[numGestureTouches].numDownFingers = 0;
   236 
   237   gestureTouch[numGestureTouches].res.x = touch->xres;
   238   gestureTouch[numGestureTouches].id = touch->id;
   239 
   240   gestureTouch[numGestureTouches].numDollarTemplates = 0;
   241 
   242   numGestureTouches++;
   243   return 0;
   244 }
   245 
   246 GestureTouch * SDL_GetGestureTouch(int id) {
   247   int i;
   248   for(i = 0;i < numGestureTouches; i++) {
   249     //printf("%i ?= %i\n",gestureTouch[i].id,id);
   250     if(gestureTouch[i].id == id) return &gestureTouch[i];
   251   }
   252   return NULL;
   253 }
   254 
   255 int SDL_SendGestureMulti(GestureTouch* touch,float dTheta,float dDist) {
   256   SDL_Event event;
   257   event.mgesture.type = SDL_MULTIGESTURE;
   258   event.mgesture.touchId = touch->id;
   259   event.mgesture.x = touch->centroid.x;
   260   event.mgesture.y = touch->centroid.y;
   261   event.mgesture.dTheta = dTheta;
   262   event.mgesture.dDist = dDist;  
   263   return SDL_PushEvent(&event) > 0;
   264 }
   265 
   266 int SDL_SendGestureDollar(GestureTouch* touch,int gestureId,float error) {
   267   SDL_Event event;
   268   event.dgesture.type = SDL_DOLLARGESTURE;
   269   event.dgesture.touchId = touch->id;
   270   /*
   271     //TODO: Add this to give location of gesture?
   272   event.mgesture.x = touch->centroid.x;
   273   event.mgesture.y = touch->centroid.y;
   274   */
   275   event.dgesture.gestureId = gestureId;
   276   event.dgesture.error = error;  
   277   return SDL_PushEvent(&event) > 0;
   278 }
   279 
   280 void SDL_GestureProcessEvent(SDL_Event* event)
   281 {
   282   if(event->type == SDL_FINGERMOTION || 
   283      event->type == SDL_FINGERDOWN ||
   284      event->type == SDL_FINGERUP) {
   285     GestureTouch* inTouch = SDL_GetGestureTouch(event->tfinger.touchId);
   286 
   287     //Shouldn't be possible
   288     if(inTouch == NULL) return;
   289     
   290     
   291     float x = ((float)event->tfinger.x)/inTouch->res.x;
   292     float y = ((float)event->tfinger.y)/inTouch->res.y;
   293     int j,empty = -1;
   294     
   295     for(j = 0;j<inTouch->numDownFingers;j++) {
   296       if(inTouch->gestureLast[j].f.id != event->tfinger.fingerId) continue;
   297 
   298       if(event->type == SDL_FINGERUP) {
   299 	inTouch->numDownFingers--;
   300 
   301 	int bestTempl;
   302 	float error;
   303 	error = dollarRecognize(inTouch->gestureLast[j].dollarPath,
   304 				&bestTempl,inTouch);
   305 	if(bestTempl >= 0){
   306 	  //Send Event
   307 	  int gestureId = 0; //?
   308 	  SDL_SendGestureDollar(inTouch->id,gestureId,error);
   309 
   310 
   311 	  printf("Dollar error: %f\n",error);
   312 	}
   313 
   314 	inTouch->gestureLast[j] = inTouch->gestureLast[inTouch->numDownFingers];
   315 	break;
   316       }
   317       else {
   318 	float dx = x - inTouch->gestureLast[j].f.p.x;
   319 	float dy = y - inTouch->gestureLast[j].f.p.y;
   320 	DollarPath* path = &inTouch->gestureLast[j].dollarPath;
   321 	if(path->numPoints < MAXPATHSIZE) {
   322 	  path->p[path->numPoints].x = x;
   323 	  path->p[path->numPoints].y = y;
   324 	  path->length += sqrt(dx*dx + dy*dy);
   325 	  path->numPoints++;
   326 	}
   327 
   328 
   329 	inTouch->centroid.x += dx/inTouch->numDownFingers;
   330 	inTouch->centroid.y += dy/inTouch->numDownFingers;    
   331 	if(inTouch->numDownFingers > 1) {
   332 	  Point lv; //Vector from centroid to last x,y position
   333 	  Point v; //Vector from centroid to current x,y position
   334 	  lv = inTouch->gestureLast[j].cv;
   335 	  float lDist = sqrt(lv.x*lv.x + lv.y*lv.y);
   336 	  //printf("lDist = %f\n",lDist);
   337 	  v.x = x - inTouch->centroid.x;
   338 	  v.y = y - inTouch->centroid.y;
   339 	  inTouch->gestureLast[j].cv = v;
   340 	  float Dist = sqrt(v.x*v.x+v.y*v.y);
   341 	  // cos(dTheta) = (v . lv)/(|v| * |lv|)
   342 	  
   343 	  //Normalize Vectors to simplify angle calculation
   344 	  lv.x/=lDist;
   345 	  lv.y/=lDist;
   346 	  v.x/=Dist;
   347 	  v.y/=Dist;
   348 	  float dtheta = atan2(lv.x*v.y - lv.y*v.x,lv.x*v.x + lv.y*v.y);
   349 	  
   350 	  float dDist = (Dist - lDist);
   351 	  if(lDist == 0) {dDist = 0;dtheta = 0;} //To avoid impossible values
   352 	  inTouch->gestureLast[j].dDist = dDist;
   353 	  inTouch->gestureLast[j].dtheta = dtheta;
   354 	  
   355 	  //printf("dDist = %f, dTheta = %f\n",dDist,dtheta);
   356 	  //gdtheta = gdtheta*.9 + dtheta*.1;
   357 	  //gdDist  =  gdDist*.9 +  dDist*.1
   358 	  //knob.r += dDist/numDownFingers;
   359 	  //knob.ang += dtheta;
   360 	  //printf("thetaSum = %f, distSum = %f\n",gdtheta,gdDist);
   361 	  //printf("id: %i dTheta = %f, dDist = %f\n",j,dtheta,dDist);
   362 	  SDL_SendGestureMulti(inTouch,dtheta,dDist);
   363 	}
   364 	else {
   365 	  inTouch->gestureLast[j].dDist = 0;
   366 	  inTouch->gestureLast[j].dtheta = 0;
   367 	  inTouch->gestureLast[j].cv.x = 0;
   368 	  inTouch->gestureLast[j].cv.y = 0;
   369 	}
   370 	inTouch->gestureLast[j].f.p.x = x;
   371 	inTouch->gestureLast[j].f.p.y = y;
   372 	break;
   373 	//pressure?
   374       }      
   375     }
   376     
   377     if(j == inTouch->numDownFingers) {
   378       //printf("Finger Down!!!\n");
   379       inTouch->numDownFingers++;
   380       inTouch->centroid.x = (inTouch->centroid.x*(inTouch->numDownFingers - 1)+ 
   381 			     x)/inTouch->numDownFingers;
   382       inTouch->centroid.y = (inTouch->centroid.y*(inTouch->numDownFingers - 1)+
   383 			     y)/inTouch->numDownFingers;
   384       
   385       inTouch->gestureLast[j].f.id = event->tfinger.fingerId;
   386       inTouch->gestureLast[j].f.p.x  = x;
   387       inTouch->gestureLast[j].f.p.y  = y;	
   388       inTouch->gestureLast[j].cv.x = 0;
   389       inTouch->gestureLast[j].cv.y = 0;
   390 
   391       inTouch->gestureLast[j].dollarPath.length = 0;
   392       inTouch->gestureLast[j].dollarPath.p[0].x = x;
   393       inTouch->gestureLast[j].dollarPath.p[0].y = y;
   394       inTouch->gestureLast[j].dollarPath.numPoints = 1;
   395     }
   396   }
   397 }  
   398   
   399   /* vi: set ts=4 sw=4 expandtab: */
   400