1 /*****************************************************************************
2 *
3 * mtdev - Multitouch Protocol Translation Library (MIT license)
4 *
5 * Copyright (C) 2010 Henrik Rydberg <rydberg@euromail.se>
6 * Copyright (C) 2010 Canonical Ltd.
7 *
8 * Permission is hereby granted, free of charge, to any person obtaining a
9 * copy of this software and associated documentation files (the "Software"),
10 * to deal in the Software without restriction, including without limitation
11 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
12 * and/or sell copies of the Software, and to permit persons to whom the
13 * Software is furnished to do so, subject to the following conditions:
14 *
15 * The above copyright notice and this permission notice (including the next
16 * paragraph) shall be included in all copies or substantial portions of the
17 * Software.
18 *
19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
20 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
22 * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
23 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
24 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
25 * DEALINGS IN THE SOFTWARE.
26 *
27 ****************************************************************************/
28
29 #include "match.h"
30 #include <string.h>
31 #include <stdio.h>
32
33 /**
34 * Bitmap implementation of the hungarian algorithm (MIT license)
35 *
36 * Copyright (C) 2008 Henrik Rydberg <rydberg@euromail.se>
37 *
38 * Based on code released by Markus Buehren (2004) (BSD license)
39 *
40 * Copyright (C) 2004, Markus Buehren. All rights reserved.
41 */
42
43 typedef unsigned col_t[1];
44 typedef unsigned mat_t[DIM_FINGER];
45
46 #define GET1(m, x) ((m[0] >> (x)) & 1U)
47 #define SET1(m, x) (m[0] |= (1U << (x)))
48 #define CLEAR1(m, x) (m[0] &= ~(1U << (x)))
49
50 #define GET2(m, row, col) ((m[col] >> (row)) & 1U)
51 #define SET2(m, row, col) (m[col] |= (1U << (row)))
52 #define CLEAR2(m, row, col) (m[col] &= ~(1U << (row)))
53
54 /********************************************************/
55
buildixvector(int * ix,mat_t mstar,int nrows,int ncols)56 static void buildixvector(int *ix, mat_t mstar, int nrows, int ncols)
57 {
58 int row, col;
59 for (row = 0; row < nrows; row++) {
60 for (col = 0; col < ncols; col++) {
61 if (GET2(mstar, row, col)) {
62 ix[row] = col;
63 break;
64 }
65 }
66 }
67 }
68
69
70 /********************************************************/
71
72 static void step2a(int *ix, int *mdist, mat_t mstar, mat_t nmstar,
73 mat_t mprime, col_t ccol, col_t crow, int nrows, int ncols,
74 int dmin);
75 static void step2b(int *ix, int *mdist, mat_t mstar, mat_t nmstar,
76 mat_t mprime, col_t ccol, col_t crow, int nrows, int ncols,
77 int dmin);
78 static void step3(int *ix, int *mdist, mat_t mstar, mat_t nmstar,
79 mat_t mprime, col_t ccol, col_t crow, int nrows, int ncols,
80 int dmin);
81 static void step4(int *ix, int *mdist, mat_t mstar, mat_t nmstar,
82 mat_t mprime, col_t ccol, col_t crow, int nrows, int ncols,
83 int dmin, int row, int col);
84 static void step5(int *ix, int *mdist, mat_t mstar, mat_t nmstar,
85 mat_t mprime, col_t ccol, col_t crow, int nrows, int ncols,
86 int dmin);
87
ixoptimal(int * ix,int * mdist,int nrows,int ncols)88 static void ixoptimal(int *ix, int *mdist, int nrows, int ncols)
89 {
90 int *mdistTemp, *mdistEnd, *columnEnd, value, minValue;
91 int dmin, row, col;
92 col_t ccol, crow;
93 mat_t mstar, mprime, nmstar;
94
95 memset(ccol, 0, sizeof(col_t));
96 memset(crow, 0, sizeof(col_t));
97 memset(mstar, 0, sizeof(mat_t));
98 memset(mprime, 0, sizeof(mat_t));
99 memset(nmstar, 0, sizeof(mat_t));
100
101 /* initialization */
102 for (row = 0; row < nrows; row++)
103 ix[row] = -1;
104
105 mdistEnd = mdist + nrows * ncols;
106
107 /* preliminary steps */
108 if (nrows <= ncols) {
109 dmin = nrows;
110
111 for (row = 0; row < nrows; row++) {
112 /* find the smallest element in the row */
113 mdistTemp = mdist + row;
114 minValue = *mdistTemp;
115 mdistTemp += nrows;
116 while (mdistTemp < mdistEnd) {
117 value = *mdistTemp;
118 if (value < minValue)
119 minValue = value;
120 mdistTemp += nrows;
121 }
122
123 /* subtract the smallest element from each element
124 of the row */
125 mdistTemp = mdist + row;
126 while (mdistTemp < mdistEnd) {
127 *mdistTemp -= minValue;
128 mdistTemp += nrows;
129 }
130 }
131
132 /* Steps 1 and 2a */
133 for (row = 0; row < nrows; row++) {
134 for (col = 0; col < ncols; col++) {
135 if (mdist[row + nrows * col] != 0)
136 continue;
137 if (GET1(ccol, col))
138 continue;
139 SET2(mstar, row, col);
140 SET1(ccol, col);
141 break;
142 }
143 }
144 } else {
145 dmin = ncols;
146
147 for (col = 0; col < ncols; col++) {
148 /* find the smallest element in the column */
149 mdistTemp = mdist + nrows*col;
150 columnEnd = mdistTemp + nrows;
151
152 minValue = *mdistTemp++;
153 while (mdistTemp < columnEnd) {
154 value = *mdistTemp++;
155 if (value < minValue)
156 minValue = value;
157 }
158
159 /* subtract the smallest element from each element
160 of the column */
161 mdistTemp = mdist + nrows*col;
162 while (mdistTemp < columnEnd)
163 *mdistTemp++ -= minValue;
164 }
165
166 /* Steps 1 and 2a */
167 for (col = 0; col < ncols; col++) {
168 for (row = 0; row < nrows; row++) {
169 if (mdist[row + nrows * col] != 0)
170 continue;
171 if (GET1(crow, row))
172 continue;
173 SET2(mstar, row, col);
174 SET1(ccol, col);
175 SET1(crow, row);
176 break;
177 }
178 }
179 memset(crow, 0, sizeof(col_t));
180 }
181
182 /* move to step 2b */
183 step2b(ix, mdist, mstar, nmstar,
184 mprime, ccol, crow, nrows, ncols,
185 dmin);
186 }
187
188 /********************************************************/
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)189 static void step2a(int *ix, int *mdist, mat_t mstar, mat_t nmstar,
190 mat_t mprime, col_t ccol, col_t crow, int nrows, int ncols,
191 int dmin)
192 {
193 int col, row;
194
195 /* cover every column containing a starred zero */
196 for (col = 0; col < ncols; col++) {
197 for (row = 0; row < nrows; row++) {
198 if (!GET2(mstar, row, col))
199 continue;
200 SET1(ccol, col);
201 break;
202 }
203 }
204
205 /* move to step 3 */
206 step2b(ix, mdist, mstar, nmstar,
207 mprime, ccol, crow, nrows, ncols,
208 dmin);
209 }
210
211 /********************************************************/
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)212 static void step2b(int *ix, int *mdist, mat_t mstar, mat_t nmstar,
213 mat_t mprime, col_t ccol, col_t crow, int nrows, int ncols,
214 int dmin)
215 {
216 int col, ncc;
217
218 /* count covered columns */
219 ncc = 0;
220 for (col = 0; col < ncols; col++)
221 if (GET1(ccol, col))
222 ncc++;
223
224 if (ncc == dmin) {
225 /* algorithm finished */
226 buildixvector(ix, mstar, nrows, ncols);
227 } else {
228 /* move to step 3 */
229 step3(ix, mdist, mstar, nmstar,
230 mprime, ccol, crow, nrows, ncols,
231 dmin);
232 }
233
234 }
235
236 /********************************************************/
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)237 static void step3(int *ix, int *mdist, mat_t mstar, mat_t nmstar,
238 mat_t mprime, col_t ccol, col_t crow, int nrows, int ncols,
239 int dmin)
240 {
241 int zerosFound;
242 int row, col, cstar;
243
244 zerosFound = 1;
245 while (zerosFound) {
246 zerosFound = 0;
247 for (col = 0; col < ncols; col++) {
248 if (GET1(ccol, col))
249 continue;
250 for (row = 0; row < nrows; row++) {
251 if (mdist[row + nrows * col] != 0)
252 continue;
253 if (GET1(crow, row))
254 continue;
255
256 /* prime zero */
257 SET2(mprime, row, col);
258
259 /* find starred zero in current row */
260 for (cstar = 0; cstar < ncols; cstar++)
261 if (GET2(mstar, row, cstar))
262 break;
263
264 if (cstar == ncols) { /* no starred zero */
265 /* move to step 4 */
266 step4(ix, mdist, mstar, nmstar,
267 mprime, ccol, crow, nrows, ncols,
268 dmin, row, col);
269 return;
270 } else {
271 SET1(crow, row);
272 CLEAR1(ccol, cstar);
273 zerosFound = 1;
274 break;
275 }
276 }
277 }
278 }
279
280 /* move to step 5 */
281 step5(ix, mdist, mstar, nmstar,
282 mprime, ccol, crow, nrows, ncols,
283 dmin);
284 }
285
286 /********************************************************/
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)287 static void step4(int *ix, int *mdist, mat_t mstar, mat_t nmstar,
288 mat_t mprime, col_t ccol, col_t crow, int nrows, int ncols,
289 int dmin, int row, int col)
290 {
291 int rstar, cstar, primeRow, primeCol;
292
293 /* generate temporary copy of mstar */
294 memcpy(nmstar, mstar, sizeof(mat_t));
295
296 /* star current zero */
297 SET2(nmstar, row, col);
298
299 /* find starred zero in current column */
300 cstar = col;
301 for (rstar = 0; rstar < nrows; rstar++)
302 if (GET2(mstar, rstar, cstar))
303 break;
304
305 while (rstar < nrows) {
306 /* unstar the starred zero */
307 CLEAR2(nmstar, rstar, cstar);
308
309 /* find primed zero in current row */
310 primeRow = rstar;
311 for (primeCol = 0; primeCol < ncols; primeCol++)
312 if (GET2(mprime, primeRow, primeCol))
313 break;
314
315 /* star the primed zero */
316 SET2(nmstar, primeRow, primeCol);
317
318 /* find starred zero in current column */
319 cstar = primeCol;
320 for (rstar = 0; rstar < nrows; rstar++)
321 if (GET2(mstar, rstar, cstar))
322 break;
323 }
324
325 /* use temporary copy as new mstar */
326 /* delete all primes, uncover all rows */
327 memcpy(mstar, nmstar, sizeof(mat_t));
328 memset(mprime, 0, sizeof(mat_t));
329 memset(crow, 0, sizeof(col_t));
330
331 /* move to step 2a */
332 step2a(ix, mdist, mstar, nmstar,
333 mprime, ccol, crow, nrows, ncols,
334 dmin);
335 }
336
337 /********************************************************/
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)338 static void step5(int *ix, int *mdist, mat_t mstar, mat_t nmstar,
339 mat_t mprime, col_t ccol, col_t crow, int nrows, int ncols,
340 int dmin)
341 {
342 int h = 0, value;
343 int row, col, found = 0;
344
345 /* find smallest uncovered element h */
346 for (row = 0; row < nrows; row++) {
347 if (GET1(crow, row))
348 continue;
349 for (col = 0; col < ncols; col++) {
350 if (GET1(ccol, col))
351 continue;
352 value = mdist[row + nrows * col];
353 if (!found || value < h) {
354 h = value;
355 found = 1;
356 }
357 }
358 }
359
360 /* where to go if nothing uncovered? */
361 if (!found)
362 return;
363
364 /* add h to each covered row */
365 for (row = 0; row < nrows; row++) {
366 if (!GET1(crow, row))
367 continue;
368 for (col = 0; col < ncols; col++)
369 mdist[row + nrows * col] += h;
370 }
371
372 /* subtract h from each uncovered column */
373 for (col = 0; col < ncols; col++) {
374 if (GET1(ccol, col))
375 continue;
376 for (row = 0; row < nrows; row++)
377 mdist[row + nrows * col] -= h;
378 }
379
380 /* move to step 3 */
381 step3(ix, mdist, mstar, nmstar,
382 mprime, ccol, crow, nrows, ncols,
383 dmin);
384 }
385
mtdev_match(int ix[DIM_FINGER],int A[DIM2_FINGER],int nrow,int ncol)386 void mtdev_match(int ix[DIM_FINGER], int A[DIM2_FINGER], int nrow, int ncol)
387 {
388 ixoptimal(ix, A, nrow, ncol);
389 }
390
391