summaryrefslogtreecommitdiff
path: root/src/match.c
diff options
context:
space:
mode:
Diffstat (limited to 'src/match.c')
-rw-r--r--src/match.c390
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);
+}
+
View Lorry Controller Status