src/events/SDL_gesture.c
author Sam Lantinga <slouken@libsdl.org>
Thu, 15 Jul 2010 06:51:16 -0700
changeset 4663 56a2d70de945
parent 4659 063b9455bd1a
child 4665 c2493813a2f4
permissions -rw-r--r--
Started trying to build gesture code for iPhone
     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    Founation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
    17 
    18     Sam Lantinga
    19     slouken@libsdl.org
    20 */
    21 #include "SDL_config.h"
    22 
    23 /* General mouse handling code for SDL */
    24 
    25 #include "SDL_events.h"
    26 #include "SDL_events_c.h"
    27 #include "SDL_gesture_c.h"
    28 
    29 //TODO: Replace with malloc
    30 #define MAXFINGERS 3
    31 #define MAXTOUCHES 2
    32 #define MAXTEMPLATES 4
    33 #define MAXPATHSIZE 1024
    34 
    35 #define DOLLARNPOINTS 64
    36 #define DOLLARSIZE 256
    37 
    38 //PHI = ((sqrt(5)-1)/2)
    39 #define PHI 0.618033989 
    40 
    41 typedef struct {
    42   float x,y;
    43 } Point;
    44 
    45 
    46 typedef struct {
    47   Point p;
    48   float pressure;
    49   int id;
    50 } Finger;
    51 
    52 
    53 typedef struct {
    54   float length;
    55   
    56   int numPoints;
    57   Point p[MAXPATHSIZE];
    58 } DollarPath;
    59 
    60 
    61 typedef struct {
    62   Finger f;
    63   Point cv;
    64   float dtheta,dDist;
    65   DollarPath dollarPath;
    66 } TouchPoint;
    67 
    68 typedef struct {
    69   Point path[DOLLARNPOINTS];
    70   unsigned long hash;
    71 } DollarTemplate;
    72 
    73 typedef struct {
    74   int id;
    75   Point res;
    76   Point centroid;
    77   TouchPoint gestureLast[MAXFINGERS];
    78   int numDownFingers;
    79 
    80   int numDollarTemplates;
    81   DollarTemplate dollarTemplate[MAXTEMPLATES];
    82 
    83   SDL_bool recording;
    84 } GestureTouch;
    85 
    86 GestureTouch gestureTouch[MAXTOUCHES];
    87 int numGestureTouches = 0;
    88 SDL_bool recordAll;
    89 
    90 int SDL_RecordGesture(int touchId) {
    91   int i;
    92   if(touchId < 0) recordAll = SDL_TRUE;
    93   for(i = 0;i < numGestureTouches; i++) {
    94     if((touchId < 0) || (gestureTouch[i].id == touchId)) {
    95       gestureTouch[i].recording = SDL_TRUE;
    96       if(touchId >= 0)
    97 	return 1;
    98     }      
    99   }
   100   return (touchId < 0);
   101 }
   102 
   103 unsigned long SDL_HashDollar(Point* points) {
   104   unsigned long hash = 5381;
   105   int i;
   106   for(i = 0;i < DOLLARNPOINTS; i++) { 
   107     hash = ((hash<<5) + hash) + points[i].x;
   108     hash = ((hash<<5) + hash) + points[i].y;
   109   }
   110   return hash;
   111 }
   112 
   113 static int SaveTemplate(DollarTemplate *templ, FILE *fp) {
   114   int i;
   115   fprintf(fp,"%lu ",templ->hash);
   116   for(i = 0;i < DOLLARNPOINTS;i++) {
   117     fprintf(fp,"%i %i ",(int)templ->path[i].x,(int)templ->path[i].y);
   118   }
   119   fprintf(fp,"\n");
   120   return 0;
   121 }
   122 
   123 
   124 int SDL_SaveAllDollarTemplates(FILE *fp) {  
   125   int i,j,rtrn = 0;
   126   for(i = 0; i < numGestureTouches; i++) {
   127     GestureTouch* touch = &gestureTouch[i];
   128     for(j = 0;j < touch->numDollarTemplates; j++) {
   129 	rtrn += SaveTemplate(&touch->dollarTemplate[i],fp);
   130     }
   131   }
   132   return rtrn;  
   133 }
   134 
   135 int SDL_SaveDollarTemplate(unsigned long gestureId, FILE *fp) {
   136   int i,j;
   137   for(i = 0; i < numGestureTouches; i++) {
   138     GestureTouch* touch = &gestureTouch[i];
   139     for(j = 0;j < touch->numDollarTemplates; j++) {
   140       if(touch->dollarTemplate[i].hash == gestureId) {
   141 	return SaveTemplate(&touch->dollarTemplate[i],fp);
   142       }
   143     }
   144   }
   145   SDL_SetError("Unknown gestureId");
   146   return -1;
   147 }
   148 
   149 //path is an already sampled set of points
   150 //Returns the index of the gesture on success, or -1
   151 static int SDL_AddDollarGesture(GestureTouch* inTouch,Point* path) {
   152   if(inTouch == NULL) {
   153     if(numGestureTouches == 0) return -1;
   154     int i = 0;
   155     for(i = 0;i < numGestureTouches; i++) {
   156       inTouch = &gestureTouch[i];
   157       if(inTouch->numDollarTemplates < MAXTEMPLATES) {
   158 	DollarTemplate *templ = 
   159 	  &inTouch->dollarTemplate[inTouch->numDollarTemplates];
   160 	memcpy(templ->path,path,DOLLARNPOINTS*sizeof(Point));
   161 	templ->hash = SDL_HashDollar(templ->path);
   162 	inTouch->numDollarTemplates++;
   163       }
   164     }
   165     return inTouch->numDollarTemplates - 1;
   166   }else if(inTouch->numDollarTemplates < MAXTEMPLATES) {
   167     DollarTemplate *templ = 
   168       &inTouch->dollarTemplate[inTouch->numDollarTemplates];
   169     memcpy(templ->path,path,DOLLARNPOINTS*sizeof(Point));
   170     templ->hash = SDL_HashDollar(templ->path);
   171     inTouch->numDollarTemplates++;
   172     return inTouch->numDollarTemplates - 1;
   173   }
   174   return -1;
   175 }
   176 
   177 int SDL_LoadDollarTemplates(int touchId, FILE *fp) {
   178   int i,loaded = 0;
   179   GestureTouch *touch = NULL;
   180   if(touchId >= 0) {
   181     for(i = 0;i < numGestureTouches; i++)
   182       if(gestureTouch[i].id == touchId)
   183 	touch = &gestureTouch[i];
   184     if(touch == NULL) return -1;
   185   }
   186 
   187   while(!feof(fp)) {
   188     DollarTemplate templ;
   189     fscanf(fp,"%lu ",&templ.hash);
   190     for(i = 0;i < DOLLARNPOINTS; i++) {		
   191       int x,y;
   192       if(fscanf(fp,"%i %i ",&x,&y) != 2) break;
   193       templ.path[i].x = x;
   194       templ.path[i].y = y;
   195     }
   196     fscanf(fp,"\n");
   197 
   198     if(touchId >= 0) {
   199       if(SDL_AddDollarGesture(touch,templ)) loaded++;
   200     }
   201     else {
   202       for(i = 0;i < numGestureTouches; i++) {
   203 	if(gestureTouch[i].id == touchId) {
   204 	  touch = &gestureTouch[i];
   205 	  SDL_AddDollarGesture(touch,templ);
   206 	}
   207       }
   208       loaded++;
   209     }
   210   }
   211 
   212   return 1; 
   213 }
   214 
   215 
   216 float dollarDifference(Point* points,Point* templ,float ang) {
   217   //  Point p[DOLLARNPOINTS];
   218   float dist = 0;
   219   Point p;
   220   int i;
   221   for(i = 0; i < DOLLARNPOINTS; i++) {
   222     p.x = points[i].x * cos(ang) - points[i].y * sin(ang);
   223     p.y = points[i].x * sin(ang) + points[i].y * cos(ang);
   224     dist += sqrt((p.x-templ[i].x)*(p.x-templ[i].x)+
   225 		 (p.y-templ[i].y)*(p.y-templ[i].y));
   226   }
   227   return dist/DOLLARNPOINTS;
   228   
   229 }
   230 
   231 float bestDollarDifference(Point* points,Point* templ) {
   232   //------------BEGIN DOLLAR BLACKBOX----------------//
   233   //-TRANSLATED DIRECTLY FROM PSUDEO-CODE AVAILABLE AT-//
   234   //-"http://depts.washington.edu/aimgroup/proj/dollar/"-//
   235   float ta = -M_PI/4;
   236   float tb = M_PI/4;
   237   float dt = M_PI/90;
   238   float x1 = PHI*ta + (1-PHI)*tb;
   239   float f1 = dollarDifference(points,templ,x1);
   240   float x2 = (1-PHI)*ta + PHI*tb;
   241   float f2 = dollarDifference(points,templ,x2);
   242   while(abs(ta-tb) > dt) {
   243     if(f1 < f2) {
   244       tb = x2;
   245       x2 = x1;
   246       f2 = f1;
   247       x1 = PHI*ta + (1-PHI)*tb;
   248       f1 = dollarDifference(points,templ,x1);
   249     }
   250     else {
   251       ta = x1;
   252       x1 = x2;
   253       f1 = f2;
   254       x2 = (1-PHI)*ta + PHI*tb;
   255       f2 = dollarDifference(points,templ,x2);
   256     }
   257   }
   258   /*
   259   if(f1 <= f2)
   260     printf("Min angle (x1): %f\n",x1);
   261   else if(f1 >  f2)
   262     printf("Min angle (x2): %f\n",x2);
   263   */
   264   return SDL_min(f1,f2);  
   265 }
   266 
   267 //DollarPath contains raw points, plus (possibly) the calculated length
   268 int dollarNormalize(DollarPath path,Point *points) {
   269   int i;
   270   //Calculate length if it hasn't already been done
   271   if(path.length <= 0) {
   272     for(i=1;i<path.numPoints;i++) {
   273       float dx = path.p[i  ].x - 
   274 	         path.p[i-1].x;
   275       float dy = path.p[i  ].y - 
   276 	         path.p[i-1].y;
   277       path.length += sqrt(dx*dx+dy*dy);
   278     }
   279   }
   280 
   281 
   282   //Resample
   283   float interval = path.length/(DOLLARNPOINTS - 1);
   284   float dist = 0;
   285 
   286   int numPoints = 0;
   287   Point centroid; centroid.x = 0;centroid.y = 0;
   288   //printf("(%f,%f)\n",path.p[path.numPoints-1].x,path.p[path.numPoints-1].y);
   289   for(i = 1;i < path.numPoints;i++) {
   290     float d = sqrt((path.p[i-1].x-path.p[i].x)*(path.p[i-1].x-path.p[i].x)+
   291 		   (path.p[i-1].y-path.p[i].y)*(path.p[i-1].y-path.p[i].y));
   292     //printf("d = %f dist = %f/%f\n",d,dist,interval);
   293     while(dist + d > interval) {
   294       points[numPoints].x = path.p[i-1].x + 
   295 	((interval-dist)/d)*(path.p[i].x-path.p[i-1].x);
   296       points[numPoints].y = path.p[i-1].y + 
   297 	((interval-dist)/d)*(path.p[i].y-path.p[i-1].y);
   298       centroid.x += points[numPoints].x;
   299       centroid.y += points[numPoints].y;
   300       numPoints++;
   301 
   302       dist -= interval;
   303     }
   304     dist += d;
   305   }
   306   if(numPoints < 1) return 0;
   307   centroid.x /= numPoints;
   308   centroid.y /= numPoints;
   309  
   310   //printf("Centroid (%f,%f)",centroid.x,centroid.y);
   311   //Rotate Points so point 0 is left of centroid and solve for the bounding box
   312   float xmin,xmax,ymin,ymax;
   313   xmin = centroid.x;
   314   xmax = centroid.x;
   315   ymin = centroid.y;
   316   ymax = centroid.y;
   317   
   318   float ang = atan2(centroid.y - points[0].y,
   319 		    centroid.x - points[0].x);
   320 
   321   for(i = 0;i<numPoints;i++) {					       
   322     float px = points[i].x;
   323     float py = points[i].y;
   324     points[i].x = (px - centroid.x)*cos(ang) - 
   325                   (py - centroid.y)*sin(ang) + centroid.x;
   326     points[i].y = (px - centroid.x)*sin(ang) + 
   327                   (py - centroid.y)*cos(ang) + centroid.y;
   328 
   329 
   330     if(points[i].x < xmin) xmin = points[i].x;
   331     if(points[i].x > xmax) xmax = points[i].x; 
   332     if(points[i].y < ymin) ymin = points[i].y;
   333     if(points[i].y > ymax) ymax = points[i].y;
   334   }
   335 
   336   //Scale points to DOLLARSIZE, and translate to the origin
   337   float w = xmax-xmin;
   338   float h = ymax-ymin;
   339 
   340   for(i=0;i<numPoints;i++) {
   341     points[i].x = (points[i].x - centroid.x)*DOLLARSIZE/w;
   342     points[i].y = (points[i].y - centroid.y)*DOLLARSIZE/h;
   343   }  
   344   return numPoints;
   345 }
   346 
   347 float dollarRecognize(DollarPath path,int *bestTempl,GestureTouch* touch) {
   348 	
   349 	Point points[DOLLARNPOINTS];
   350 	int numPoints = dollarNormalize(path,points);
   351 	int i;
   352 	
   353 	int bestDiff = 10000;
   354 	*bestTempl = -1;
   355 	for(i = 0;i < touch->numDollarTemplates;i++) {
   356 		int diff = bestDollarDifference(points,touch->dollarTemplate[i].path);
   357 		if(diff < bestDiff) {bestDiff = diff; *bestTempl = i;}
   358 	}
   359 	return bestDiff;
   360 }
   361 
   362 int SDL_GestureAddTouch(SDL_Touch* touch) { 
   363   if(numGestureTouches >= MAXTOUCHES) return -1;
   364   
   365   gestureTouch[numGestureTouches].res.x = touch->xres;
   366   gestureTouch[numGestureTouches].res.y = touch->yres;
   367   gestureTouch[numGestureTouches].numDownFingers = 0;
   368 
   369   gestureTouch[numGestureTouches].res.x = touch->xres;
   370   gestureTouch[numGestureTouches].id = touch->id;
   371 
   372   gestureTouch[numGestureTouches].numDollarTemplates = 0;
   373 
   374   gestureTouch[numGestureTouches].recording = SDL_FALSE;
   375 
   376   numGestureTouches++;
   377   return 0;
   378 }
   379 
   380 GestureTouch * SDL_GetGestureTouch(int id) {
   381   int i;
   382   for(i = 0;i < numGestureTouches; i++) {
   383     //printf("%i ?= %i\n",gestureTouch[i].id,id);
   384     if(gestureTouch[i].id == id) return &gestureTouch[i];
   385   }
   386   return NULL;
   387 }
   388 
   389 int SDL_SendGestureMulti(GestureTouch* touch,float dTheta,float dDist) {
   390   SDL_Event event;
   391   event.mgesture.type = SDL_MULTIGESTURE;
   392   event.mgesture.touchId = touch->id;
   393   event.mgesture.x = touch->centroid.x;
   394   event.mgesture.y = touch->centroid.y;
   395   event.mgesture.dTheta = dTheta;
   396   event.mgesture.dDist = dDist;  
   397   return SDL_PushEvent(&event) > 0;
   398 }
   399 
   400 int SDL_SendGestureDollar(GestureTouch* touch,int gestureId,float error) {
   401   SDL_Event event;
   402   event.dgesture.type = SDL_DOLLARGESTURE;
   403   event.dgesture.touchId = touch->id;
   404   /*
   405     //TODO: Add this to give location of gesture?
   406   event.mgesture.x = touch->centroid.x;
   407   event.mgesture.y = touch->centroid.y;
   408   */
   409   event.dgesture.gestureId = gestureId;
   410   event.dgesture.error = error;  
   411   return SDL_PushEvent(&event) > 0;
   412 }
   413 
   414 
   415 int SDL_SendDollarRecord(GestureTouch* touch,int gestureId) {
   416   SDL_Event event;
   417   event.dgesture.type = SDL_DOLLARRECORD;
   418   event.dgesture.touchId = touch->id;
   419   event.dgesture.gestureId = gestureId;
   420 
   421   return SDL_PushEvent(&event) > 0;
   422 }
   423 
   424 
   425 void SDL_GestureProcessEvent(SDL_Event* event)
   426 {
   427   if(event->type == SDL_FINGERMOTION || 
   428      event->type == SDL_FINGERDOWN ||
   429      event->type == SDL_FINGERUP) {
   430     GestureTouch* inTouch = SDL_GetGestureTouch(event->tfinger.touchId);
   431 
   432     //Shouldn't be possible
   433     if(inTouch == NULL) return;
   434     
   435     
   436     float x = ((float)event->tfinger.x)/inTouch->res.x;
   437     float y = ((float)event->tfinger.y)/inTouch->res.y;
   438     int j,empty = -1;
   439     
   440     for(j = 0;j<inTouch->numDownFingers;j++) {
   441       if(inTouch->gestureLast[j].f.id != event->tfinger.fingerId) continue;
   442       //Finger Up
   443       if(event->type == SDL_FINGERUP) {
   444 	inTouch->numDownFingers--;
   445 
   446 	if(inTouch->recording) {
   447 	  inTouch->recording = SDL_FALSE;
   448 	  Point path[DOLLARNPOINTS];
   449 	  dollarNormalize(inTouch->gestureLast[j].dollarPath,path);
   450 	  int index;
   451 	  if(recordAll) {
   452 	    index = SDL_AddDollarGesture(NULL,path);
   453 	    int i;
   454 	    for(i = 0;i < numGestureTouches; i++)
   455 	      gestureTouch[i].recording = SDL_FALSE;
   456 	  }
   457 	  else {
   458 	    index = SDL_AddDollarGesture(inTouch,path);
   459 	  }
   460 	  
   461 	  if(index >= 0) {
   462 	    SDL_SendDollarRecord(inTouch,inTouch->dollarTemplate[index].hash);
   463 	  }
   464 	  else {
   465 	    SDL_SendDollarRecord(inTouch,-1);
   466 	  }
   467 	}
   468 	else {	
   469 	  int bestTempl;
   470 	  float error;
   471 	  error = dollarRecognize(inTouch->gestureLast[j].dollarPath,
   472 				  &bestTempl,inTouch);
   473 	  if(bestTempl >= 0){
   474 	    //Send Event
   475 	    int gestureId = inTouch->dollarTemplate[bestTempl].hash;
   476 	    SDL_SendGestureDollar(inTouch,gestureId,error);
   477 	    printf("Dollar error: %f\n",error);
   478 	  }
   479 	} 
   480 	inTouch->gestureLast[j] = inTouch->gestureLast[inTouch->numDownFingers];
   481 	break;
   482       }
   483       else {
   484 	float dx = x - inTouch->gestureLast[j].f.p.x;
   485 	float dy = y - inTouch->gestureLast[j].f.p.y;
   486 	DollarPath* path = &inTouch->gestureLast[j].dollarPath;
   487 	if(path->numPoints < MAXPATHSIZE) {
   488 	  path->p[path->numPoints].x = x;
   489 	  path->p[path->numPoints].y = y;
   490 	  path->length += sqrt(dx*dx + dy*dy);
   491 	  path->numPoints++;
   492 	}
   493 
   494 
   495 	inTouch->centroid.x += dx/inTouch->numDownFingers;
   496 	inTouch->centroid.y += dy/inTouch->numDownFingers;    
   497 	if(inTouch->numDownFingers > 1) {
   498 	  Point lv; //Vector from centroid to last x,y position
   499 	  Point v; //Vector from centroid to current x,y position
   500 	  lv = inTouch->gestureLast[j].cv;
   501 	  float lDist = sqrt(lv.x*lv.x + lv.y*lv.y);
   502 	  //printf("lDist = %f\n",lDist);
   503 	  v.x = x - inTouch->centroid.x;
   504 	  v.y = y - inTouch->centroid.y;
   505 	  inTouch->gestureLast[j].cv = v;
   506 	  float Dist = sqrt(v.x*v.x+v.y*v.y);
   507 	  // cos(dTheta) = (v . lv)/(|v| * |lv|)
   508 	  
   509 	  //Normalize Vectors to simplify angle calculation
   510 	  lv.x/=lDist;
   511 	  lv.y/=lDist;
   512 	  v.x/=Dist;
   513 	  v.y/=Dist;
   514 	  float dtheta = atan2(lv.x*v.y - lv.y*v.x,lv.x*v.x + lv.y*v.y);
   515 	  
   516 	  float dDist = (Dist - lDist);
   517 	  if(lDist == 0) {dDist = 0;dtheta = 0;} //To avoid impossible values
   518 	  inTouch->gestureLast[j].dDist = dDist;
   519 	  inTouch->gestureLast[j].dtheta = dtheta;
   520 	  
   521 	  //printf("dDist = %f, dTheta = %f\n",dDist,dtheta);
   522 	  //gdtheta = gdtheta*.9 + dtheta*.1;
   523 	  //gdDist  =  gdDist*.9 +  dDist*.1
   524 	  //knob.r += dDist/numDownFingers;
   525 	  //knob.ang += dtheta;
   526 	  //printf("thetaSum = %f, distSum = %f\n",gdtheta,gdDist);
   527 	  //printf("id: %i dTheta = %f, dDist = %f\n",j,dtheta,dDist);
   528 	  SDL_SendGestureMulti(inTouch,dtheta,dDist);
   529 	}
   530 	else {
   531 	  inTouch->gestureLast[j].dDist = 0;
   532 	  inTouch->gestureLast[j].dtheta = 0;
   533 	  inTouch->gestureLast[j].cv.x = 0;
   534 	  inTouch->gestureLast[j].cv.y = 0;
   535 	}
   536 	inTouch->gestureLast[j].f.p.x = x;
   537 	inTouch->gestureLast[j].f.p.y = y;
   538 	break;
   539 	//pressure?
   540       }      
   541     }
   542     
   543     if(j == inTouch->numDownFingers) {
   544       //printf("Finger Down!!!\n");
   545       inTouch->numDownFingers++;
   546       inTouch->centroid.x = (inTouch->centroid.x*(inTouch->numDownFingers - 1)+ 
   547 			     x)/inTouch->numDownFingers;
   548       inTouch->centroid.y = (inTouch->centroid.y*(inTouch->numDownFingers - 1)+
   549 			     y)/inTouch->numDownFingers;
   550       
   551       inTouch->gestureLast[j].f.id = event->tfinger.fingerId;
   552       inTouch->gestureLast[j].f.p.x  = x;
   553       inTouch->gestureLast[j].f.p.y  = y;	
   554       inTouch->gestureLast[j].cv.x = 0;
   555       inTouch->gestureLast[j].cv.y = 0;
   556 
   557       inTouch->gestureLast[j].dollarPath.length = 0;
   558       inTouch->gestureLast[j].dollarPath.p[0].x = x;
   559       inTouch->gestureLast[j].dollarPath.p[0].y = y;
   560       inTouch->gestureLast[j].dollarPath.numPoints = 1;
   561     }
   562   }
   563 }  
   564   
   565   /* vi: set ts=4 sw=4 expandtab: */
   566