11 #include "list/list.h"
17 #define MAX_X CWIID_IR_X_MAX
18 #define MAX_Y CWIID_IR_Y_MAX
28 /* Obtained from http://tog.acm.org/GraphicsGems/gemsiii/insectc.c */
29 /* Faster Line Segment Intersection */
30 /* Franklin Antonio */
31 static bool lines_intersect(const struct coord *start1,
32 const struct coord *start2,
33 const struct coord *start3,
34 const struct coord *start4,
35 struct coord *intersect)
37 double Ax,Bx,Cx,Ay,By,Cy,d,e,f,num;
40 Ax = start2->x - start1->x;
41 Bx = start3->x - start4->x;
43 if(Ax<0) { /* X bound box test*/
44 lo.x = start2->x; hi.x = start1->x;
46 hi.x = start2->x; lo.x = start1->x;
49 if(hi.x < start4->x || start3->x < lo.x) return false;
51 if(hi.x < start3->x || start4->x < lo.x) return false;
54 Ay = start2->y - start1->y;
55 By = start3->y - start4->y;
57 if(Ay<0) { /* Y bound box test*/
58 lo.y = start2->y; hi.y = start1->y;
60 hi.y = start2->y; lo.y = start1->y;
63 if(hi.y < start4->y || start3->y < lo.y) return false;
65 if(hi.y < start3->y || start4->y < lo.y) return false;
69 Cx = start1->x - start3->x;
70 Cy = start1->y - start3->y;
71 d = By*Cx - Bx*Cy; /* alpha numerator*/
72 f = Ay*Bx - Ax*By; /* both denominator*/
73 if (f>0) { /* alpha tests*/
74 if (d < 0 || d > f) return false;
76 if (d > 0 || d < f) return false;
79 e = Ax*Cy - Ay*Cx; /* beta numerator*/
80 if (f > 0) { /* beta tests*/
81 if (e < 0 || e > f) return false;
83 if (e > 0 || e < f) return false;
86 /*compute intersection coordinates*/
87 /* Don't worry about tiny values of f (< 1/2 pixel across screen) */
88 if (fabs(f) < 1.0 / (2.0 * MAX_X * MAX_Y))
91 num = d*Ax; /* numerator */
92 intersect->x = start1->x + num / f; /* intersection x */
95 intersect->y = start1->y + num / f; /* intersection y */
100 // A set of very useful macros that you will find in most
101 // code that I write whether I use them in a program or
104 #define abs(a) (((a)<0) ? -(a) : (a))
105 #define sign(a) (((a)<0) ? -1 : (a)>0 ? 1 : 0)
107 // The following code implements a Bresenham line drawing
108 // algorithm. There are 4 separate routines each optimized
109 // for one of the four pixel depths supported by SDL. SDL
110 // support many pixel formats, but it only support 8, 16,
111 // 24, and 32 bit pixels.
113 //----------------------------------------------------------
115 // Draw lines in 8 bit surfaces.
117 static void line8(SDL_Surface *s,
142 yOffset = sy * s->pitch;
147 lineAddr = ((Uint8 *)(s->pixels)) + (y * s->pitch);
153 if (y >= 0 && y < MAX_Y && x >= 0 && x < MAX_X)
154 *(lineAddr + x) = (Uint8)color;
175 if (y >= 0 && y < MAX_Y && x >= 0 && x < MAX_X)
176 *(lineAddr + x) = (Uint8)color;
194 //----------------------------------------------------------
196 // Draw lines in 16 bit surfaces. Note that this code will
197 // also work on 15 bit surfaces.
199 static void line16(SDL_Surface *s,
224 yOffset = sy * s->pitch;
229 lineAddr = ((Uint8 *)s->pixels) + (y * s->pitch);
235 if (y >= 0 && y < MAX_Y && x >= 0 && x < MAX_X)
236 *((Uint16 *)(lineAddr + (x << 1))) = (Uint16)color;
257 if (y >= 0 && y < MAX_Y && x >= 0 && x < MAX_X)
258 *((Uint16 *)(lineAddr + (x << 1))) = (Uint16)color;
276 //----------------------------------------------------------
278 // Draw lines in 24 bit surfaces. 24 bit surfaces require
279 // special handling because the pixels don't fall on even
280 // address boundaries. Instead of being able to store a
281 // single byte, word, or long you have to store 3
282 // individual bytes. As a result 24 bit graphics is slower
283 // than the other pixel sizes.
285 static void line24(SDL_Surface *s,
303 #if (SDL_BYTEORDER == SDL_BIG_ENDIAN)
314 yOffset = sy * s->pitch;
319 lineAddr = ((Uint8 *)(s->pixels)) + (y * s->pitch);
325 Uint8 *p = (lineAddr + (x * 3));
326 if (y >= 0 && y < MAX_Y && x >= 0 && x < MAX_X)
327 memcpy(p, &color, 3);
348 Uint8 *p = (lineAddr + (x * 3));
349 if (y >= 0 && y < MAX_Y && x >= 0 && x < MAX_X)
350 memcpy(p, &color, 3);
368 //----------------------------------------------------------
370 // Draw lines in 32 bit surfaces. Note that this routine
371 // ignores alpha values. It writes them into the surface
372 // if they are included in the pixel, but does nothing
375 static void line32(SDL_Surface *s,
400 yOffset = sy * s->pitch;
405 lineAddr = ((Uint8 *)(s->pixels)) + (y * s->pitch);
411 if (y >= 0 && y < MAX_Y && x >= 0 && x < MAX_X)
412 *((Uint32 *)(lineAddr + (x << 2))) = (Uint32)color;
433 if (y >= 0 && y < MAX_Y && x >= 0 && x < MAX_X)
434 *((Uint32 *)(lineAddr + (x << 2))) = (Uint32)color;
452 //----------------------------------------------------------
454 // Examine the depth of a surface and select a line
455 // drawing routine optimized for the bytes/pixel of the
458 static void line(SDL_Surface *s,
463 switch (s->format->BytesPerPixel)
466 line8(s, x1, y1, x2, y2, color);
469 line16(s, x1, y1, x2, y2, color);
472 line24(s, x1, y1, x2, y2, color);
475 line32(s, x1, y1, x2, y2, color);
488 struct coord start, end;
489 struct list_node list;
491 struct timeval expires;
494 static void thick_line(SDL_Surface *s, const struct line_segment *l,
497 struct line_segment adj;
498 int x, y, minx = MAX_X, miny = MAX_Y, maxx = 0, maxy = 0;
503 for (x = -1; x < 2; x++) {
504 for (y = -1; y < 2; y++) {
505 adj.start.x = l->start.x + x;
506 adj.end.x = l->end.x + x;
507 adj.start.y = l->start.y + y;
508 adj.end.y = l->end.y + y;
510 if (adj.start.x < minx)
512 if (adj.start.x > maxx)
514 if (adj.start.y < miny)
516 if (adj.start.y > maxy)
518 if (adj.end.x < minx)
520 if (adj.end.x > maxx)
522 if (adj.end.y < miny)
524 if (adj.end.y > maxy)
527 line(s, adj.start.x, adj.start.y, adj.end.x, adj.end.y,
540 SDL_UpdateRect(s, minx, miny, maxx-minx, maxy-miny);
541 SDL_UnlockSurface(s);
544 static bool intersect(const struct coord *c1, const struct coord *c2,
545 const struct line_segment *seg, struct coord *intersect)
547 return lines_intersect(c1, c2, &seg->start, &seg->end, intersect);
550 static double dist(const struct coord *c1, const struct coord *c2)
552 double x = (c1->x - c2->x), y = (c1->y - c2->y);
553 return sqrt(x * x + y * y);
556 static unsigned rad_to_deg(double rad)
558 return ((unsigned)(rad / M_PI * 180) + 360) % 360;
561 static void bounce(struct ball *ball, const struct line_segment *line,
562 struct coord *move, double increase_speed)
564 double len, speed, lang, iang, oang;
565 struct coord isect, new;
567 new.x = ball->pos.x + move->x;
568 new.y = ball->pos.y + move->y;
570 /* How far were we supposed to move ball? */
571 len = sqrt(move->x * move->x + move->y * move->y);
573 /* How far is it to intersection? */
574 if (!intersect(&ball->pos, &new, line, &isect))
575 errx(1, "No intersection any more?\n");;
576 len -= dist(&ball->pos, &isect);
578 /* Move ball to intersection. */
580 printf("ball is now at %f,%f\n", ball->pos.x, ball->pos.y);
582 /* Outgoing angle = 2 * line angle - incident angle. */
583 lang = atan2(line->end.x - line->start.x, line->end.y - line->start.y);
584 iang = atan2(ball->move.x, ball->move.y);
585 oang = 2 * lang - iang, 2*M_PI;
586 printf("lang = %u, iang = %u, oang=%u\n",
591 /* Set new direction for ball, at slightly increased speed */
592 speed = sqrt(ball->move.x * ball->move.x + ball->move.y * ball->move.y);
593 speed += increase_speed;
594 ball->move.x = sin(oang) * speed;
595 ball->move.y = cos(oang) * speed;
598 move->x = sin(oang) * len;
599 move->y = cos(oang) * len;
600 printf("len = %f, move = %f,%f\n", len, move->x, move->y);
603 static struct line_segment border[] = {
604 { { 0, 0, }, { MAX_X-1, 0 } },
605 { { MAX_X-1, 0, }, { MAX_X-1, MAX_Y-1 } },
606 { { MAX_X-1, MAX_Y-1, }, { 0, MAX_Y-1 } },
607 { { 0, MAX_Y-1, }, { 0, 0 } },
610 static inline float deg_to_rad(float degrees)
612 return degrees * M_PI / 180;
615 static SDL_Surface *sub_surface(SDL_Surface *screen, int w, int h)
617 return SDL_CreateRGBSurface(SDL_SWSURFACE, w, h,
618 screen->format->BitsPerPixel,
619 screen->format->Rmask,
620 screen->format->Gmask,
621 screen->format->Bmask,
622 screen->format->Amask);
625 static SDL_Surface *ball_surface(SDL_Surface *screen)
629 /* Just like the screen surface. */
630 ball = sub_surface(screen, 5, 5);
632 /* Lock the surface */
633 SDL_LockSurface(ball);
635 /* Black, white corners. */
636 memset(ball->pixels, 0x00, ball->pitch * 5);
637 line(ball, 0, 0, 0, 0, 0xFFFFFFFF);
638 line(ball, 4, 0, 4, 0, 0xFFFFFFFF);
639 line(ball, 0, 4, 0, 4, 0xFFFFFFFF);
640 line(ball, 4, 4, 4, 4, 0xFFFFFFFF);
641 SDL_UnlockSurface(ball);
646 static void clear_ignore(struct list_head *lines)
648 struct line_segment *i;
649 printf("Unignoring...\n");
650 list_for_each(lines, i, list)
654 static void make_cross(unsigned x, unsigned y,
655 struct line_segment *seg1,
656 struct line_segment *seg2)
668 static void calibrate(SDL_Surface *screen,
669 cwiid_wiimote_t *wiimote,
670 struct coord *calib, unsigned x, unsigned y)
672 struct line_segment seg1, seg2;
673 struct cwiid_state state;
676 make_cross(x, y, &seg1, &seg2);
677 thick_line(screen, &seg1, 0);
678 thick_line(screen, &seg2, 0);
682 if (cwiid_get_state(wiimote, &state))
683 errx(1, "No wii state");
685 for (i = 0; i < CWIID_IR_SRC_COUNT; i++) {
686 if (state.ir_src[i].valid)
690 if (SDL_PollEvent(&event)
691 && (event.type == SDL_QUIT
692 || (event.type == SDL_KEYDOWN
693 && event.key.keysym.sym == SDLK_ESCAPE))) {
697 } while (i == CWIID_IR_SRC_COUNT);
699 thick_line(screen, &seg1, 0xFFFFFFFF);
700 thick_line(screen, &seg2, 0xFFFFFFFF);
702 calib->x = state.ir_src[i].pos[0];
703 calib->y = state.ir_src[i].pos[1];
704 printf("Calibration point: %u,%u\n",
705 state.ir_src[i].pos[0],
706 state.ir_src[i].pos[1]);
707 make_cross(calib->x, calib->y, &seg1, &seg2);
709 thick_line(screen, &seg1, 0xFFFF0000);
710 thick_line(screen, &seg2, 0xFFFF0000);
713 static bool map_coord(unsigned int x, unsigned int y,
714 const struct coord calib[],
717 struct coord line_start, line_end;
718 struct coord pos = { x, y }, intersect;
721 /* Calibration layout:
727 * We figure out the distance to each side, and then map using:
729 * x = d1 / (d1 + d2) * (rawend - rawstart) + rawstart
732 line_end.x = MAX_X-1;
733 line_start.y = line_end.y = y;
735 if (!lines_intersect(&calib[0], &calib[3], &line_start, &line_end,
738 d1 = dist(&pos, &intersect);
739 if (!lines_intersect(&calib[1], &calib[2], &line_start, &line_end,
742 d2 = dist(&pos, &intersect);
743 res->x = d1 / (d1 + d2) * (MAX_X - 2*CALIB_OFF) + CALIB_OFF;
746 line_end.y = MAX_Y-1;
747 line_start.x = line_end.x = x;
749 if (!lines_intersect(&calib[0], &calib[1], &line_start, &line_end,
752 d1 = dist(&pos, &intersect);
753 if (!lines_intersect(&calib[3], &calib[2], &line_start, &line_end,
756 d2 = dist(&pos, &intersect);
757 res->y = d1 / (d1 + d2) * MAX_Y;
762 static struct line_segment *new_line(unsigned int startx, unsigned int starty,
763 unsigned int endx, unsigned int endy,
764 const struct coord calib[])
766 struct line_segment *l = malloc(sizeof(*l));
769 if (!map_coord(startx, starty, calib, &l->start)
770 || !map_coord(endx, endy, calib, &l->end)) {
778 int main(int argc, char *argv[])
781 SDL_Surface *screen, *ballsur, *savesur = NULL;
784 unsigned int i, time_since_last_ir = INT_MAX;
786 struct timeval line_life = { 3, 0 };
787 cwiid_wiimote_t *wiimote;
788 struct cwiid_state state;
789 struct coord calib[4];
790 bdaddr_t addr = *BDADDR_ANY;
791 bool needs_calibration = true;
793 struct cwiid_ir_src *ir, last_ir = { .valid = 0 };
795 videoflags = SDL_SWSURFACE;
797 if (argv[1] && streq(argv[1], "--fullscreen")) {
798 videoflags |= SDL_FULLSCREEN;
803 if (argv[1] && strstarts(argv[1], "--calib=")) {
804 needs_calibration = false;
805 if (sscanf(argv[1], "--calib=%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf",
806 &calib[0].x, &calib[0].y,
807 &calib[1].x, &calib[1].y,
808 &calib[2].x, &calib[2].y,
809 &calib[3].x, &calib[3].y) != 8)
810 errx(1, "Not enough calibration points");
816 errx(1, "Usage: %s [--fullscreen] [--calib=x,y,x,y,x,y,x,y] ballx bally ballangle ballspeed\n"
817 " Where ballangle is 0 for north, 90 for east, etc\n",
820 ball.pos.x = atol(argv[1]);
821 ball.pos.y = atol(argv[2]);
822 ball.move.x = sin(deg_to_rad(atof(argv[3]))) * atol(argv[4]);
823 ball.move.y = cos(deg_to_rad(atof(argv[3]))) * atol(argv[4]);
824 printf("ball move = %f,%f\n", ball.move.x, ball.move.y);
827 if ( SDL_Init(SDL_INIT_VIDEO) < 0 ) {
828 fprintf(stderr, "Couldn't initialize SDL: %s\n",SDL_GetError());
834 printf("Put Wiimote in discoverable mode (press 1+2)\n");
835 wiimote = cwiid_open(&addr, 0);
837 errx(1, "Can't find the Wiimote");
839 if (cwiid_set_rpt_mode(wiimote, CWIID_RPT_IR))
840 errx(1, "Can't set IR repeat mode");
842 if ( (screen=SDL_SetVideoMode(MAX_X,MAX_Y,video_bpp,videoflags)) == NULL ) {
843 errx(1, "Couldn't set %dx%dx%d video mode: %s",
844 MAX_X, MAX_Y,video_bpp, SDL_GetError());
847 /* Set the surface pixels and refresh! */
848 if ( SDL_LockSurface(screen) < 0 ) {
849 errx(1, "Couldn't lock the display surface: %s",
853 memset(screen->pixels, 0xFF, screen->pitch * screen->h);
855 SDL_UnlockSurface(screen);
856 SDL_UpdateRect(screen, 0, 0, 0, 0);
858 if (needs_calibration) {
860 calibrate(screen, wiimote, &calib[0], CALIB_OFF, 6);
862 calibrate(screen, wiimote, &calib[1],
863 MAX_X - 1 - CALIB_OFF, 6);
865 calibrate(screen, wiimote, &calib[2],
866 MAX_X - 1 - CALIB_OFF, MAX_Y - 7);
868 calibrate(screen, wiimote, &calib[3],
869 CALIB_OFF, MAX_Y - 7);
872 /* Draw borders, put them in list. */
873 for (i = 0; i < ARRAY_SIZE(border); i++) {
874 border[i].ignore = false;
875 border[i].expires.tv_sec = LONG_MAX;
876 border[i].expires.tv_usec = 0;
877 list_add(&lines, &border[i].list);
878 thick_line(screen, &border[i], 0);
881 ballsur = ball_surface(screen);
884 struct coord new, isect, move = ball.move;
885 struct line_segment *best_l, *l, *next;
889 bool ignored = false;
891 gettimeofday(&now, NULL);
894 list_for_each_safe(&lines, l, next, list) {
895 if (timercmp(&l->expires, &now, <)) {
896 printf("Deleting line %p\n", l);
898 /* FIXME: Undraw properly. */
899 thick_line(screen, l, 0xFFFFFFFF);
905 best_d = MAX_X + MAX_Y;
907 new.x = ball.pos.x + move.x;
908 new.y = ball.pos.y + move.y;
910 list_for_each(&lines, l, list) {
911 if (!l->ignore && intersect(&ball.pos, &new, l, &isect)) {
913 d = dist(&ball.pos, &isect);
922 printf("Ball bouncing off (%f,%f %f,%f)!\n",
923 best_l->start.x, best_l->start.y,
924 best_l->end.x, best_l->end.y);
925 /* Only increase speed on *first* bounce, to avoid
926 * mega speed increases. */
927 bounce(&ball, best_l, &move, ignored ? 0.0 : 0.1);
929 /* If we moved, stop ignoring lines. */
930 if (ignored && (move.x > 0.001 || move.y > 0.001))
931 clear_ignore(&lines);
933 /* Don't hit the same line twice. */
934 printf("Ignoring that line\n");
935 best_l->ignore = ignored = true;
939 if (ignored && (move.x > 0.001 || move.y > 0.001)) {
940 clear_ignore(&lines);
941 printf("Moving by %f,%f to %f,%f\n",
942 move.x, move.y, new.x, new.y);
945 /* Restore what was there before ball. */
947 SDL_BlitSurface(savesur, NULL, screen, &rect);
948 SDL_UpdateRect(screen, rect.x, rect.y, rect.w, rect.h);
950 savesur = sub_surface(screen, ballsur->w, ballsur->h);
953 /* Save away image under new ball. */
956 rect.x = new.x - ballsur->w/2;
957 rect.y = new.y - ballsur->h/2;
958 SDL_BlitSurface(screen, &rect, savesur, NULL);
961 SDL_BlitSurface(ballsur, NULL, screen, &rect);
962 SDL_UpdateRect(screen, rect.x, rect.y, rect.w, rect.h);
963 /* That SDL_BlitSurface can crop rect. */
964 rect.x = new.x - ballsur->w/2;
965 rect.y = new.y - ballsur->h/2;
969 if (SDL_PollEvent(&event)
970 && (event.type == SDL_QUIT
971 || (event.type == SDL_KEYDOWN
972 && event.key.keysym.sym == SDLK_ESCAPE))) {
977 if (cwiid_get_state(wiimote, &state))
978 errx(1, "No wii state");
980 /* Find biggest state. */
981 ir = &state.ir_src[0];
982 for (i = 0; i < CWIID_IR_SRC_COUNT; i++) {
983 if (!state.ir_src[i].valid)
985 if (!ir->valid || state.ir_src[i].size > ir->size)
986 ir = &state.ir_src[0];
990 /* Give it some slack for missing one or two... */
991 if (time_since_last_ir <= 5) {
992 struct line_segment *n;
994 n = new_line(last_ir.pos[0],
1000 timeradd(&now, &line_life, &n->expires);
1001 list_add_tail(&lines, &n->list);
1002 thick_line(screen, n, 0);
1005 time_since_last_ir = 0;
1008 time_since_last_ir++;