1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
|
#include <math.h>
#include <stdio.h>
#include "gm.h"
static wii_pt bounds[2][4];
static int g_calibrated[4];
double get_dist( wii_pt *p1, wii_pt *p2 ) {
double d = (p1->x - p2->x)*(p1->x - p2->x) +
(p1->y - p2->y)*(p1->y - p2->y);
return sqrt(d);
}
/* This call stores information about the expected
rectangle for each wii
*/
void wii_calibrate( int wii_id, wii_pt *coords ) {
double max_dist = 0.0;
int pt1 = 0, pt2 = 0, i, j;
/* Points with maximal distance span a rectangle */
for( i=0; i<4; ++i )
for( j=1; j<4; ++j ) {
double dist = get_dist( coords+i, coords+j );
if( dist > max_dist ) {
pt1 = i; pt2 = j;
max_dist = dist;
}
}
if( coords[pt1].x > coords[pt2].x ) {
bounds[0][wii_id] = coords[pt1];
bounds[1][wii_id] = coords[pt2];
} else {
bounds[1][wii_id] = coords[pt1];
bounds[0][wii_id] = coords[pt2];
}
g_calibrated[wii_id] = 1;
}
int wii_point_to_led( int wii_id, wii_pt *coords ) {
// Ick nenn die immer so.
const double x0 = bounds[0][wii_id].x;
const double y0 = bounds[0][wii_id].y;
const double x1 = bounds[1][wii_id].x;
const double y1 = bounds[1][wii_id].y;
const double xp = coords->x;
const double yp = coords->y;
//printf( "%lf:%lf ", xp, yp );
// Where m is slope of orthogonal line
// And n is square of diagonal of bounding box
double m = (x1-x0)*(xp-x0)+(y1-y0)*(yp-y0);
double n = (x1-x0)*(x1-x0)+(y1-y0)*(y1-y0);
// To find out distance of coord to our base line
double q = (xp-x0)*(y1-y0)-(yp-y0)*(x1-x0);
//printf( "%lf %lf %lf\n", m, n, q );
double offs_on_line = m/n;
double dist_to_line = q/n;
// If wii is not calibrated, we cannot detect point
if( !g_calibrated[wii_id] )
return -1;
// Point is too far away to be considered a match
if( ( dist_to_line > 0.1 ) || ( dist_to_line < -0.1 ) ) {
// puts( "too far " );
return -1;
}
// printf( "%1.3lf\n", offs_on_line );
// Check, which segment our line hits
if( ( offs_on_line > (-1.0/6.0) ) && ( offs_on_line <= (1.0/6.0) ) )
return 0;
if( ( offs_on_line > ( 1.0/6.0) ) && ( offs_on_line <= (3.0/6.0) ) )
return 1;
if( ( offs_on_line > ( 3.0/6.0) ) && ( offs_on_line <= (5.0/6.0) ) )
return 2;
if( ( offs_on_line > ( 5.0/6.0) ) && ( offs_on_line <= (7.0/6.0) ) )
return 3;
// No match!
return -1;
}
|
