11 #include "list/list.h"
15 #define MAX_X CWIID_IR_X_MAX
16 #define MAX_Y CWIID_IR_Y_MAX
18 /* Obtained from http://tog.acm.org/GraphicsGems/gemsiii/insectc.c */
19 /* Faster Line Segment Intersection */
20 /* Franklin Antonio */
22 /* The use of some short working variables allows this code to run */
23 /* faster on 16-bit computers, but is not essential. It should not */
24 /* affect operation on 32-bit computers. The short working variables*/
25 /* to not restrict the range of valid input values, as these were */
26 /* constrained in any case, due to algorithm restrictions. */
27 static bool lines_intersect(double x1, double y1,
34 double Ax,Bx,Cx,Ay,By,Cy,d,e,f,num;
35 double x1lo,x1hi,y1lo,y1hi;
40 if(Ax<0) { /* X bound box test*/
46 if(x1hi < x4 || x3 < x1lo) return false;
48 if(x1hi < x3 || x4 < x1lo) return false;
54 if(Ay<0) { /* Y bound box test*/
60 if(y1hi < y4 || y3 < y1lo) return false;
62 if(y1hi < y3 || y4 < y1lo) return false;
68 d = By*Cx - Bx*Cy; /* alpha numerator*/
69 f = Ay*Bx - Ax*By; /* both denominator*/
70 if(f>0) { /* alpha tests*/
71 if(d<0 || d>f) return false;
73 if(d>0 || d<f) return false;
76 e = Ax*Cy - Ay*Cx; /* beta numerator*/
77 if(f>0) { /* beta tests*/
78 if(e<0 || e>f) return false;
80 if(e>0 || e<f) return false;
83 /*compute intersection coordinates*/
85 if(f==0) return false;
86 num = d*Ax; /* numerator */
87 *x = x1 + num / f; /* intersection x */
90 *y = y1 + num / f; /* intersection y */
95 // A set of very useful macros that you will find in most
96 // code that I write whether I use them in a program or
99 #define abs(a) (((a)<0) ? -(a) : (a))
100 #define sign(a) (((a)<0) ? -1 : (a)>0 ? 1 : 0)
102 // The following code implements a Bresenham line drawing
103 // algorithm. There are 4 separate routines each optimized
104 // for one of the four pixel depths supported by SDL. SDL
105 // support many pixel formats, but it only support 8, 16,
106 // 24, and 32 bit pixels.
108 //----------------------------------------------------------
110 // Draw lines in 8 bit surfaces.
112 static void line8(SDL_Surface *s,
137 yOffset = sy * s->pitch;
142 lineAddr = ((Uint8 *)(s->pixels)) + (y * s->pitch);
148 if (y >= 0 && y < MAX_Y && x >= 0 && x < MAX_X)
149 *(lineAddr + x) = (Uint8)color;
170 if (y >= 0 && y < MAX_Y && x >= 0 && x < MAX_X)
171 *(lineAddr + x) = (Uint8)color;
189 //----------------------------------------------------------
191 // Draw lines in 16 bit surfaces. Note that this code will
192 // also work on 15 bit surfaces.
194 static void line16(SDL_Surface *s,
219 yOffset = sy * s->pitch;
224 lineAddr = ((Uint8 *)s->pixels) + (y * s->pitch);
230 if (y >= 0 && y < MAX_Y && x >= 0 && x < MAX_X)
231 *((Uint16 *)(lineAddr + (x << 1))) = (Uint16)color;
252 if (y >= 0 && y < MAX_Y && x >= 0 && x < MAX_X)
253 *((Uint16 *)(lineAddr + (x << 1))) = (Uint16)color;
271 //----------------------------------------------------------
273 // Draw lines in 24 bit surfaces. 24 bit surfaces require
274 // special handling because the pixels don't fall on even
275 // address boundaries. Instead of being able to store a
276 // single byte, word, or long you have to store 3
277 // individual bytes. As a result 24 bit graphics is slower
278 // than the other pixel sizes.
280 static void line24(SDL_Surface *s,
298 #if (SDL_BYTEORDER == SDL_BIG_ENDIAN)
309 yOffset = sy * s->pitch;
314 lineAddr = ((Uint8 *)(s->pixels)) + (y * s->pitch);
320 Uint8 *p = (lineAddr + (x * 3));
321 if (y >= 0 && y < MAX_Y && x >= 0 && x < MAX_X)
322 memcpy(p, &color, 3);
343 Uint8 *p = (lineAddr + (x * 3));
344 if (y >= 0 && y < MAX_Y && x >= 0 && x < MAX_X)
345 memcpy(p, &color, 3);
363 //----------------------------------------------------------
365 // Draw lines in 32 bit surfaces. Note that this routine
366 // ignores alpha values. It writes them into the surface
367 // if they are included in the pixel, but does nothing
370 static void line32(SDL_Surface *s,
395 yOffset = sy * s->pitch;
400 lineAddr = ((Uint8 *)(s->pixels)) + (y * s->pitch);
406 if (y >= 0 && y < MAX_Y && x >= 0 && x < MAX_X)
407 *((Uint32 *)(lineAddr + (x << 2))) = (Uint32)color;
428 if (y >= 0 && y < MAX_Y && x >= 0 && x < MAX_X)
429 *((Uint32 *)(lineAddr + (x << 2))) = (Uint32)color;
447 //----------------------------------------------------------
449 // Examine the depth of a surface and select a line
450 // drawing routine optimized for the bytes/pixel of the
453 static void line(SDL_Surface *s,
458 switch (s->format->BytesPerPixel)
461 line8(s, x1, y1, x2, y2, color);
464 line16(s, x1, y1, x2, y2, color);
467 line24(s, x1, y1, x2, y2, color);
470 line32(s, x1, y1, x2, y2, color);
490 struct coord start, end;
491 struct list_node list;
493 struct timeval expires;
496 static void thick_line(SDL_Surface *s, const struct line_segment *l,
499 struct line_segment adj;
500 int x, y, minx = MAX_X, miny = MAX_Y, maxx = 0, maxy = 0;
505 for (x = -1; x < 2; x++) {
506 for (y = -1; y < 2; y++) {
507 adj.start.x = l->start.x + x;
508 adj.end.x = l->end.x + x;
509 adj.start.y = l->start.y + y;
510 adj.end.y = l->end.y + y;
512 if (adj.start.x < minx)
514 if (adj.start.x > maxx)
516 if (adj.start.y < miny)
518 if (adj.start.y > maxy)
520 if (adj.end.x < minx)
522 if (adj.end.x > maxx)
524 if (adj.end.y < miny)
526 if (adj.end.y > maxy)
529 line(s, adj.start.x, adj.start.y, adj.end.x, adj.end.y,
542 SDL_UpdateRect(s, minx, miny, maxx-minx, maxy-miny);
543 SDL_UnlockSurface(s);
546 static bool intersect(const struct coord *c1, const struct coord *c2,
547 const struct line_segment *seg, struct coord *intersect)
549 return lines_intersect(c1->x, c1->y, c2->x, c2->y,
550 seg->start.x, seg->start.y,
551 seg->end.x, seg->end.y,
552 &intersect->x, &intersect->y);
555 static double dist(const struct coord *c1, const struct coord *c2)
557 float x = (c1->x - c2->x), y = (c1->y - c2->y);
558 return sqrt(x * x + y * y);
561 static unsigned rad_to_deg(double rad)
563 return ((unsigned)(rad / M_PI * 180) + 360) % 360;
566 static void bounce(struct ball *ball, const struct line_segment *line,
567 struct coord *remainder)
569 double len, speed, lang, iang, oang;
570 struct coord isect, new;
572 new.x = ball->pos.x + ball->move.x;
573 new.y = ball->pos.y + ball->move.y;
575 /* How far were we supposed to move ball? */
576 len = sqrt(remainder->x * remainder->x + remainder->y * remainder->y);
578 /* How far is it to intersection? */
579 intersect(&ball->pos, &new, line, &isect);
580 len -= dist(&ball->pos, &isect);
582 /* Move ball to intersection. */
584 printf("ball is now at %f,%f\n", ball->pos.x, ball->pos.y);
586 /* Outgoing angle = 2 * line angle - incident angle. */
587 lang = atan2(line->end.x - line->start.x, line->end.y - line->start.y);
588 iang = atan2(ball->move.x, ball->move.y);
589 oang = 2 * lang - iang, 2*M_PI;
590 printf("lang = %u, iang = %u, oang=%u\n",
595 /* Set new direction for ball, at same speed */
596 speed = sqrt(ball->move.x * ball->move.x + ball->move.y * ball->move.y);
598 ball->move.x = sin(oang) * speed;
599 ball->move.y = cos(oang) * speed;
602 remainder->x = sin(oang) * len;
603 remainder->y = cos(oang) * len;
604 printf("len = %f, remainder = %f,%f\n", len, remainder->x, remainder->y);
607 static struct line_segment border[] = {
608 { { 0, 0, }, { MAX_X-1, 0 } },
609 { { MAX_X-1, 0, }, { MAX_X-1, MAX_Y-1 } },
610 { { MAX_X-1, MAX_Y-1, }, { 0, MAX_Y-1 } },
611 { { 0, MAX_Y-1, }, { 0, 0 } },
614 static inline float deg_to_rad(float degrees)
616 return degrees * M_PI / 180;
619 static SDL_Surface *sub_surface(SDL_Surface *screen, int w, int h)
621 return SDL_CreateRGBSurface(SDL_SWSURFACE, w, h,
622 screen->format->BitsPerPixel,
623 screen->format->Rmask,
624 screen->format->Gmask,
625 screen->format->Bmask,
626 screen->format->Amask);
629 static SDL_Surface *ball_surface(SDL_Surface *screen)
633 /* Just like the screen surface. */
634 ball = sub_surface(screen, 5, 5);
636 /* Lock the surface */
637 SDL_LockSurface(ball);
639 /* Black, white corners. */
640 memset(ball->pixels, 0x00, ball->pitch * 5);
641 line(ball, 0, 0, 0, 0, 0xFFFFFFFF);
642 line(ball, 4, 0, 4, 0, 0xFFFFFFFF);
643 line(ball, 0, 4, 0, 4, 0xFFFFFFFF);
644 line(ball, 4, 4, 4, 4, 0xFFFFFFFF);
645 SDL_UnlockSurface(ball);
650 static void clear_ignore(struct list_head *lines)
652 struct line_segment *i;
653 printf("Unignoring...\n");
654 list_for_each(lines, i, list)
658 int main(int argc, char *argv[])
661 SDL_Surface *screen, *ballsur, *savesur = NULL;
666 bool creating_lines = false;
667 struct timeval line_life = { 5, 0 };
671 errx(1, "Usage: %s ballx bally ballangle ballspeed\n"
672 " Where ballangle is 0 for north, 90 for east, etc\n",
675 ball.pos.x = atol(argv[1]);
676 ball.pos.y = atol(argv[2]);
677 ball.move.x = sin(deg_to_rad(atof(argv[3]))) * atol(argv[4]);
678 ball.move.y = cos(deg_to_rad(atof(argv[3]))) * atol(argv[4]);
679 printf("ball move = %f,%f\n", ball.move.x, ball.move.y);
682 if ( SDL_Init(SDL_INIT_VIDEO) < 0 ) {
683 fprintf(stderr, "Couldn't initialize SDL: %s\n",SDL_GetError());
688 videoflags = SDL_SWSURFACE;
690 /* Set 640x480 video mode */
691 if ( (screen=SDL_SetVideoMode(MAX_X,MAX_Y,video_bpp,videoflags)) == NULL ) {
692 errx(1, "Couldn't set %dx%dx%d video mode: %s",
693 MAX_X, MAX_Y,video_bpp, SDL_GetError());
696 /* Set the surface pixels and refresh! */
697 if ( SDL_LockSurface(screen) < 0 ) {
698 errx(1, "Couldn't lock the display surface: %s",
702 memset(screen->pixels, 0xFF, screen->pitch * screen->h);
704 SDL_UnlockSurface(screen);
705 SDL_UpdateRect(screen, 0, 0, 0, 0);
707 /* Draw borders, put them in list. */
708 for (i = 0; i < ARRAY_SIZE(border); i++) {
709 border[i].ignore = false;
710 border[i].expires.tv_sec = LONG_MAX;
711 border[i].expires.tv_usec = 0;
712 list_add(&lines, &border[i].list);
713 thick_line(screen, &border[i], 0);
716 ballsur = ball_surface(screen);
719 struct coord new, isect, move = ball.move;
720 struct line_segment *best_l, *l, *next;
725 gettimeofday(&now, NULL);
728 list_for_each_safe(&lines, l, next, list) {
729 if (timercmp(&l->expires, &now, <)) {
731 /* FIXME: Undraw properly. */
732 thick_line(screen, l, 0xFFFFFFFF);
738 best_d = MAX_X + MAX_Y;
740 new.x = ball.pos.x + move.x;
741 new.y = ball.pos.y + move.y;
743 list_for_each(&lines, l, list) {
744 if (!l->ignore && intersect(&ball.pos, &new, l, &isect)) {
746 d = dist(&ball.pos, &isect);
755 printf("Ball bouncing off (%f,%f %f,%f)!\n",
756 best_l->start.x, best_l->start.y,
757 best_l->end.x, best_l->end.y);
758 bounce(&ball, best_l, &move);
760 /* If we moved, stop ignoring lines. */
761 if (move.x > 0.001 || move.y > 0.001)
762 clear_ignore(&lines);
764 /* Don't hit the same line twice. */
765 printf("Ignoring that line\n");
766 best_l->ignore = true;
770 if (move.x > 0.001 || move.y > 0.001) {
771 clear_ignore(&lines);
772 printf("Moving by %f,%f to %f,%f\n", move.x, move.y, new.x, new.y);
775 /* Restore what was there before ball. */
777 SDL_BlitSurface(savesur, NULL, screen, &rect);
778 SDL_UpdateRect(screen, rect.x, rect.y, rect.w, rect.h);
780 savesur = sub_surface(screen, ballsur->w, ballsur->h);
783 /* Save away image under new ball. */
786 rect.x = new.x - ballsur->w/2;
787 rect.y = new.y - ballsur->h/2;
788 SDL_BlitSurface(screen, &rect, savesur, NULL);
791 SDL_BlitSurface(ballsur, NULL, screen, &rect);
792 SDL_UpdateRect(screen, rect.x, rect.y, rect.w, rect.h);
793 /* That SDL_BlitSurface can crop rect. */
794 rect.x = new.x - ballsur->w/2;
795 rect.y = new.y - ballsur->h/2;
799 while (SDL_PollEvent(&event)) {
800 if (event.type == SDL_QUIT) {
804 if (event.type == SDL_MOUSEBUTTONDOWN)
805 creating_lines = true;
806 else if (event.type == SDL_MOUSEBUTTONUP)
807 creating_lines = false;
809 if (creating_lines && event.type == SDL_MOUSEMOTION) {
810 struct line_segment *n = malloc(sizeof(*n));
811 n->start.x = event.motion.x - event.motion.xrel;
812 n->start.y = event.motion.y - event.motion.yrel;
813 n->end.x = event.motion.x;
814 n->end.y = event.motion.y;
816 timeradd(&now, &line_life, &n->expires);
817 list_add_tail(&lines, &n->list);
818 thick_line(screen, n, 0);