aboutsummaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
Diffstat
-rw-r--r--Makefile15
-rw-r--r--match/match.c326
-rw-r--r--match/match.h25
-rw-r--r--match/test.c40
-rw-r--r--src/common.h3
-rw-r--r--src/hwdata.h1
-rw-r--r--src/multitouch.c2
-rw-r--r--src/state.c72
-rw-r--r--src/state.h3
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;
};
////////////////////////////////////////////////////////
generated by cgit v1.2.3 (git 2.25.1) at 2024-06-10 23:00:43 +0000