/*****************************************************************************
*
* 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);
}