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