1 files changed, 392 insertions, 0 deletions

diff --git a/src/match.c b/src/match.c
new file mode 100644
index 0000000..1b3700c
--- /dev/null
+++ b/src/match.c
@@ -0,0 +1,392 @@
+/*****************************************************************************
+ *
+ * mtdev - MT device event converter (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 (GPL 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.
+ * See CREDITS file for full license terms.
+ *
+ */
+
+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 n, 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 match_fingers(int ix[DIM_FINGER], int A[DIM2_FINGER], int nrow, int ncol)
+{
+        ixoptimal(ix, A, nrow, ncol);
+}
+