diff options
Diffstat (limited to 'src/match.c')
-rw-r--r-- | src/match.c | 390 |
1 files changed, 390 insertions, 0 deletions
diff --git a/src/match.c b/src/match.c new file mode 100644 index 0000000..4247e5a --- /dev/null +++ b/src/match.c @@ -0,0 +1,390 @@ +/***************************************************************************** + * + * mtdev - Multitouch Protocol Translation Library (MIT license) + * + * Copyright (C) 2010 Henrik Rydberg <rydberg@euromail.se> + * Copyright (C) 2010 Canonical Ltd. + * + * Permission is hereby granted, free of charge, to any person obtaining a + * copy of this software and associated documentation files (the "Software"), + * to deal in the Software without restriction, including without limitation + * the rights to use, copy, modify, merge, publish, distribute, sublicense, + * and/or sell copies of the Software, and to permit persons to whom the + * Software is furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice (including the next + * paragraph) shall be included in all copies or substantial portions of the + * Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL + * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER + * DEALINGS IN THE SOFTWARE. + * + ****************************************************************************/ + +#include "match.h" +#include <string.h> +#include <stdio.h> + +/** + * Bitmap implementation of the hungarian algorithm (MIT license) + * + * Copyright (C) 2008 Henrik Rydberg <rydberg@euromail.se> + * + * Based on code released by Markus Buehren (2004) (BSD license) + * + * Copyright (C) 2004, Markus Buehren. All rights reserved. + */ + +typedef unsigned col_t[1]; +typedef unsigned mat_t[DIM_FINGER]; + +#define GET1(m, x) ((m[0] >> (x)) & 1U) +#define SET1(m, x) (m[0] |= (1U << (x))) +#define CLEAR1(m, x) (m[0] &= ~(1U << (x))) + +#define GET2(m, row, col) ((m[col] >> (row)) & 1U) +#define SET2(m, row, col) (m[col] |= (1U << (row))) +#define CLEAR2(m, row, col) (m[col] &= ~(1U << (row))) + +/********************************************************/ + +static void buildixvector(int *ix, mat_t mstar, int nrows, int ncols) +{ + int row, col; + for (row = 0; row < nrows; row++) { + for (col = 0; col < ncols; col++) { + if (GET2(mstar, row, col)) { + ix[row] = col; + break; + } + } + } +} + + +/********************************************************/ + +static void step2a(int *ix, int *mdist, mat_t mstar, mat_t nmstar, + mat_t mprime, col_t ccol, col_t crow, int nrows, int ncols, + int dmin); +static void step2b(int *ix, int *mdist, mat_t mstar, mat_t nmstar, + mat_t mprime, col_t ccol, col_t crow, int nrows, int ncols, + int dmin); +static void step3(int *ix, int *mdist, mat_t mstar, mat_t nmstar, + mat_t mprime, col_t ccol, col_t crow, int nrows, int ncols, + int dmin); +static void step4(int *ix, int *mdist, mat_t mstar, mat_t nmstar, + mat_t mprime, col_t ccol, col_t crow, int nrows, int ncols, + int dmin, int row, int col); +static void step5(int *ix, int *mdist, mat_t mstar, mat_t nmstar, + mat_t mprime, col_t ccol, col_t crow, int nrows, int ncols, + int dmin); + +static void ixoptimal(int *ix, int *mdist, int nrows, int ncols) +{ + int *mdistTemp, *mdistEnd, *columnEnd, value, minValue; + int dmin, row, col; + col_t ccol, crow; + mat_t mstar, mprime, nmstar; + + memset(ccol, 0, sizeof(col_t)); + memset(crow, 0, sizeof(col_t)); + memset(mstar, 0, sizeof(mat_t)); + memset(mprime, 0, sizeof(mat_t)); + memset(nmstar, 0, sizeof(mat_t)); + + /* initialization */ + for (row = 0; row < nrows; row++) + ix[row] = -1; + + mdistEnd = mdist + nrows * ncols; + + /* 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) + continue; + if (GET1(ccol, col)) + continue; + SET2(mstar, row, col); + SET1(ccol, col); + break; + } + } + } else { + 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) + continue; + if (GET1(crow, row)) + continue; + SET2(mstar, row, col); + SET1(ccol, col); + SET1(crow, row); + break; + } + } + memset(crow, 0, sizeof(col_t)); + } + + /* move to step 2b */ + step2b(ix, mdist, mstar, nmstar, + mprime, ccol, crow, nrows, ncols, + dmin); +} + +/********************************************************/ +static void step2a(int *ix, int *mdist, mat_t mstar, mat_t nmstar, + mat_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 = 0; row < nrows; row++) { + if (!GET2(mstar, row, col)) + continue; + SET1(ccol, col); + break; + } + } + + /* move to step 3 */ + step2b(ix, mdist, mstar, nmstar, + mprime, ccol, crow, nrows, ncols, + dmin); +} + +/********************************************************/ +static void step2b(int *ix, int *mdist, mat_t mstar, mat_t nmstar, + mat_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 (GET1(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, int *mdist, mat_t mstar, mat_t nmstar, + mat_t mprime, col_t ccol, col_t crow, int nrows, int ncols, + int dmin) +{ + int zerosFound; + int row, col, cstar; + + zerosFound = 1; + while (zerosFound) { + zerosFound = 0; + for (col = 0; col < ncols; col++) { + if (GET1(ccol, col)) + continue; + for (row = 0; row < nrows; row++) { + if (mdist[row + nrows * col] != 0) + continue; + if (GET1(crow, row)) + continue; + + /* prime zero */ + SET2(mprime, row, col); + + /* find starred zero in current row */ + for (cstar = 0; cstar < ncols; cstar++) + if (GET2(mstar, row, cstar)) + break; + + if (cstar == ncols) { /* no starred zero */ + /* move to step 4 */ + step4(ix, mdist, mstar, nmstar, + mprime, ccol, crow, nrows, ncols, + dmin, row, col); + return; + } else { + SET1(crow, row); + CLEAR1(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, int *mdist, mat_t mstar, mat_t nmstar, + mat_t mprime, col_t ccol, col_t crow, int nrows, int ncols, + int dmin, int row, int col) +{ + int rstar, cstar, primeRow, primeCol; + + /* generate temporary copy of mstar */ + memcpy(nmstar, mstar, sizeof(mat_t)); + + /* star current zero */ + SET2(nmstar, row, col); + + /* find starred zero in current column */ + cstar = col; + for (rstar = 0; rstar < nrows; rstar++) + if (GET2(mstar, rstar, cstar)) + break; + + while (rstar < nrows) { + /* unstar the starred zero */ + CLEAR2(nmstar, rstar, cstar); + + /* find primed zero in current row */ + primeRow = rstar; + for (primeCol = 0; primeCol < ncols; primeCol++) + if (GET2(mprime, primeRow, primeCol)) + break; + + /* star the primed zero */ + SET2(nmstar, primeRow, primeCol); + + /* find starred zero in current column */ + cstar = primeCol; + for (rstar = 0; rstar < nrows; rstar++) + if (GET2(mstar, rstar, cstar)) + break; + } + + /* use temporary copy as new mstar */ + /* delete all primes, uncover all rows */ + memcpy(mstar, nmstar, sizeof(mat_t)); + memset(mprime, 0, sizeof(mat_t)); + memset(crow, 0, sizeof(col_t)); + + /* move to step 2a */ + step2a(ix, mdist, mstar, nmstar, + mprime, ccol, crow, nrows, ncols, + dmin); +} + +/********************************************************/ +static void step5(int *ix, int *mdist, mat_t mstar, mat_t nmstar, + mat_t mprime, col_t ccol, col_t crow, int nrows, int ncols, + int dmin) +{ + int h = 0, value; + int row, col, found = 0; + + /* find smallest uncovered element h */ + for (row = 0; row < nrows; row++) { + if (GET1(crow, row)) + continue; + for (col = 0; col < ncols; col++) { + if (GET1(ccol, col)) + continue; + value = mdist[row + nrows * col]; + if (!found || value < h) { + h = value; + found = 1; + } + } + } + + /* where to go if nothing uncovered? */ + if (!found) + return; + + /* add h to each covered row */ + for (row = 0; row < nrows; row++) { + if (!GET1(crow, row)) + continue; + for (col = 0; col < ncols; col++) + mdist[row + nrows * col] += h; + } + + /* subtract h from each uncovered column */ + for (col = 0; col < ncols; col++) { + if (GET1(ccol, col)) + continue; + 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 mtdev_match(int ix[DIM_FINGER], int A[DIM2_FINGER], int nrow, int ncol) +{ + ixoptimal(ix, A, nrow, ncol); +} + |