From 46812235bf758624b703e656539ccf14d8e2cd3f Mon Sep 17 00:00:00 2001 From: Rusty Russell Date: Sun, 20 Jan 2008 19:14:43 +1100 Subject: [PATCH] First cut of calibration, works but clips. --- prpong.c | 181 +++++++++++++++++++++++++++++++++++++++++++++++++++---- 1 file changed, 169 insertions(+), 12 deletions(-) diff --git a/prpong.c b/prpong.c index 36b297e..b814c43 100644 --- a/prpong.c +++ b/prpong.c @@ -12,10 +12,13 @@ #include #include #include +#include #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; -- 2.39.2