KickoffHEADmaster

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 @@
+#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;
+}