# HG changeset patch
# User Jim Grandpre <jim.tla@gmail.com>
# Date 1276760487 14400
# Node ID 7dbcd71216dfc98ada8de22b4a6d07d4dafb254a
# Parent  744b75ad18b8d946ad550c4e7dcf0050ae13ca4a
Added $1 gesture recognition.
Functional.

diff -r 744b75ad18b8 -r 7dbcd71216df src/events/SDL_touch.c
--- a/src/events/SDL_touch.c	Tue Jun 15 02:12:16 2010 -0400
+++ b/src/events/SDL_touch.c	Thu Jun 17 03:41:27 2010 -0400
@@ -322,7 +322,7 @@
 	nf.last_y = y;
 	nf.last_pressure = pressure;
 	SDL_AddFinger(touch,&nf);
-
+	//if(x < 0 || y < 0) return 0; //should defer if only a partial input
 	posted = 0;
 	if (SDL_GetEventState(SDL_FINGERDOWN) == SDL_ENABLE) {
 	    SDL_Event event;
@@ -364,15 +364,14 @@
     int xrel;
     int yrel;
     int x_max = 0, y_max = 0;
-
+	
     if (!touch || touch->flush_motion) {
-        return 0;
+		return 0;
     }
-
-	if(finger == NULL) {
-		SDL_SendFingerDown(id,fingerid,SDL_TRUE,x,y,pressure);
-		return 0;
-	} else {
+    
+    if(finger == NULL) {
+		return SDL_SendFingerDown(id,fingerid,SDL_TRUE,x,y,pressure);	
+    } else {
 		/* the relative motion is calculated regarding the last position */
 		if (relative) {
 			xrel = x;
@@ -389,9 +388,9 @@
 		
 		/* Drop events that don't change state */
 		if (!xrel && !yrel) {
-	#if 0
+#if 0
 			printf("Touch event didn't change state - dropped!\n");
-	#endif
+#endif
 			return 0;
 		}
 		
diff -r 744b75ad18b8 -r 7dbcd71216df touchTest/gestureTest.c
--- a/touchTest/gestureTest.c	Tue Jun 15 02:12:16 2010 -0400
+++ b/touchTest/gestureTest.c	Thu Jun 17 03:41:27 2010 -0400
@@ -4,6 +4,7 @@
 #include <SDL_touch.h>
 
 #define PI 3.1415926535897
+#define PHI ((sqrt(5)-1)/2)
 #define WIDTH 640
 #define HEIGHT 480
 #define BPP 4
@@ -11,8 +12,8 @@
 
 #define MAXFINGERS 3
 
-
-
+#define DOLLARNPOINTS 64
+#define DOLLARSIZE 256
 
 //MUST BE A POWER OF 2!
 #define EVENT_BUF_SIZE 256
@@ -45,10 +46,26 @@
 } Line;
 
 
+typedef struct {
+  float length;
+  
+  int numPoints;
+  Point p[EVENT_BUF_SIZE]; //To be safe
+} DollarPath;
+
+
 Finger finger[MAXFINGERS];
 
 Finger gestureLast[MAXFINGERS];
+DollarPath dollarPath[MAXFINGERS];
+
+#define MAXTEMPLATES 4
+
+Point dollarTemplate[MAXTEMPLATES][DOLLARNPOINTS];
+int numDollarTemplates = 0;
+#ifdef DRAW_VECTOR_EST
 Line gestureLine[MAXFINGERS];
+#endif
 
 void handler (int sig)
 {
@@ -110,8 +127,7 @@
 	setpix(screen,tx,(int)(y+r*sin(a)),c);
 	setpix(screen,tx,(int)(y-r*sin(a)),c);
       }
-    }
-    else {
+    }    else {
       //Draw Outline
       setpix(screen,(int)(x+r*cos(a)),(int)(y+r*sin(a)),c);
       setpix(screen,(int)(x-r*cos(a)),(int)(y+r*sin(a)),c);
@@ -135,6 +151,165 @@
   }
 }
 
+void drawDollarPath(SDL_Surface* screen,Point* points,int numPoints,
+		    int rad,unsigned int col){
+  int i;
+  for(i=0;i<numPoints;i++) {
+    drawCircle(screen,points[i].x+screen->w/2,
+	       points[i].y+screen->h/2,
+	       rad,col);
+  }
+}
+
+float dollarDifference(Point* points,Point* templ,float ang) {
+  //  Point p[DOLLARNPOINTS];
+  float dist = 0;
+  Point p;
+  int i;
+  for(i = 0; i < DOLLARNPOINTS; i++) {
+    p.x = points[i].x * cos(ang) - points[i].y * sin(ang);
+    p.y = points[i].x * sin(ang) + points[i].y * cos(ang);
+    dist += sqrt((p.x-templ[i].x)*(p.x-templ[i].x)+
+		 (p.y-templ[i].y)*(p.y-templ[i].y));
+  }
+  return dist/DOLLARNPOINTS;
+  
+}
+
+float bestDollarDifference(Point* points,Point* templ) {
+  //------------BEGIN DOLLAR BLACKBOX----------------//
+  //-TRANSLATED DIRECTLY FROM PSUDEO-CODE AVAILABLE AT-//
+  //-"http://depts.washington.edu/aimgroup/proj/dollar/"-//
+  float ta = -PI/4;
+  float tb = PI/4;
+  float dt = PI/90;
+  float x1 = PHI*ta + (1-PHI)*tb;
+  float f1 = dollarDifference(points,templ,x1);
+  float x2 = (1-PHI)*ta + PHI*tb;
+  float f2 = dollarDifference(points,templ,x2);
+  while(abs(ta-tb) > dt) {
+    if(f1 < f2) {
+      tb = x2;
+      x2 = x1;
+      f2 = f1;
+      x1 = PHI*ta + (1-PHI)*tb;
+      f1 = dollarDifference(points,templ,x1);
+    }
+    else {
+      ta = x1;
+      x1 = x2;
+      f1 = f2;
+      x2 = (1-PHI)*ta + PHI*tb;
+      f2 = dollarDifference(points,templ,x2);
+    }
+  }
+  return SDL_min(f1,f2);
+  
+}
+
+float dollarRecognize(SDL_Surface* screen, DollarPath path,int *bestTempl) {
+
+  Point points[DOLLARNPOINTS];
+  int numPoints = dollarNormalize(path,points);
+  int i;
+  
+  int k;
+  for(k = 0;k<DOLLARNPOINTS;k++) {
+    printf("(%f,%f)\n",points[k].x,
+	   points[k].y);
+  }
+
+  drawDollarPath(screen,points,numPoints,-15,0xFF6600);
+
+  int bestDiff = 10000;
+  *bestTempl = -1;
+  for(i = 0;i < numDollarTemplates;i++) {
+    int diff = bestDollarDifference(points,dollarTemplate[i]);
+    if(diff < bestDiff) {bestDiff = diff; *bestTempl = i;}
+  }
+  return bestDiff;
+}
+
+//DollarPath contains raw points, plus (possibly) the calculated length
+int dollarNormalize(DollarPath path,Point *points) {
+  int i;
+  //Calculate length if it hasn't already been done
+  if(path.length <= 0) {
+    for(i=1;i<path.numPoints;i++) {
+      float dx = path.p[i  ].x - 
+	         path.p[i-1].x;
+      float dy = path.p[i  ].y - 
+	         path.p[i-1].y;
+      path.length += sqrt(dx*dx+dy*dy);
+    }
+  }
+
+
+  //Resample
+  float interval = path.length/(DOLLARNPOINTS - 1);
+  float dist = 0;
+
+  int numPoints = 0;
+  Point centroid; centroid.x = 0;centroid.y = 0;
+  //printf("(%f,%f)\n",path.p[path.numPoints-1].x,path.p[path.numPoints-1].y);
+  for(i = 1;i < path.numPoints;i++) {
+    float d = sqrt((path.p[i-1].x-path.p[i].x)*(path.p[i-1].x-path.p[i].x)+
+		   (path.p[i-1].y-path.p[i].y)*(path.p[i-1].y-path.p[i].y));
+    //printf("d = %f dist = %f/%f\n",d,dist,interval);
+    while(dist + d > interval) {
+      points[numPoints].x = path.p[i-1].x + 
+	((interval-dist)/d)*(path.p[i].x-path.p[i-1].x);
+      points[numPoints].y = path.p[i-1].y + 
+	((interval-dist)/d)*(path.p[i].y-path.p[i-1].y);
+      centroid.x += points[numPoints].x;
+      centroid.y += points[numPoints].y;
+      numPoints++;
+
+      dist -= interval;
+    }
+    dist += d;
+  }
+  if(numPoints < 1) return 0;
+  centroid.x /= numPoints;
+  centroid.y /= numPoints;
+ 
+  //printf("Centroid (%f,%f)",centroid.x,centroid.y);
+  //Rotate Points so point 0 is left of centroid and solve for the bounding box
+  float xmin,xmax,ymin,ymax;
+  xmin = centroid.x;
+  xmax = centroid.x;
+  ymin = centroid.y;
+  ymax = centroid.y;
+  
+  float ang = atan2(centroid.y - points[0].y,
+		    centroid.x - points[0].x);
+
+  for(i = 0;i<numPoints;i++) {					       
+    float px = points[i].x;
+    float py = points[i].y;
+    points[i].x = (px - centroid.x)*cos(ang) - 
+                  (py - centroid.y)*sin(ang) + centroid.x;
+    points[i].y = (px - centroid.x)*sin(ang) + 
+                  (py - centroid.y)*cos(ang) + centroid.y;
+
+
+    if(points[i].x < xmin) xmin = points[i].x;
+    if(points[i].x > xmax) xmax = points[i].x; 
+    if(points[i].y < ymin) ymin = points[i].y;
+    if(points[i].y > ymax) ymax = points[i].y;
+  }
+
+  //Scale points to DOLLARSIZE, and translate to the origin
+  float w = xmax-xmin;
+  float h = ymax-ymin;
+
+  for(i=0;i<numPoints;i++) {
+    points[i].x = (points[i].x - centroid.x)*DOLLARSIZE/w;
+    points[i].y = (points[i].y - centroid.y)*DOLLARSIZE/h;
+  }  
+  return numPoints;
+}
+
 void DrawScreen(SDL_Surface* screen, int h)
 {
   int x, y, xm,ym,c;
@@ -161,7 +336,9 @@
 //draw Touch History
   for(i = 0;i < MAXFINGERS;i++) {
     gestureLast[i].id = -1;
+#ifdef DRAW_VECTOR_EST
     gestureLine[i].points = 0;
+#endif
   }
   for(i = SDL_max(0,eventWrite - EVENT_BUF_SIZE);i != eventWrite;i++) {
     SDL_Event event = events[i&(EVENT_BUF_SIZE-1)];
@@ -175,11 +352,13 @@
       float x = ((float)event.tfinger.x)/inTouch->xres;
       float y = ((float)event.tfinger.y)/inTouch->yres;
       int j,empty = -1;
+      
       for(j = 0;j<MAXFINGERS;j++) {
 	if(gestureLast[j].id == event.tfinger.fingerId) {
 	  if(event.type == SDL_FINGERUP) {
+#ifdef DRAW_VECTOR_EST
 	    if(gestureLine[j].points > 0)
-	    drawLine(screen,
+	      drawLine(screen,
 		     gestureLine[j].s.x*screen->w,
 		     gestureLine[j].s.y*screen->h,
 		     (gestureLine[j].s.x +50*gestureLine[j].d.x)*screen->w,
@@ -187,10 +366,47 @@
 		     0xFF00);
 
 	    gestureLine[j].points = 0;
+#endif
+	    //ignore last point - probably invalid
+	    dollarPath[j].numPoints--;
+	    
+	    
+	    float dx = dollarPath[j].p[dollarPath[j].numPoints].x - 
+	      dollarPath[j].p[dollarPath[j].numPoints - 1].x;
+	    float dy = dollarPath[j].p[dollarPath[j].numPoints].y - 
+	      dollarPath[j].p[dollarPath[j].numPoints - 1].y;
+	    dollarPath[j].length -= sqrt(dx*dx+dy*dy);
+
+	    if(!keystat[32]){ //spacebar
+	      int bestTempl;
+	      float error = dollarRecognize(screen,dollarPath[j],&bestTempl);
+	      printf("%i\n",bestTempl);
+	      if(bestTempl >= 0){
+		drawDollarPath(screen,dollarTemplate[bestTempl]
+			       ,DOLLARNPOINTS,-15,0x0066FF);\
+		
+		printf("ERROR: %f\n",error);
+	      }
+	      
+	    }
+	    else if(numDollarTemplates < MAXTEMPLATES) {
+	      
+	      dollarNormalize(dollarPath[j],
+			      dollarTemplate[numDollarTemplates]);
+	      int k;
+	      /*
+	      for(k = 0;k<DOLLARNPOINTS;k++) {
+		printf("(%f,%f)\n",dollarTemplate[numDollarTemplates][i].x,
+		       dollarTemplate[numDollarTemplates][i].y);
+		       }*/
+	      numDollarTemplates++;	      
+	    }
+
 	    gestureLast[j].id = -1;
 	    break;
 	  }
 	  else {
+#ifdef DRAW_VECTOR_EST
 	    if(gestureLine[j].points == 1) {
 	      gestureLine[j].d.x = x - gestureLine[j].s.x;
 	      gestureLine[j].d.y = y - gestureLine[j].s.y;
@@ -211,9 +427,18 @@
 	    gestureLine[j].s.y /= gestureLine[j].points;
 
 	    gestureLine[j].d.x /= gestureLine[j].points;
-	    gestureLine[j].d.y /= gestureLine[j].points;
+	    gestureLine[j].d.y /= gestureLine[j].points;	    
+#endif
 
-	    
+	    dollarPath[j].p[dollarPath[j].numPoints].x = x;
+	    dollarPath[j].p[dollarPath[j].numPoints].y = y;
+	    float dx = (dollarPath[j].p[dollarPath[j].numPoints-1].x-
+		      dollarPath[j].p[dollarPath[j].numPoints  ].x);
+	    float dy = (dollarPath[j].p[dollarPath[j].numPoints-1].y-
+		      dollarPath[j].p[dollarPath[j].numPoints  ].y);
+	    dollarPath[j].length += sqrt(dx*dx + dy*dy);
+
+	    dollarPath[j].numPoints++;
 	    
 
 	    gestureLast[j].p.x = x;
@@ -226,17 +451,23 @@
 	  empty = j;
 	}
       }
-
+      
       if(j >= MAXFINGERS && empty >= 0) {
-	printf("Finger Down!!!\n");
+	//	printf("Finger Down!!!\n");
 	j = empty; //important that j is the index of the added finger
 	gestureLast[j].id = event.tfinger.fingerId;
 	gestureLast[j].p.x  = x;
 	gestureLast[j].p.y  = y;
-
+#ifdef DRAW_VECTOR_EST
 	gestureLine[j].s.x = x;
 	gestureLine[j].s.y = y;
 	gestureLine[j].points = 1;
+#endif
+
+	dollarPath[j].length = 0;
+	dollarPath[j].p[0].x = x;
+	dollarPath[j].p[0].y = y;
+	dollarPath[j].numPoints = 1;
       }
 
       //draw the touch && each centroid:
@@ -273,7 +504,7 @@
 		   ,20,0xFF);
 
   
-  
+  keystat[32] = 0;	      
 
   if(SDL_MUSTLOCK(screen)) SDL_UnlockSurface(screen);
   
@@ -400,11 +631,11 @@
       }
     //And draw
     DrawScreen(screen,h);
-    /*
-      for(i=0;i<512;i++) 
-      if(keystat[i]) printf("%i\n",i);
-      printf("Buttons:%i\n",bstatus);
-    */
+    
+    //for(i=0;i<512;i++) 
+    // if(keystat[i]) printf("%i\n",i);
+    
+    
   }  
   SDL_Quit();