First cut of calibration, works but clips.
authorRusty Russell <rusty@rustcorp.com.au>
Sun, 20 Jan 2008 08:14:43 +0000 (19:14 +1100)
committerRusty Russell <rusty@rustcorp.com.au>
Sun, 20 Jan 2008 08:14:43 +0000 (19:14 +1100)
prpong.c

index 36b297e600d02c3c8c774b922d28a64e8d01e079..b814c436516e2472e3c422a472f92b6168edcc0b 100644 (file)
--- a/prpong.c
+++ b/prpong.c
 #include <SDL/SDL.h>
 #include <cwiid.h>
 #include <assert.h>
+#include <unistd.h>
 
 #define MAX_X CWIID_IR_X_MAX
 #define MAX_Y CWIID_IR_Y_MAX
 
+#define CALIB_OFF 50
+
 struct coord
 {
        double x;
@@ -648,6 +651,130 @@ static void clear_ignore(struct list_head *lines)
                i->ignore = false;
 }
 
+static void make_cross(unsigned x, unsigned y,
+                      struct line_segment *seg1,
+                      struct line_segment *seg2)
+{
+       seg1->start.x = x-5;
+       seg1->start.y = y-5;
+       seg1->end.x = x+5;
+       seg1->end.y = y+5;
+       seg2->start.x = x-5;
+       seg2->start.y = y+5;
+       seg2->end.x = x+5;
+       seg2->end.y = y-5;
+}
+
+static void calibrate(SDL_Surface *screen,
+                     cwiid_wiimote_t *wiimote,
+                     struct coord *calib, unsigned x, unsigned y)
+{
+       struct line_segment seg1, seg2;
+       struct cwiid_state state;
+       unsigned int i;
+
+       make_cross(x, y, &seg1, &seg2);
+       thick_line(screen, &seg1, 0);
+       thick_line(screen, &seg2, 0);
+
+       do {
+               SDL_Event event;
+               if (cwiid_get_state(wiimote, &state))
+                       errx(1, "No wii state");
+
+               for (i = 0; i < CWIID_IR_SRC_COUNT; i++) {
+                       if (state.ir_src[i].valid)
+                               break;
+               }
+               SDL_Delay(25);
+               if (SDL_PollEvent(&event)
+                   && (event.type == SDL_QUIT
+                       || (event.type == SDL_KEYDOWN
+                           && event.key.keysym.sym == SDLK_ESCAPE))) {
+                       SDL_Quit();
+                       exit(0);
+               }
+       } while (i == CWIID_IR_SRC_COUNT);
+
+       thick_line(screen, &seg1, 0xFFFFFFFF);
+       thick_line(screen, &seg2, 0xFFFFFFFF);
+
+       calib->x = state.ir_src[i].pos[0];
+       calib->y = state.ir_src[i].pos[1];
+       printf("Calibration point: %u,%u\n",
+              state.ir_src[i].pos[0],
+              state.ir_src[i].pos[1]);
+       make_cross(calib->x, calib->y, &seg1, &seg2);
+
+       thick_line(screen, &seg1, 0xFFFF0000);
+       thick_line(screen, &seg2, 0xFFFF0000);
+}
+
+static bool map_coord(unsigned int x, unsigned int y,
+                     const struct coord calib[],
+                     struct coord *res)
+{
+       struct coord line_start, line_end;
+       struct coord pos = { x, y }, intersect;
+       double d1, d2;
+
+       /* Calibration layout:
+        *
+        *   0    1
+        *
+        *   3    2
+        *
+        * We figure out the distance to each side, and then map using:
+        *
+        * x = d1 / (d1 + d2) * (rawend - rawstart) + rawstart
+        */
+       line_start.x = 0;
+       line_end.x = MAX_X-1;
+       line_start.y = line_end.y = y;
+
+       if (!lines_intersect(&calib[0], &calib[3], &line_start, &line_end,
+                            &intersect))
+               return false;
+       d1 = dist(&pos, &intersect);
+       if (!lines_intersect(&calib[1], &calib[2], &line_start, &line_end,
+                            &intersect))
+               return false;
+       d2 = dist(&pos, &intersect);
+       res->x = d1 / (d1 + d2) * (MAX_X - 2*CALIB_OFF) + CALIB_OFF;
+
+       line_start.y = 0;
+       line_end.y = MAX_Y-1;
+       line_start.x = line_end.x = x;
+
+       if (!lines_intersect(&calib[0], &calib[1], &line_start, &line_end,
+                            &intersect))
+               return false;
+       d1 = dist(&pos, &intersect);
+       if (!lines_intersect(&calib[3], &calib[2], &line_start, &line_end,
+                            &intersect))
+               return false;
+       d2 = dist(&pos, &intersect);
+       res->y = d1 / (d1 + d2) * MAX_Y;
+
+       return true;
+}
+
+static struct line_segment *new_line(unsigned int startx, unsigned int starty,
+                                    unsigned int endx, unsigned int endy,
+                                    const struct coord calib[])
+{
+       struct line_segment *l = malloc(sizeof(*l));
+
+       l->ignore = false;
+       if (!map_coord(startx, starty, calib, &l->start)
+           || !map_coord(endx, endy, calib, &l->end)) {
+               free(l);
+               return NULL;
+       }
+       return l;
+}
+
+
 int main(int argc, char *argv[])
 {
        struct ball ball;
@@ -659,20 +786,34 @@ int main(int argc, char *argv[])
        struct timeval line_life = { 3, 0 };
        cwiid_wiimote_t *wiimote;
        struct cwiid_state state;
+       struct coord calib[4];
        bdaddr_t addr = *BDADDR_ANY;
+       bool needs_calibration = true;
        LIST_HEAD(lines);
        struct cwiid_ir_src *ir, last_ir = { .valid = 0 };
 
        videoflags = SDL_SWSURFACE;
 
        if (argv[1] && streq(argv[1], "--fullscreen")) {
-               videoflags = SDL_SWSURFACE;
+               videoflags |= SDL_FULLSCREEN;
+               argc--;
+               argv++;
+       }
+
+       if (argv[1] && strstarts(argv[1], "--calib=")) {
+               needs_calibration = false;
+               if (sscanf(argv[1], "--calib=%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf",
+                          &calib[0].x, &calib[0].y,
+                          &calib[1].x, &calib[1].y,
+                          &calib[2].x, &calib[2].y,
+                          &calib[3].x, &calib[3].y) != 8)
+                       errx(1, "Not enough calibration points");
                argc--;
                argv++;
        }
 
        if (argc != 5)
-               errx(1, "Usage: %s [--fullscreen] ballx bally ballangle ballspeed\n"
+               errx(1, "Usage: %s [--fullscreen] [--calib=x,y,x,y,x,y,x,y] ballx bally ballangle ballspeed\n"
                     "   Where ballangle is 0 for north, 90 for east, etc\n",
                        argv[0]);
 
@@ -714,6 +855,20 @@ int main(int argc, char *argv[])
        SDL_UnlockSurface(screen);
        SDL_UpdateRect(screen, 0, 0, 0, 0);
 
+       if (needs_calibration) {
+               /* Calibration */
+               calibrate(screen, wiimote, &calib[0], CALIB_OFF, 6);
+               sleep(1);
+               calibrate(screen, wiimote, &calib[1],
+                         MAX_X - 1 - CALIB_OFF, 6);
+               sleep(1);
+               calibrate(screen, wiimote, &calib[2],
+                         MAX_X - 1 - CALIB_OFF, MAX_Y - 7);
+               sleep(1);
+               calibrate(screen, wiimote, &calib[3],
+                         CALIB_OFF, MAX_Y - 7);
+       }
+
        /* Draw borders, put them in list. */
        for (i = 0; i < ARRAY_SIZE(border); i++) {
                border[i].ignore = false;
@@ -834,16 +989,18 @@ int main(int argc, char *argv[])
                if (ir->valid) {
                        /* Give it some slack for missing one or two... */
                        if (time_since_last_ir <= 5) {
-                               struct line_segment *n = malloc(sizeof(*n));
-                               /* Wiimote coordinates are backwards for us. */
-                               n->start.x = last_ir.pos[0];
-                               n->start.y = MAX_Y - last_ir.pos[1];
-                               n->end.x = ir->pos[0];
-                               n->end.y = MAX_Y - ir->pos[1];
-                               n->ignore = false;
-                               timeradd(&now, &line_life, &n->expires);
-                               list_add_tail(&lines, &n->list);
-                               thick_line(screen, n, 0);
+                               struct line_segment *n;
+
+                               n = new_line(last_ir.pos[0],
+                                            last_ir.pos[1],
+                                            ir->pos[0],
+                                            ir->pos[1],
+                                            calib);
+                               if (n) {
+                                       timeradd(&now, &line_life, &n->expires);
+                                       list_add_tail(&lines, &n->list);
+                                       thick_line(screen, n, 0);
+                               }
                        }
                        time_since_last_ir = 0;
                        last_ir = *ir;