diff options
author | Henrik Rydberg <rydberg@euromail.se> | 2008-11-06 23:01:41 +0100 |
---|---|---|
committer | Henrik Rydberg <rydberg@euromail.se> | 2008-11-06 23:01:41 +0100 |
commit | d44b4794c679141a08fd802475bb542ecf5b7c51 (patch) | |
tree | 74251970cfbcc58d0252366be6e5e668149b8a48 | |
parent | 63de72d8d810e3692c96c7385b497bb4d68ef54a (diff) | |
download | xorg-input-kobomultitouch-d44b4794c679141a08fd802475bb542ecf5b7c51.tar.gz xorg-input-kobomultitouch-d44b4794c679141a08fd802475bb542ecf5b7c51.tar.bz2 xorg-input-kobomultitouch-d44b4794c679141a08fd802475bb542ecf5b7c51.zip |
ok, fast (but not fastest) matcher in place, no check output...
Signed-off-by: Henrik Rydberg <rydberg@euromail.se>
-rw-r--r-- | Makefile | 15 | ||||
-rw-r--r-- | match/match.c | 326 | ||||
-rw-r--r-- | match/match.h | 25 | ||||
-rw-r--r-- | match/test.c | 40 | ||||
-rw-r--r-- | src/common.h | 3 | ||||
-rw-r--r-- | src/hwdata.h | 1 | ||||
-rw-r--r-- | src/multitouch.c | 2 | ||||
-rw-r--r-- | src/state.c | 72 | ||||
-rw-r--r-- | src/state.h | 3 |
9 files changed, 473 insertions, 14 deletions
@@ -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; }; //////////////////////////////////////////////////////// |