9 files changed, 473 insertions, 14 deletions

diff --git a/Makefile b/Makefile
index bc5cc02..c805140 100644
--- a/Makefile
+++ b/Makefile
@@ -1,6 +1,8 @@
 LIBRARY = multitouch.so
 FDIS    = 11-multitouch.fdi
-MODULES = src
+MODULES = match src
+
+o_match = match
 
 o_src   = capabilities \
         iobuffer \
@@ -31,13 +33,11 @@ OPTS	= -O3
 .PHONY: all clean
 .PRECIOUS: obj/%.o
 
-all:    $(OBJS) $(TLIB) $(TOBJ)
-
-test:   $(TBIN)
+all:    $(OBJS) $(TLIB) $(TOBJ) $(TBIN)
 
 bin/%:  obj/%.o
         @mkdir -p $(@D)
-        gcc $< $(OBJS) $(LIBS) -o $@
+        gcc $< -o $@
 
 $(TLIB): $(OBJS)
         @rm -f $(TLIB)
@@ -62,3 +62,8 @@ install: $(TLIB) $(TFDI)
         install -d "$(DESTDIR)/$(DFDI)"
         install -m 755 $(TLIB) "$(DESTDIR)/$(DLIB)"
         install -m 644 $(TFDI) "$(DESTDIR)/$(DFDI)"
+
+test:
+        gcc $< $(OBJS) -o LINKTEST
+
+obj/match/test.o: match/match.c
diff --git a/match/match.c b/match/match.c
new file mode 100644
index 0000000..604faaf
--- /dev/null
+++ b/match/match.c
@@ -0,0 +1,326 @@
+#include "match.h"
+#include <string.h>
+#include <stdio.h>
+
+/**
+ * MATLAB implementation of the hungarian algorithm (2008)
+ *
+ * modified by Henrik Rydberg (2008)
+ */
+
+const float BIG_VALUE = 1e20;
+
+typedef unsigned short col_t;
+
+#define GETBIT2(m, row, col) ((m[col]>>row)&1U)
+#define SETBIT2(m, row, col) (m[col]|=(1U<<row))
+#define CLEARBIT2(m, row, col) (m[col]&=~(1U<<row))
+
+#define GETBIT(m, x) ((m>>x)&1U)
+#define SETBIT(m, x) (m|=(1U<<x))
+#define CLEARBIT(m, x) (m&=~(1U<<x))
+
+/********************************************************/
+
+static void buildixvector(int *ix, col_t *mstar, int nrows, int ncols)
+{
+        int row, col;
+        for (row = 0; row < nrows; row++) {
+                for (col = 0; col < ncols; col++) {
+                        if (GETBIT2(mstar, row, col)) {
+                                ix[row] = col;
+                                break;
+                        }
+                }
+        }
+}
+
+
+/********************************************************/
+
+static void step2a(int *ix, float *mdist, col_t *mstar, col_t *nmstar, col_t *mprime, col_t ccol, col_t crow, int nrows, int ncols, int dmin);
+static void step2b(int *ix, float *mdist, col_t *mstar, col_t *nmstar, col_t *mprime, col_t ccol, col_t crow, int nrows, int ncols, int dmin);
+static void step3 (int *ix, float *mdist, col_t *mstar, col_t *nmstar, col_t *mprime, col_t ccol, col_t crow, int nrows, int ncols, int dmin);
+static void step4 (int *ix, float *mdist, col_t *mstar, col_t *nmstar, col_t *mprime, col_t ccol, col_t crow, int nrows, int ncols, int dmin, int row, int col);
+static void step5 (int *ix, float *mdist, col_t *mstar, col_t *nmstar, col_t *mprime, col_t ccol, col_t crow, int nrows, int ncols, int dmin);
+
+static void ixoptimal(int *ix, float *mdist, int nrows, int ncols)
+{
+        float *mdistTemp, *mdistEnd, *columnEnd, value, minValue;
+        int nelem, dmin, row, col;
+        col_t ccol,crow, mstar[DIM_FINGER],mprime[DIM_FINGER],nmstar[DIM_FINGER];
+
+        ccol = crow = 0;
+        memset(mstar, 0, sizeof(mstar));
+        memset(mprime, 0, sizeof(mprime));
+        memset(nmstar, 0, sizeof(nmstar));
+
+        
+        /* initialization */
+        for(row=0; row<nrows; row++)
+                ix[row] = -1;
+        
+        nelem   = nrows * ncols;
+        mdistEnd = mdist + nelem;
+
+        /* preliminary steps */
+        if(nrows <= ncols) {
+                dmin = nrows;
+                
+                for(row=0; row<nrows; row++) {
+                        /* find the smallest element in the row */
+                        mdistTemp = mdist + row;
+                        minValue = *mdistTemp;
+                        mdistTemp += nrows;                     
+                        while(mdistTemp < mdistEnd) {
+                                value = *mdistTemp;
+                                if(value < minValue)
+                                        minValue = value;
+                                mdistTemp += nrows;
+                        }
+                        
+                        /* subtract the smallest element from each element of the row */
+                        mdistTemp = mdist + row;
+                        while(mdistTemp < mdistEnd) {
+                                *mdistTemp -= minValue;
+                                mdistTemp += nrows;
+                        }
+                }
+                
+                /* Steps 1 and 2a */
+                for(row=0; row<nrows; row++)
+                        for(col=0; col<ncols; col++)
+                                if(mdist[row + nrows*col] == 0)
+                                        if(!GETBIT(ccol, col)) {
+                                                SETBIT2(mstar, row, col);
+                                                SETBIT(ccol, col);
+                                                break;
+                                        }
+        }
+        else /* if(nrows > ncols) */
+        {
+                dmin = ncols;
+                
+                for(col=0; col<ncols; col++)
+                {
+                        /* find the smallest element in the column */
+                        mdistTemp = mdist     + nrows*col;
+                        columnEnd      = mdistTemp + nrows;
+                        
+                        minValue = *mdistTemp++;                        
+                        while(mdistTemp < columnEnd)
+                        {
+                                value = *mdistTemp++;
+                                if(value < minValue)
+                                        minValue = value;
+                        }
+                        
+                        /* subtract the smallest element from each element of the column */
+                        mdistTemp = mdist + nrows*col;
+                        while(mdistTemp < columnEnd)
+                                *mdistTemp++ -= minValue;
+                }
+                
+                /* Steps 1 and 2a */
+                for(col=0; col<ncols; col++)
+                        for(row=0; row<nrows; row++)
+                                if(mdist[row + nrows*col] == 0)
+                                        if(!GETBIT(crow, row))
+                                        {
+                                                SETBIT2(mstar, row, col);
+                                                SETBIT(ccol, col);
+                                                SETBIT(crow, row);
+                                                break;
+                                        }
+                for(row=0; row<nrows; row++)
+                        CLEARBIT(crow, row);
+                
+        }       
+        
+        /* move to step 2b */
+        step2b(ix, mdist, mstar, nmstar, mprime, ccol, crow, nrows, ncols, dmin);
+}
+
+/********************************************************/
+static void step2a(int *ix, float *mdist, col_t *mstar, col_t *nmstar, col_t *mprime, col_t ccol, col_t crow, int nrows, int ncols, int dmin)
+{
+        int col, row;
+        
+        /* cover every column containing a starred zero */
+        for(col=0; col<ncols; col++) {
+                for(row=col;row<nrows;row++) {
+                        if(GETBIT2(mstar, row, col)) {
+                                SETBIT(ccol, col);
+                                break;
+                        }
+                }       
+        }
+
+        /* move to step 3 */
+        step2b(ix, mdist, mstar, nmstar, mprime, ccol, crow, nrows, ncols, dmin);
+}
+
+/********************************************************/
+static void step2b(int *ix, float *mdist, col_t *mstar, col_t *nmstar, col_t *mprime, col_t ccol, col_t crow, int nrows, int ncols, int dmin)
+{
+        int col, ncc;
+        
+        /* count covered columns */
+        ncc = 0;
+        for(col=0; col<ncols; col++)
+                if(GETBIT(ccol, col))
+                        ncc++;
+                        
+        if(ncc == dmin)
+        {
+                /* algorithm finished */
+                buildixvector(ix, mstar, nrows, ncols);
+        }
+        else
+        {
+                /* move to step 3 */
+                step3(ix, mdist, mstar, nmstar, mprime, ccol, crow, nrows, ncols, dmin);
+        }
+        
+}
+
+/********************************************************/
+static void step3(int *ix, float *mdist, col_t *mstar, col_t *nmstar, col_t *mprime, col_t ccol, col_t crow, int nrows, int ncols, int dmin)
+{
+        bool zerosFound;
+        int row, col, cstar;
+
+        zerosFound = 1;
+        while(zerosFound)
+        {
+                zerosFound = 0;         
+                for(col=0; col<ncols; col++)
+                        if(!GETBIT(ccol,col))
+                                for(row=0; row<nrows; row++)
+                                        if((!GETBIT(crow,row)) && (mdist[row + nrows*col] == 0))
+                                        {
+                                                /* prime zero */
+                                                SETBIT2(mprime, row, col);
+                                                
+                                                /* find starred zero in current row */
+                                                for(cstar=0; cstar<ncols; cstar++)
+                                                        if(GETBIT2(mstar, row, cstar))
+                                                                break;
+                                                
+                                                if(cstar == ncols) /* no starred zero found */
+                                                {
+                                                        /* move to step 4 */
+                                                        step4(ix, mdist, mstar, nmstar, mprime, ccol, crow, nrows, ncols, dmin, row, col);
+                                                        return;
+                                                }
+                                                else
+                                                {
+                                                        SETBIT(crow, row);
+                                                        CLEARBIT(ccol,cstar);
+                                                        zerosFound              = 1;
+                                                        break;
+                                                }
+                                        }
+        }
+        
+        /* move to step 5 */
+        step5(ix, mdist, mstar, nmstar, mprime, ccol, crow, nrows, ncols, dmin);
+}
+
+/********************************************************/
+static void step4(int *ix, float *mdist, col_t *mstar, col_t *nmstar, col_t *mprime, col_t ccol, col_t crow, int nrows, int ncols, int dmin, int row, int col)
+{       
+        int n, rstar, cstar, primeRow, primeCol;
+        int nelem = nrows*ncols;
+        
+        /* generate temporary copy of mstar */
+        for(n=0; n<nelem; n++)
+                nmstar[n] = mstar[n];
+        
+        /* star current zero */
+        SETBIT2(nmstar, row, col);
+
+        /* find starred zero in current column */
+        cstar = col;
+        for(rstar=0; rstar<nrows; rstar++)
+                if(GETBIT2(mstar, rstar, cstar))
+                        break;
+
+        while(rstar<nrows)
+        {
+                /* unstar the starred zero */
+                CLEARBIT2(nmstar, rstar, cstar);
+        
+                /* find primed zero in current row */
+                primeRow = rstar;
+                for(primeCol=0; primeCol<ncols; primeCol++)
+                        if(GETBIT2(mprime, primeRow, primeCol))
+                                break;
+                                                                
+                /* star the primed zero */
+                SETBIT2(nmstar, primeRow, primeCol);
+        
+                /* find starred zero in current column */
+                cstar = primeCol;
+                for(rstar=0; rstar<nrows; rstar++)
+                        if(GETBIT2(mstar, rstar, cstar))
+                                break;
+        }       
+
+        /* use temporary copy as new mstar */
+        /* delete all primes, uncover all rows */
+        for(n=0; n<nelem; n++)
+        {
+                mprime[n] = 0;
+                mstar[n]  = nmstar[n];
+        }
+        for(n=0; n<nrows; n++)
+                CLEARBIT(crow, n);
+        
+        /* move to step 2a */
+        step2a(ix, mdist, mstar, nmstar, mprime, ccol, crow, nrows, ncols, dmin);
+}
+
+/********************************************************/
+static void step5(int *ix, float *mdist, col_t *mstar, col_t *nmstar, col_t *mprime, col_t ccol, col_t crow, int nrows, int ncols, int dmin)
+{
+        float h, value;
+        int row, col;
+        
+        /* find smallest uncovered element h */
+        h = BIG_VALUE;
+        for(row=0; row<nrows; row++)
+                if(!GETBIT(crow, row))
+                        for(col=0; col<ncols; col++)
+                                if(!GETBIT(ccol,col))
+                                {
+                                        value = mdist[row + nrows*col];
+                                        if(value < h)
+                                                h = value;
+                                }
+        
+        /* add h to each covered row */
+        for(row=0; row<nrows; row++)
+                if(GETBIT(crow, row))
+                        for(col=0; col<ncols; col++)
+                                mdist[row + nrows*col] += h;
+        
+        /* subtract h from each uncovered column */
+        for(col=0; col<ncols; col++)
+                if(!GETBIT(ccol,col))
+                        for(row=0; row<nrows; row++)
+                                mdist[row + nrows*col] -= h;
+        
+        /* move to step 3 */
+        step3(ix, mdist, mstar, nmstar, mprime, ccol, crow, nrows, ncols, dmin);
+}
+
+////////////////////////////////////////////////////////
+
+void match_fingers(int ix[DIM_FINGER], float A[DIM2_FINGER], int nrow, int ncol)
+{
+        ixoptimal(ix, A, nrow, ncol);
+}
+
+////////////////////////////////////////////////////////
+
diff --git a/match/match.h b/match/match.h
new file mode 100644
index 0000000..8936de4
--- /dev/null
+++ b/match/match.h
@@ -0,0 +1,25 @@
+#ifndef MATCHER_H
+#define MATCHER_H
+
+/**
+ * Special implementation of the hungarian algorithm.
+ * The maximum number of fingers matches a uint32.
+ * Bitmasks are used extensively.
+ */
+
+#define DIM_FINGER 16
+#define DIM2_FINGER (DIM_FINGER * DIM_FINGER)
+
+#define MIN(a, b) ((a) < (b) ? (a) : (b))
+#define MAX(a, b) ((a) < (b) ? (b) : (a))
+
+typedef int bool;
+
+////////////////////////////////////////////////////////
+
+void match_fingers(int index[DIM_FINGER], float A[DIM2_FINGER],
+                   int nrow, int ncol);
+
+////////////////////////////////////////////////////////
+
+#endif
diff --git a/match/test.c b/match/test.c
new file mode 100644
index 0000000..1544765
--- /dev/null
+++ b/match/test.c
@@ -0,0 +1,40 @@
+#include <stdio.h>
+#include <time.h>
+#include "match.c"
+
+#define ITS 1000000
+
+int main(int argc,char* argv[])
+{
+        // column-by-column matrix
+        float A[DIM2_FINGER];
+        float x1[DIM_FINGER]={1,5,2,3,4,5,6,7,8};
+        float y1[DIM_FINGER]={1,5,2,3,4,5.1,6,7,8};
+        float x2[DIM_FINGER]={1.1,3,2,4,5,6,7,8};
+        float y2[DIM_FINGER]={1,3,2,4,5,6,7,8};
+        int index[DIM_FINGER];
+        int n1 = 4;
+        int n2 = 7;
+
+        int i, j;
+        
+        for (i = 0; i < n1; i++) {
+                for (j = 0; j < n2; j++) {
+                        A[i + n1 * j] =
+                                (x1[i] - x2[j]) * (x1[i] - x2[j]) +
+                                (y1[i] - y2[j]) * (y1[i] - y2[j]);
+                }
+        }
+
+        clock_t t1 = clock();
+        for (i = 0; i < ITS; i++)
+                match_fingers(index, A, n1, n2);
+        clock_t t2 = clock();
+
+        printf("%lf matches per second\n", ITS * ((float)CLOCKS_PER_SEC / (t2 - t1)));
+
+        for (i = 0; i < n1; i++)
+                printf("match[%d] = %d\n", i, index[i]);
+        
+        return 0;
+}
diff --git a/src/common.h b/src/common.h
index 5a83f95..c57ae1a 100644
--- a/src/common.h
+++ b/src/common.h
@@ -7,6 +7,7 @@
 #include <xf86Xinput.h>
 #include <linux/input.h>
 #include <errno.h>
+#include <match/match.h>
 //#include <exevents.h>
 
 ////////////////////////////////////////////////////////
@@ -27,8 +28,6 @@
 #define ABS_MT_POSITION_X       0x35
 #define ABS_MT_POSITION_Y       0x36
 
-typedef int bool;
-
 #define SYSCALL(call) while (((call) == -1) && (errno == EINTR))
 
 ////////////////////////////////////////////////////////
diff --git a/src/hwdata.h b/src/hwdata.h
index 239e022..59cff0d 100644
--- a/src/hwdata.h
+++ b/src/hwdata.h
@@ -3,7 +3,6 @@
 
 #include "common.h"
 
-#define DIM_FINGER 16
 #define DIM_BUTTON 3
 
 #define MT_BUTTON_LEFT 0
diff --git a/src/multitouch.c b/src/multitouch.c
index f6e3d05..21f3e1e 100644
--- a/src/multitouch.c
+++ b/src/multitouch.c
@@ -82,7 +82,7 @@ static void read_input(LocalDevicePtr local)
         if (local->fd >= 0) {
                 while (read_synchronized_event(mt, local->fd)) {
                         modify_state(&mt->ns, &mt->hw);
-                        // and something in between here
+                        output_state(&mt->ns);
                         mt->os = mt->ns;
                 }
         }
diff --git a/src/state.c b/src/state.c
index 46b7fef..fc3580d 100644
--- a/src/state.c
+++ b/src/state.c
@@ -1,4 +1,5 @@
 #include "state.h"
+#include <stdlib.h>
 
 /******************************************************/
 
@@ -9,11 +10,58 @@ void init_state(struct State *s)
 
 /******************************************************/
 
+inline int fincomp(const struct FingerState* a,const struct FingerState* b)
+{
+        return a->id - b->id;
+}
+
+inline float dist2(const struct FingerData* a,const struct FingerData* b)
+{
+        float dx = a->position_x - b->position_x;
+        float dy = a->position_y - b->position_y;
+
+        return dx * dx + dy * dy;
+}
+
 void modify_state(struct State *s, const struct HWData* hw)
 {
-        int i;
-        if (s->button[0] != hw->button[0])
-                xf86Msg(X_INFO, "multitouch: button changed\n");
+        float A[DIM2_FINGER], *row;
+        int id[DIM_FINGER], index[DIM_FINGER], i, j;
+
+        for (j = 0; j < s->nfinger; j++) {
+                id[j] = s->finger[j].id;
+                row = A + hw->nfinger * j;
+                for (i = 0; i < hw->nfinger; i++)
+                        row[i] = dist2(&hw->finger[i], &s->finger[j].hw);
+        }
+
+        match_fingers(index, A, hw->nfinger, s->nfinger);
+
+        s->nfinger = 0;
+
+        /* update matched fingers */
+        for (i = 0; i < hw->nfinger; i++) {
+                if ((j = index[i]) >= 0) {
+                        s->finger[s->nfinger].id = id[j];
+                        s->finger[s->nfinger].hw = hw->finger[i];
+                        s->nfinger++;
+                }
+        }
+
+        /* create new fingers */
+        for (i = 0; i < hw->nfinger; i++) {
+                if (index[i] < 0) {
+                        s->finger[s->nfinger].id = ++s->lastid;
+                        s->finger[s->nfinger].hw = hw->finger[i];
+                        s->nfinger++;
+                }
+        }
+
+        /* sort fingers in touching order */
+        qsort(s->finger, s->nfinger, sizeof(struct FingerState),
+              (int (*)(const void*,const void*))fincomp);
+
+        /* copy buttons */
         for (i = 0; i < DIM_BUTTON; i++)
                 s->button[i] = hw->button[i];
 }
@@ -23,9 +71,11 @@ void modify_state(struct State *s, const struct HWData* hw)
 const struct FingerState *find_finger(const struct State *s, int id)
 {
         int i;
+
         for (i = 0; i < s->nfinger; i++)
                 if (s->finger[i].id == id)
-                        return s->finger+i;
+                        return s->finger + i;
+
         return NULL;
 }
 
@@ -33,6 +83,20 @@ const struct FingerState *find_finger(const struct State *s, int id)
 
 void output_state(const struct State *s)
 {
+        int i;
+        printf("buttons: %d%d%d\n", s->button[0], s->button[1], s->button[2]);
+        printf("fingers: %d\n", s->nfinger);
+        for (i = 0; i < s->nfinger; i++) {
+                printf("  %+02d %+05d:%+05d +%05d:%+05d %+05d %+05d:%+05d\n",
+                       s->finger[i].id,
+                       s->finger[i].hw.touch_major,
+                       s->finger[i].hw.touch_minor,
+                       s->finger[i].hw.width_major,
+                       s->finger[i].hw.width_minor,
+                       s->finger[i].hw.orientation,
+                       s->finger[i].hw.position_x,
+                       s->finger[i].hw.position_y);
+        }
 }
 
 /******************************************************/
diff --git a/src/state.h b/src/state.h
index 9c36617..8b41920 100644
--- a/src/state.h
+++ b/src/state.h
@@ -14,7 +14,8 @@ struct FingerState {
 
 struct State {
         struct FingerState finger[DIM_FINGER];
-        int nfinger, button[DIM_BUTTON];
+        int button[DIM_BUTTON];
+        int nfinger, lastid;
 };
 
 ////////////////////////////////////////////////////////