1 files changed, 105 insertions, 32 deletions

diff --git a/pdp_opencv_bm.cc b/pdp_opencv_bm.cc
index 1d26dab..79f0c61 100755
--- a/pdp_opencv_bm.cc
+++ b/pdp_opencv_bm.cc
@@ -25,6 +25,7 @@
 #include <limits.h>
 #include <dlfcn.h>
 #include <ctype.h>
+#include <math.h>
 
 #include "pdp.h"
 
@@ -39,6 +40,7 @@ typedef struct pdp_opencv_bm_struct
 
   t_outlet *x_outlet0;
   t_outlet *x_outlet1;
+  t_outlet *x_outlet2;
   t_atom x_list[3];
 
   int x_packet0;
@@ -57,19 +59,27 @@ typedef struct pdp_opencv_bm_struct
 
   int x_nightmode;
   int x_threshold;
+  int x_useprevious;
+  int x_minblocks;
   CvFont font;
 
 } t_pdp_opencv_bm;
 
 static void pdp_opencv_bm_process_rgb(t_pdp_opencv_bm *x)
 {
-    t_pdp     *header = pdp_packet_header(x->x_packet0);
-    short int *data   = (short int *)pdp_packet_data(x->x_packet0);
-    t_pdp     *newheader = pdp_packet_header(x->x_packet1);
-    short int *newdata = (short int *)pdp_packet_data(x->x_packet1); 
-    int i,j,k,im;
-    int marked;
-    int px,py;
+  t_pdp     *header = pdp_packet_header(x->x_packet0);
+  short int *data   = (short int *)pdp_packet_data(x->x_packet0);
+  t_pdp     *newheader = pdp_packet_header(x->x_packet1);
+  short int *newdata = (short int *)pdp_packet_data(x->x_packet1); 
+  int i,j,k,im;
+  int marked;
+  int px,py;
+  double meanangle=0.0, meanx=0.0, meany=0.0, maxamp=0.0, maxangle=0.0;
+  int nbblocks=0;
+  CvPoint orig, dest;
+  double angle=0.0;
+  double hypotenuse=0.0;
+  char tindex[4];
 
     if ((x->x_width != (t_int)header->info.image.width) || 
         (x->x_height != (t_int)header->info.image.height) || (!x->image)) 
@@ -117,31 +127,79 @@ static void pdp_opencv_bm_process_rgb(t_pdp_opencv_bm *x)
         
     cvCalcOpticalFlowBM( x->prev_grey, x->grey, 
                          x->x_blocksize, x->x_shiftsize,
-                         x->x_maxrange, 1,
+                         x->x_maxrange, x->x_useprevious,
                          x->x_velx, x->x_vely  );
 
+    nbblocks = 0;
     for( py=0; py<x->x_velsize.height; py++ ) 
     {
       for( px=0; px<x->x_velsize.width; px++ )
       {
-        float velxf = (float)*( x->x_velx->imageData + py * x->x_velx->widthStep + px);
-        float velyf = (float)*( x->x_vely->imageData + py * x->x_vely->widthStep + px);
-
-        if ( sqrt( velxf*velxf + velyf*velyf ) > x->x_threshold )
+        // post( "pdp_opencv_bm : (%d,%d) values (%f,%f)", px, py, velxf, velyf );
+        orig.x = (px*x->x_width)/x->x_velsize.width;
+        orig.y = (py*x->x_height)/x->x_velsize.height;
+        dest.x = (int)(orig.x + cvGet2D(x->x_velx, py, px).val[0]);
+        dest.y = (int)(orig.y + cvGet2D(x->x_vely, py, px).val[0]);
+        angle = -atan2( (double) cvGet2D(x->x_vely, py, px).val[0], (double) cvGet2D(x->x_velx, py, px).val[0] );
+        hypotenuse = sqrt( pow(orig.y - dest.y, 2) + pow(orig.x - dest.x, 2) );
+
+        /* Now draw the tips of the arrow. I do some scaling so that the
+        * tips look proportional to the main line of the arrow.
+        */
+        if (hypotenuse >= x->x_threshold)
         {
-          // post( "pdp_opencv_bm : (%d,%d) values (%f,%f)", px, py, velxf, velyf );
-          CvPoint orig, dest;
-          orig.x = px*x->x_shiftsize.width;
-          orig.y = py*x->x_shiftsize.height;
-          dest.x = px*x->x_shiftsize.width+(int)velxf;
-          dest.y = py*x->x_shiftsize.height+(int)velyf;
-
-          cvLine( x->image, orig, dest, CV_RGB(0,255,0), 1, 8 );
-        }
+          cvLine( x->image, orig, dest, CV_RGB(0,255,0), (int)hypotenuse/10, CV_AA, 0 );
+
+          orig.x = (int) (dest.x - (x->x_shiftsize.width/4) * cos(angle + M_PI / 4));
+          orig.y = (int) (dest.y + (x->x_shiftsize.height/4) * sin(angle + M_PI / 4));
+          cvLine( x->image, orig, dest, CV_RGB(0,0,255), (int)hypotenuse/10, CV_AA, 0 );
+          orig.x = (int) (dest.x - (x->x_shiftsize.width/4) * cos(angle - M_PI / 4));
+          orig.y = (int) (dest.y + (x->x_shiftsize.height/4) * sin(angle - M_PI / 4));
+          cvLine( x->image, orig, dest, CV_RGB(0,0,255), (int)hypotenuse/10, CV_AA, 0 );
+          meanx = (meanx*nbblocks+cvGet2D(x->x_velx, py, px).val[0])/(nbblocks+1);
+          meany = (meanx*nbblocks+cvGet2D(x->x_vely, py, px).val[0])/(nbblocks+1);
+          if ( hypotenuse > maxamp )
+          {
+             maxamp = hypotenuse;
+             maxangle = angle;
+          } 
+          // post( "pdp_opencv_bm : block %d : amp : %f : angle : %f", nbblocks, hypotenuse, (angle*180)/M_PI );
+          nbblocks++;
+        } 
+
       }
     }
 
-    CV_SWAP( x->prev_grey, x->grey, x->swap_temp );
+    meanangle=-atan2( meany, meanx ); 
+    // post( "pdp_opencv_bm : meanangle : %f", (meanangle*180)/M_PI );
+
+    if ( nbblocks >= x->x_minblocks )
+    {
+      orig.x = (int) (x->x_width/2);
+      orig.y = (int) (x->x_height/2);
+      dest.x = (int) (orig.x+((x->x_width>x->x_height)?x->x_height/2:x->x_width/2)*cos(meanangle));
+      dest.y = (int) (orig.y-((x->x_width>x->x_height)?x->x_height/2:x->x_width/2)*sin(meanangle));
+      cvLine( x->image, orig, dest, CV_RGB(255,255,255), 3, CV_AA, 0 );
+      orig.x = (int) (dest.x - (x->x_shiftsize.width/2) * cos(meanangle + M_PI / 4));
+      orig.y = (int) (dest.y + (x->x_shiftsize.height/2) * sin(meanangle + M_PI / 4));
+      cvLine( x->image, orig, dest, CV_RGB(255,255,255), 3, CV_AA, 0 );
+      orig.x = (int) (dest.x - (x->x_shiftsize.width/2) * cos(meanangle - M_PI / 4));
+      orig.y = (int) (dest.y + (x->x_shiftsize.height/2) * sin(meanangle - M_PI / 4));
+      cvLine( x->image, orig, dest, CV_RGB(255,255,255), 3, CV_AA, 0 );
+
+      // outputs the average angle of movement
+      meanangle = (meanangle*180)/M_PI;
+      SETFLOAT(&x->x_list[0], meanangle);
+      outlet_list( x->x_outlet1, 0, 1, x->x_list );
+
+      // outputs the amplitude and angle of the maximum movement
+      maxangle = (maxangle*180)/M_PI;
+      SETFLOAT(&x->x_list[0], maxamp);
+      SETFLOAT(&x->x_list[1], maxangle);
+      outlet_list( x->x_outlet2, 0, 2, x->x_list );
+    }
+
+    memcpy( x->prev_grey->imageData, x->grey->imageData, x->x_size );
 
     memcpy( newdata, x->image->imageData, x->x_size*3 );
     return;
@@ -152,6 +210,16 @@ static void pdp_opencv_bm_nightmode(t_pdp_opencv_bm *x, t_floatarg f)
   if ((f==0.0)||(f==1.0)) x->x_nightmode = (int)f;
 }
 
+static void pdp_opencv_bm_useprevious(t_pdp_opencv_bm *x, t_floatarg f)
+{
+  if ((f==0.0)||(f==1.0)) x->x_useprevious = (int)f;
+}
+
+static void pdp_opencv_bm_minblocks(t_pdp_opencv_bm *x, t_floatarg f)
+{
+  if (f>=1.0) x->x_minblocks = (int)f;
+}
+
 static void pdp_opencv_bm_threshold(t_pdp_opencv_bm *x, t_floatarg f)
 {
   if (f>=0.0) x->x_threshold = (int)f;
@@ -159,8 +227,8 @@ static void pdp_opencv_bm_threshold(t_pdp_opencv_bm *x, t_floatarg f)
 
 static void pdp_opencv_bm_blocksize(t_pdp_opencv_bm *x, t_floatarg fwidth, t_floatarg fheight )
 {
-  if (fwidth>=1.0) x->x_blocksize.width = (int)fwidth;
-  if (fheight>=1.0) x->x_blocksize.height = (int)fheight;
+  if (fwidth>=5.0) x->x_blocksize.width = (int)fwidth;
+  if (fheight>=5.0) x->x_blocksize.height = (int)fheight;
 
   x->x_velsize.width = (x->x_width-x->x_blocksize.width)/x->x_shiftsize.width; 
   x->x_velsize.height = (x->x_height-x->x_blocksize.height)/x->x_shiftsize.height; 
@@ -173,8 +241,8 @@ static void pdp_opencv_bm_blocksize(t_pdp_opencv_bm *x, t_floatarg fwidth, t_flo
 
 static void pdp_opencv_bm_shiftsize(t_pdp_opencv_bm *x, t_floatarg fwidth, t_floatarg fheight )
 {
-  if (fwidth>=1.0) x->x_shiftsize.width = (int)fwidth;
-  if (fheight>=1.0) x->x_shiftsize.height = (int)fheight;
+  if (fwidth>=5.0) x->x_shiftsize.width = (int)fwidth;
+  if (fheight>=5.0) x->x_shiftsize.height = (int)fheight;
 
   x->x_velsize.width = (x->x_width-x->x_blocksize.width)/x->x_shiftsize.width; 
   x->x_velsize.height = (x->x_height-x->x_blocksize.height)/x->x_shiftsize.height; 
@@ -266,6 +334,7 @@ void *pdp_opencv_bm_new(t_floatarg f)
 
   x->x_outlet0 = outlet_new(&x->x_obj, &s_anything); 
   x->x_outlet1 = outlet_new(&x->x_obj, &s_anything);
+  x->x_outlet2 = outlet_new(&x->x_obj, &s_anything);
 
   x->x_packet0 = -1;
   x->x_packet1 = -1;
@@ -277,12 +346,14 @@ void *pdp_opencv_bm_new(t_floatarg f)
 
   x->x_nightmode=0;
   x->x_threshold=10;
-  x->x_blocksize.width = 10;
-  x->x_blocksize.height = 10;
-  x->x_shiftsize.width = 10;
-  x->x_shiftsize.height = 10;
-  x->x_maxrange.width = x->x_width;
-  x->x_maxrange.height = x->x_height;
+  x->x_blocksize.width = 20;
+  x->x_blocksize.height = 20;
+  x->x_shiftsize.width = 20;
+  x->x_shiftsize.height = 20;
+  x->x_maxrange.width = 10;
+  x->x_maxrange.height = 10;
+  x->x_useprevious = 0;
+  x->x_minblocks = 10;
   x->x_velsize.width = (x->x_width-x->x_blocksize.width)/x->x_shiftsize.width; 
   x->x_velsize.height = (x->x_height-x->x_blocksize.height)/x->x_shiftsize.height; 
 
@@ -319,6 +390,8 @@ void pdp_opencv_bm_setup(void)
     class_addmethod(pdp_opencv_bm_class, (t_method)pdp_opencv_bm_blocksize, gensym("blocksize"), A_FLOAT, A_FLOAT, A_NULL );   
     class_addmethod(pdp_opencv_bm_class, (t_method)pdp_opencv_bm_shiftsize, gensym("shiftsize"), A_FLOAT, A_FLOAT, A_NULL );   
     class_addmethod(pdp_opencv_bm_class, (t_method)pdp_opencv_bm_maxrange, gensym("maxrange"), A_FLOAT, A_FLOAT, A_NULL );   
+    class_addmethod(pdp_opencv_bm_class, (t_method)pdp_opencv_bm_useprevious, gensym("useprevious"), A_FLOAT, A_NULL );   
+    class_addmethod(pdp_opencv_bm_class, (t_method)pdp_opencv_bm_minblocks, gensym("minblocks"), A_FLOAT, A_NULL );   
 
 }