11 #include "list/list.h"
17 #define MAX_X CWIID_IR_X_MAX
18 #define MAX_Y CWIID_IR_Y_MAX
26 /* Obtained from http://tog.acm.org/GraphicsGems/gemsiii/insectc.c */
27 /* Faster Line Segment Intersection */
28 /* Franklin Antonio */
29 static bool lines_intersect(const struct coord *start1,
30 const struct coord *start2,
31 const struct coord *start3,
32 const struct coord *start4,
33 struct coord *intersect)
35 double Ax,Bx,Cx,Ay,By,Cy,d,e,f,num;
38 Ax = start2->x - start1->x;
39 Bx = start3->x - start4->x;
41 if(Ax<0) { /* X bound box test*/
42 lo.x = start2->x; hi.x = start1->x;
44 hi.x = start2->x; lo.x = start1->x;
47 if(hi.x < start4->x || start3->x < lo.x) return false;
49 if(hi.x < start3->x || start4->x < lo.x) return false;
52 Ay = start2->y - start1->y;
53 By = start3->y - start4->y;
55 if(Ay<0) { /* Y bound box test*/
56 lo.y = start2->y; hi.y = start1->y;
58 hi.y = start2->y; lo.y = start1->y;
61 if(hi.y < start4->y || start3->y < lo.y) return false;
63 if(hi.y < start3->y || start4->y < lo.y) return false;
67 Cx = start1->x - start3->x;
68 Cy = start1->y - start3->y;
69 d = By*Cx - Bx*Cy; /* alpha numerator*/
70 f = Ay*Bx - Ax*By; /* both denominator*/
71 if (f>0) { /* alpha tests*/
72 if (d < 0 || d > f) return false;
74 if (d > 0 || d < f) return false;
77 e = Ax*Cy - Ay*Cx; /* beta numerator*/
78 if (f > 0) { /* beta tests*/
79 if (e < 0 || e > f) return false;
81 if (e > 0 || e < f) return false;
84 /*compute intersection coordinates*/
85 /* Don't worry about tiny values of f (< 1/2 pixel across screen) */
86 if (fabs(f) < 1.0 / (2.0 * MAX_X * MAX_Y))
89 num = d*Ax; /* numerator */
90 intersect->x = start1->x + num / f; /* intersection x */
93 intersect->y = start1->y + num / f; /* intersection y */
98 // A set of very useful macros that you will find in most
99 // code that I write whether I use them in a program or
102 #define abs(a) (((a)<0) ? -(a) : (a))
103 #define sign(a) (((a)<0) ? -1 : (a)>0 ? 1 : 0)
105 // The following code implements a Bresenham line drawing
106 // algorithm. There are 4 separate routines each optimized
107 // for one of the four pixel depths supported by SDL. SDL
108 // support many pixel formats, but it only support 8, 16,
109 // 24, and 32 bit pixels.
111 //----------------------------------------------------------
113 // Draw lines in 8 bit surfaces.
115 static void line8(SDL_Surface *s,
140 yOffset = sy * s->pitch;
145 lineAddr = ((Uint8 *)(s->pixels)) + (y * s->pitch);
151 if (y >= 0 && y < MAX_Y && x >= 0 && x < MAX_X)
152 *(lineAddr + x) = (Uint8)color;
173 if (y >= 0 && y < MAX_Y && x >= 0 && x < MAX_X)
174 *(lineAddr + x) = (Uint8)color;
192 //----------------------------------------------------------
194 // Draw lines in 16 bit surfaces. Note that this code will
195 // also work on 15 bit surfaces.
197 static void line16(SDL_Surface *s,
222 yOffset = sy * s->pitch;
227 lineAddr = ((Uint8 *)s->pixels) + (y * s->pitch);
233 if (y >= 0 && y < MAX_Y && x >= 0 && x < MAX_X)
234 *((Uint16 *)(lineAddr + (x << 1))) = (Uint16)color;
255 if (y >= 0 && y < MAX_Y && x >= 0 && x < MAX_X)
256 *((Uint16 *)(lineAddr + (x << 1))) = (Uint16)color;
274 //----------------------------------------------------------
276 // Draw lines in 24 bit surfaces. 24 bit surfaces require
277 // special handling because the pixels don't fall on even
278 // address boundaries. Instead of being able to store a
279 // single byte, word, or long you have to store 3
280 // individual bytes. As a result 24 bit graphics is slower
281 // than the other pixel sizes.
283 static void line24(SDL_Surface *s,
301 #if (SDL_BYTEORDER == SDL_BIG_ENDIAN)
312 yOffset = sy * s->pitch;
317 lineAddr = ((Uint8 *)(s->pixels)) + (y * s->pitch);
323 Uint8 *p = (lineAddr + (x * 3));
324 if (y >= 0 && y < MAX_Y && x >= 0 && x < MAX_X)
325 memcpy(p, &color, 3);
346 Uint8 *p = (lineAddr + (x * 3));
347 if (y >= 0 && y < MAX_Y && x >= 0 && x < MAX_X)
348 memcpy(p, &color, 3);
366 //----------------------------------------------------------
368 // Draw lines in 32 bit surfaces. Note that this routine
369 // ignores alpha values. It writes them into the surface
370 // if they are included in the pixel, but does nothing
373 static void line32(SDL_Surface *s,
398 yOffset = sy * s->pitch;
403 lineAddr = ((Uint8 *)(s->pixels)) + (y * s->pitch);
409 if (y >= 0 && y < MAX_Y && x >= 0 && x < MAX_X)
410 *((Uint32 *)(lineAddr + (x << 2))) = (Uint32)color;
431 if (y >= 0 && y < MAX_Y && x >= 0 && x < MAX_X)
432 *((Uint32 *)(lineAddr + (x << 2))) = (Uint32)color;
450 //----------------------------------------------------------
452 // Examine the depth of a surface and select a line
453 // drawing routine optimized for the bytes/pixel of the
456 static void line(SDL_Surface *s,
461 switch (s->format->BytesPerPixel)
464 line8(s, x1, y1, x2, y2, color);
467 line16(s, x1, y1, x2, y2, color);
470 line24(s, x1, y1, x2, y2, color);
473 line32(s, x1, y1, x2, y2, color);
486 struct coord start, end;
487 struct list_node list;
489 struct timeval expires;
492 static void thick_line(SDL_Surface *s, const struct line_segment *l,
495 struct line_segment adj;
496 int x, y, minx = MAX_X, miny = MAX_Y, maxx = 0, maxy = 0;
501 for (x = -1; x < 2; x++) {
502 for (y = -1; y < 2; y++) {
503 adj.start.x = l->start.x + x;
504 adj.end.x = l->end.x + x;
505 adj.start.y = l->start.y + y;
506 adj.end.y = l->end.y + y;
508 if (adj.start.x < minx)
510 if (adj.start.x > maxx)
512 if (adj.start.y < miny)
514 if (adj.start.y > maxy)
516 if (adj.end.x < minx)
518 if (adj.end.x > maxx)
520 if (adj.end.y < miny)
522 if (adj.end.y > maxy)
525 line(s, adj.start.x, adj.start.y, adj.end.x, adj.end.y,
538 SDL_UpdateRect(s, minx, miny, maxx-minx, maxy-miny);
539 SDL_UnlockSurface(s);
542 static bool intersect(const struct coord *c1, const struct coord *c2,
543 const struct line_segment *seg, struct coord *intersect)
545 return lines_intersect(c1, c2, &seg->start, &seg->end, intersect);
548 static double dist(const struct coord *c1, const struct coord *c2)
550 double x = (c1->x - c2->x), y = (c1->y - c2->y);
551 return sqrt(x * x + y * y);
554 static unsigned rad_to_deg(double rad)
556 return ((unsigned)(rad / M_PI * 180) + 360) % 360;
559 static void bounce(struct ball *ball, const struct line_segment *line,
560 struct coord *move, double increase_speed)
562 double len, speed, lang, iang, oang;
563 struct coord isect, new;
565 new.x = ball->pos.x + move->x;
566 new.y = ball->pos.y + move->y;
568 /* How far were we supposed to move ball? */
569 len = sqrt(move->x * move->x + move->y * move->y);
571 /* How far is it to intersection? */
572 if (!intersect(&ball->pos, &new, line, &isect))
573 errx(1, "No intersection any more?\n");;
574 len -= dist(&ball->pos, &isect);
576 /* Move ball to intersection. */
578 printf("ball is now at %f,%f\n", ball->pos.x, ball->pos.y);
580 /* Outgoing angle = 2 * line angle - incident angle. */
581 lang = atan2(line->end.x - line->start.x, line->end.y - line->start.y);
582 iang = atan2(ball->move.x, ball->move.y);
583 oang = 2 * lang - iang, 2*M_PI;
584 printf("lang = %u, iang = %u, oang=%u\n",
589 /* Set new direction for ball, at slightly increased speed */
590 speed = sqrt(ball->move.x * ball->move.x + ball->move.y * ball->move.y);
591 speed += increase_speed;
592 ball->move.x = sin(oang) * speed;
593 ball->move.y = cos(oang) * speed;
596 move->x = sin(oang) * len;
597 move->y = cos(oang) * len;
598 printf("len = %f, move = %f,%f\n", len, move->x, move->y);
601 static struct line_segment border[] = {
602 { { 0, 0, }, { MAX_X-1, 0 } },
603 { { MAX_X-1, 0, }, { MAX_X-1, MAX_Y-1 } },
604 { { MAX_X-1, MAX_Y-1, }, { 0, MAX_Y-1 } },
605 { { 0, MAX_Y-1, }, { 0, 0 } },
608 static inline float deg_to_rad(float degrees)
610 return degrees * M_PI / 180;
613 static SDL_Surface *sub_surface(SDL_Surface *screen, int w, int h)
615 return SDL_CreateRGBSurface(SDL_SWSURFACE, w, h,
616 screen->format->BitsPerPixel,
617 screen->format->Rmask,
618 screen->format->Gmask,
619 screen->format->Bmask,
620 screen->format->Amask);
623 static SDL_Surface *ball_surface(SDL_Surface *screen)
627 /* Just like the screen surface. */
628 ball = sub_surface(screen, 5, 5);
630 /* Lock the surface */
631 SDL_LockSurface(ball);
633 /* Black, white corners. */
634 memset(ball->pixels, 0x00, ball->pitch * 5);
635 line(ball, 0, 0, 0, 0, 0xFFFFFFFF);
636 line(ball, 4, 0, 4, 0, 0xFFFFFFFF);
637 line(ball, 0, 4, 0, 4, 0xFFFFFFFF);
638 line(ball, 4, 4, 4, 4, 0xFFFFFFFF);
639 SDL_UnlockSurface(ball);
644 static void clear_ignore(struct list_head *lines)
646 struct line_segment *i;
647 printf("Unignoring...\n");
648 list_for_each(lines, i, list)
652 static uint16_t *find_valid_point(struct cwiid_state *state)
656 for (i = 0; i < CWIID_IR_SRC_COUNT; i++) {
657 if (state->ir_src[i].valid)
658 return state->ir_src[i].pos;
663 static void calibrate(SDL_Surface *screen,
664 cwiid_wiimote_t *wiimote,
665 struct coord *calib, unsigned x, unsigned y)
668 struct cwiid_state state;
669 unsigned int last_x = MAX_X, last_y = MAX_Y, count = 0;
671 /* Must see it for a full half second. */
675 if (cwiid_get_state(wiimote, &state))
676 errx(1, "No wii state");
678 pos = find_valid_point(&state);
679 if (!pos || pos[0] != last_x || pos[1] != last_y)
689 if (SDL_PollEvent(&event)
690 && (event.type == SDL_QUIT
691 || (event.type == SDL_KEYDOWN
692 && event.key.keysym.sym == SDLK_ESCAPE))) {
700 printf("Calibration point: %u,%u\n", last_x, last_y);
703 static bool map_coord(unsigned int x, unsigned int y,
704 const struct coord calib[],
707 struct coord line_start, line_end;
708 struct coord pos = { x, y }, intersect;
711 /* Calibration layout:
717 * We figure out the distance to each side, and then map using:
719 * x = d1 / (d1 + d2) * (rawend - rawstart) + rawstart
722 line_end.x = MAX_X-1;
723 line_start.y = line_end.y = y;
725 if (!lines_intersect(&calib[0], &calib[3], &line_start, &line_end,
728 d1 = dist(&pos, &intersect);
729 if (!lines_intersect(&calib[1], &calib[2], &line_start, &line_end,
732 d2 = dist(&pos, &intersect);
733 res->x = d1 / (d1 + d2) * MAX_X;
736 line_end.y = MAX_Y-1;
737 line_start.x = line_end.x = x;
739 if (!lines_intersect(&calib[0], &calib[1], &line_start, &line_end,
742 d1 = dist(&pos, &intersect);
743 if (!lines_intersect(&calib[3], &calib[2], &line_start, &line_end,
746 d2 = dist(&pos, &intersect);
747 res->y = d1 / (d1 + d2) * MAX_Y;
752 static struct line_segment *new_line(unsigned int startx, unsigned int starty,
753 unsigned int endx, unsigned int endy,
754 const struct coord calib[])
756 struct line_segment *l = malloc(sizeof(*l));
759 if (!map_coord(startx, starty, calib, &l->start)
760 || !map_coord(endx, endy, calib, &l->end)) {
768 int main(int argc, char *argv[])
771 SDL_Surface *screen, *ballsur, *savesur = NULL;
774 unsigned int i, time_since_last_ir = INT_MAX;
776 struct timeval line_life = { 3, 0 };
777 cwiid_wiimote_t *wiimote;
778 struct cwiid_state state;
779 struct coord calib[4];
780 bdaddr_t addr = *BDADDR_ANY;
781 bool mouse = false, needs_calibration = true, drawing = false;
783 struct cwiid_ir_src *ir, last_ir = { .valid = 0 };
784 struct line_segment *n;
786 videoflags = SDL_SWSURFACE;
788 if (argv[1] && streq(argv[1], "--fullscreen")) {
789 videoflags |= SDL_FULLSCREEN;
794 if (argv[1] && strstarts(argv[1], "--calib=")) {
795 needs_calibration = false;
796 if (sscanf(argv[1], "--calib=%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf",
797 &calib[0].x, &calib[0].y,
798 &calib[1].x, &calib[1].y,
799 &calib[2].x, &calib[2].y,
800 &calib[3].x, &calib[3].y) != 8)
801 errx(1, "Not enough calibration points");
806 if (argv[1] && streq(argv[1], "--mouse")) {
808 needs_calibration = false;
809 calib[0].x = calib[0].y = 0;
810 calib[1].x = MAX_X - 1;
812 calib[2].x = MAX_X - 1;
813 calib[2].y = MAX_Y - 1;
815 calib[3].y = MAX_Y - 1;
821 errx(1, "Usage: %s [--fullscreen] [--calib=x,y,x,y,x,y,x,y] [--mouse] ballx bally ballangle ballspeed\n"
822 " Where ballangle is 0 for north, 90 for east, etc\n",
825 ball.pos.x = atol(argv[1]);
826 ball.pos.y = atol(argv[2]);
827 ball.move.x = sin(deg_to_rad(atof(argv[3]))) * atol(argv[4]);
828 ball.move.y = cos(deg_to_rad(atof(argv[3]))) * atol(argv[4]);
829 printf("ball move = %f,%f\n", ball.move.x, ball.move.y);
832 if (SDL_Init(SDL_INIT_VIDEO) < 0) {
833 fprintf(stderr, "Couldn't initialize SDL: %s\n",SDL_GetError());
840 printf("Put Wiimote in discoverable mode (press 1+2)\n");
841 wiimote = cwiid_open(&addr, 0);
843 errx(1, "Can't find the Wiimote");
845 if (cwiid_set_rpt_mode(wiimote, CWIID_RPT_IR))
846 errx(1, "Can't set IR repeat mode");
849 if ( (screen=SDL_SetVideoMode(MAX_X,MAX_Y,video_bpp,videoflags)) == NULL ) {
850 errx(1, "Couldn't set %dx%dx%d video mode: %s",
851 MAX_X, MAX_Y,video_bpp, SDL_GetError());
854 /* Set the surface pixels and refresh! */
855 if ( SDL_LockSurface(screen) < 0 ) {
856 errx(1, "Couldn't lock the display surface: %s",
860 memset(screen->pixels, 0xFF, screen->pitch * screen->h);
862 SDL_UnlockSurface(screen);
863 SDL_UpdateRect(screen, 0, 0, 0, 0);
865 if (needs_calibration) {
867 calibrate(screen, wiimote, &calib[0], 0, 0);
868 calibrate(screen, wiimote, &calib[1], MAX_X - 1, 0);
869 calibrate(screen, wiimote, &calib[2], MAX_X - 1, MAX_Y - 1);
870 calibrate(screen, wiimote, &calib[3], 0, MAX_Y - 1);
873 /* Draw borders, put them in list. */
874 for (i = 0; i < ARRAY_SIZE(border); i++) {
875 border[i].ignore = false;
876 border[i].expires.tv_sec = LONG_MAX;
877 border[i].expires.tv_usec = 0;
878 list_add(&lines, &border[i].list);
879 thick_line(screen, &border[i], 0);
882 ballsur = ball_surface(screen);
885 struct coord new, isect, move = ball.move;
886 struct line_segment *best_l, *l, *next;
890 bool ignored = false;
892 gettimeofday(&now, NULL);
895 list_for_each_safe(&lines, l, next, list) {
896 if (timercmp(&l->expires, &now, <)) {
897 printf("Deleting line %p\n", l);
899 /* FIXME: Undraw properly. */
900 thick_line(screen, l, 0xFFFFFFFF);
906 best_d = MAX_X + MAX_Y;
908 new.x = ball.pos.x + move.x;
909 new.y = ball.pos.y + move.y;
911 list_for_each(&lines, l, list) {
912 if (!l->ignore && intersect(&ball.pos, &new, l, &isect)) {
914 d = dist(&ball.pos, &isect);
923 printf("Ball bouncing off (%f,%f %f,%f)!\n",
924 best_l->start.x, best_l->start.y,
925 best_l->end.x, best_l->end.y);
926 /* Only increase speed on *first* bounce, to avoid
927 * mega speed increases. */
928 bounce(&ball, best_l, &move, ignored ? 0.0 : 0.1);
930 /* If we moved, stop ignoring lines. */
931 if (ignored && (move.x > 0.001 || move.y > 0.001))
932 clear_ignore(&lines);
934 /* Don't hit the same line twice. */
935 printf("Ignoring that line\n");
936 best_l->ignore = ignored = true;
940 if (ignored && (move.x > 0.001 || move.y > 0.001)) {
941 clear_ignore(&lines);
942 printf("Moving by %f,%f to %f,%f\n",
943 move.x, move.y, new.x, new.y);
946 /* Restore what was there before ball. */
948 SDL_BlitSurface(savesur, NULL, screen, &rect);
949 SDL_UpdateRect(screen, rect.x, rect.y, rect.w, rect.h);
951 savesur = sub_surface(screen, ballsur->w, ballsur->h);
954 /* Save away image under new ball. */
957 rect.x = new.x - ballsur->w/2;
958 rect.y = new.y - ballsur->h/2;
959 SDL_BlitSurface(screen, &rect, savesur, NULL);
962 SDL_BlitSurface(ballsur, NULL, screen, &rect);
963 SDL_UpdateRect(screen, rect.x, rect.y, rect.w, rect.h);
964 /* That SDL_BlitSurface can crop rect. */
965 rect.x = new.x - ballsur->w/2;
966 rect.y = new.y - ballsur->h/2;
970 while (SDL_PollEvent(&event)) {
971 if (event.type == SDL_QUIT
972 || (event.type == SDL_KEYDOWN
973 && event.key.keysym.sym == SDLK_ESCAPE)) {
978 switch (event.type) {
979 case SDL_MOUSEBUTTONDOWN:
982 case SDL_MOUSEBUTTONUP:
985 case SDL_MOUSEMOTION:
989 n = new_line(event.motion.x,
998 timeradd(&now, &line_life,
1000 list_add_tail(&lines, &n->list);
1001 thick_line(screen, n, 0);
1010 if (cwiid_get_state(wiimote, &state))
1011 errx(1, "No wii state");
1013 /* Find biggest state. */
1014 ir = &state.ir_src[0];
1015 for (i = 0; i < CWIID_IR_SRC_COUNT; i++) {
1016 if (!state.ir_src[i].valid)
1018 if (!ir->valid || state.ir_src[i].size > ir->size)
1019 ir = &state.ir_src[0];
1023 /* Give it some slack for missing one or two... */
1024 if (time_since_last_ir <= 5) {
1025 n = new_line(last_ir.pos[0],
1031 timeradd(&now, &line_life, &n->expires);
1032 list_add_tail(&lines, &n->list);
1033 thick_line(screen, n, 0);
1036 time_since_last_ir = 0;
1039 time_since_last_ir++;