13 /* Obtained from http://tog.acm.org/GraphicsGems/gemsiii/insectc.c */
14 /* Faster Line Segment Intersection */
15 /* Franklin Antonio */
18 #define DONT_INTERSECT 0
19 #define DO_INTERSECT 1
22 /* The SAME_SIGNS macro assumes arithmetic where the exclusive-or */
23 /* operation will work on sign bits. This works for twos-complement,*/
24 /* and most other machine arithmetic. */
25 #define SAME_SIGNS( a, b ) \
26 (((long) ((unsigned long) a ^ (unsigned long) b)) >= 0 )
29 /* The use of some short working variables allows this code to run */
30 /* faster on 16-bit computers, but is not essential. It should not */
31 /* affect operation on 32-bit computers. The short working variables*/
32 /* to not restrict the range of valid input values, as these were */
33 /* constrained in any case, due to algorithm restrictions. */
34 static int lines_intersect(long x1, long y1, long x2, long y2, long x3, long y3,
35 long x4,long y4, long *x, long *y)
38 long Ax,Bx,Cx,Ay,By,Cy,d,e,f,num,offset;
39 short x1lo,x1hi,y1lo,y1hi;
44 if(Ax<0) { /* X bound box test*/
45 x1lo=(short)x2; x1hi=(short)x1;
47 x1hi=(short)x2; x1lo=(short)x1;
50 if(x1hi < (short)x4 || (short)x3 < x1lo) return DONT_INTERSECT;
52 if(x1hi < (short)x3 || (short)x4 < x1lo) return DONT_INTERSECT;
58 if(Ay<0) { /* Y bound box test*/
59 y1lo=(short)y2; y1hi=(short)y1;
61 y1hi=(short)y2; y1lo=(short)y1;
64 if(y1hi < (short)y4 || (short)y3 < y1lo) return DONT_INTERSECT;
66 if(y1hi < (short)y3 || (short)y4 < y1lo) return DONT_INTERSECT;
72 d = By*Cx - Bx*Cy; /* alpha numerator*/
73 f = Ay*Bx - Ax*By; /* both denominator*/
74 if(f>0) { /* alpha tests*/
75 if(d<0 || d>f) return DONT_INTERSECT;
77 if(d>0 || d<f) return DONT_INTERSECT;
80 e = Ax*Cy - Ay*Cx; /* beta numerator*/
81 if(f>0) { /* beta tests*/
82 if(e<0 || e>f) return DONT_INTERSECT;
84 if(e>0 || e<f) return DONT_INTERSECT;
87 /*compute intersection coordinates*/
89 if(f==0) return PARALLEL;
90 num = d*Ax; /* numerator */
91 offset = SAME_SIGNS(num,f) ? f/2 : -f/2; /* round direction*/
92 *x = x1 + (num+offset) / f; /* intersection x */
95 offset = SAME_SIGNS(num,f) ? f/2 : -f/2;
96 *y = y1 + (num+offset) / f; /* intersection y */
101 // A set of very useful macros that you will find in most
102 // code that I write whether I use them in a program or
105 #define abs(a) (((a)<0) ? -(a) : (a))
106 #define sign(a) (((a)<0) ? -1 : (a)>0 ? 1 : 0)
108 // The following code implements a Bresenham line drawing
109 // algorithm. There are 4 separate routines each optimized
110 // for one of the four pixel depths supported by SDL. SDL
111 // support many pixel formats, but it only support 8, 16,
112 // 24, and 32 bit pixels.
114 //----------------------------------------------------------
116 // Draw lines in 8 bit surfaces.
118 static void line8(SDL_Surface *s,
143 yOffset = sy * s->pitch;
148 lineAddr = ((Uint8 *)(s->pixels)) + (y * s->pitch);
154 if (y >= 0 && y < MAX_Y && x >= 0 && x < MAX_X)
155 *(lineAddr + x) = (Uint8)color;
176 if (y >= 0 && y < MAX_Y && x >= 0 && x < MAX_X)
177 *(lineAddr + x) = (Uint8)color;
195 //----------------------------------------------------------
197 // Draw lines in 16 bit surfaces. Note that this code will
198 // also work on 15 bit surfaces.
200 static void line16(SDL_Surface *s,
225 yOffset = sy * s->pitch;
230 lineAddr = ((Uint8 *)s->pixels) + (y * s->pitch);
236 if (y >= 0 && y < MAX_Y && x >= 0 && x < MAX_X)
237 *((Uint16 *)(lineAddr + (x << 1))) = (Uint16)color;
258 if (y >= 0 && y < MAX_Y && x >= 0 && x < MAX_X)
259 *((Uint16 *)(lineAddr + (x << 1))) = (Uint16)color;
277 //----------------------------------------------------------
279 // Draw lines in 24 bit surfaces. 24 bit surfaces require
280 // special handling because the pixels don't fall on even
281 // address boundaries. Instead of being able to store a
282 // single byte, word, or long you have to store 3
283 // individual bytes. As a result 24 bit graphics is slower
284 // than the other pixel sizes.
286 static void line24(SDL_Surface *s,
304 #if (SDL_BYTEORDER == SDL_BIG_ENDIAN)
315 yOffset = sy * s->pitch;
320 lineAddr = ((Uint8 *)(s->pixels)) + (y * s->pitch);
326 Uint8 *p = (lineAddr + (x * 3));
327 if (y >= 0 && y < MAX_Y && x >= 0 && x < MAX_X)
328 memcpy(p, &color, 3);
349 Uint8 *p = (lineAddr + (x * 3));
350 if (y >= 0 && y < MAX_Y && x >= 0 && x < MAX_X)
351 memcpy(p, &color, 3);
369 //----------------------------------------------------------
371 // Draw lines in 32 bit surfaces. Note that this routine
372 // ignores alpha values. It writes them into the surface
373 // if they are included in the pixel, but does nothing
376 static void line32(SDL_Surface *s,
401 yOffset = sy * s->pitch;
406 lineAddr = ((Uint8 *)(s->pixels)) + (y * s->pitch);
412 if (y >= 0 && y < MAX_Y && x >= 0 && x < MAX_X)
413 *((Uint32 *)(lineAddr + (x << 2))) = (Uint32)color;
434 if (y >= 0 && y < MAX_Y && x >= 0 && x < MAX_X)
435 *((Uint32 *)(lineAddr + (x << 2))) = (Uint32)color;
453 //----------------------------------------------------------
455 // Examine the depth of a surface and select a line
456 // drawing routine optimized for the bytes/pixel of the
459 static void line(SDL_Surface *s,
464 switch (s->format->BytesPerPixel)
467 line8(s, x1, y1, x2, y2, color);
470 line16(s, x1, y1, x2, y2, color);
473 line24(s, x1, y1, x2, y2, color);
476 line32(s, x1, y1, x2, y2, color);
496 struct coord start, end;
499 static void thick_line(SDL_Surface *s, const struct line_segment *l)
501 struct line_segment adj;
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 line(s, adj.start.x, adj.start.y, adj.end.x, adj.end.y,
518 static bool intersect(const struct coord *c1, const struct coord *c2,
519 const struct line_segment *seg, struct coord *intersect)
521 return lines_intersect(c1->x, c1->y, c2->x, c2->y,
522 seg->start.x, seg->start.y,
523 seg->end.x, seg->end.y,
524 &intersect->x, &intersect->y) == DO_INTERSECT;
527 static double dist(const struct coord *c1, const struct coord *c2)
529 long x = (c1->x - c2->x), y = (c1->y - c2->y);
530 return sqrt(x * x + y * y);
533 static unsigned rad_to_deg(double rad)
535 return ((unsigned)(rad / M_PI * 180) + 360) % 360;
538 static void bounce(struct ball *ball, const struct line_segment *line,
539 struct coord *remainder)
541 double len, speed, lang, iang, oang;
542 struct coord isect, new;
544 new.x = ball->pos.x + ball->move.x;
545 new.y = ball->pos.y + ball->move.y;
547 /* How far were we supposed to move ball? */
548 len = sqrt(remainder->x * remainder->x + remainder->y * remainder->y);
550 /* How far is it to intersection? */
551 intersect(&ball->pos, &new, line, &isect);
552 len -= dist(&ball->pos, &isect);
554 /* Move ball to intersection. */
557 /* Outgoing angle = 2 * line angle - incident angle. */
558 lang = atan2(line->end.x - line->start.x, line->end.y - line->start.y);
559 iang = atan2(ball->move.x, ball->move.y);
560 oang = 2 * lang - iang, 2*M_PI;
561 printf("lang = %u, iang = %u, oang=%u\n",
566 /* Set new direction for ball, at same speed */
567 speed = sqrt(ball->move.x * ball->move.x + ball->move.y * ball->move.y);
568 ball->move.x = round(sin(oang) * speed);
569 ball->move.y = round(cos(oang) * speed);
572 remainder->x = round(sin(oang) * len);
573 remainder->y = round(cos(oang) * len);
574 printf("len = %f, remainder = %li,%li\n", len, remainder->x, remainder->y);
577 static struct line_segment border[] = {
578 { { 0, 0, }, { MAX_X-1, 0 } },
579 { { MAX_X-1, 0, }, { MAX_X-1, MAX_Y-1 } },
580 { { MAX_X-1, MAX_Y-1, }, { 0, MAX_Y-1 } },
581 { { 0, MAX_Y-1, }, { 0, 0 } },
584 static inline float deg_to_rad(float degrees)
586 return degrees * M_PI / 180;
589 static SDL_Surface *sub_surface(SDL_Surface *screen, int w, int h)
591 return SDL_CreateRGBSurface(SDL_SWSURFACE, w, h,
592 screen->format->BitsPerPixel,
593 screen->format->Rmask,
594 screen->format->Gmask,
595 screen->format->Bmask,
596 screen->format->Amask);
599 static SDL_Surface *ball_surface(SDL_Surface *screen)
603 /* Just like the screen surface. */
604 ball = sub_surface(screen, 5, 5);
606 /* Lock the surface */
607 SDL_LockSurface(ball);
609 /* Black, white corners. */
610 memset(ball->pixels, 0x00, ball->pitch * 5);
611 line(ball, 0, 0, 0, 0, 0xFFFFFFFF);
612 line(ball, 4, 0, 4, 0, 0xFFFFFFFF);
613 line(ball, 0, 4, 0, 4, 0xFFFFFFFF);
614 line(ball, 4, 4, 4, 4, 0xFFFFFFFF);
615 SDL_UnlockSurface(ball);
620 int main(int argc, char *argv[])
623 struct line_segment lines[ARRAY_SIZE(border) + 1];
624 static int isect_count = 0;
625 SDL_Surface *screen, *ballsur, *savesur = NULL;
630 struct line_segment *last_l = NULL;
634 errx(1, "Usage: %s ballx bally ballangle ballspeed linestartx linestarty lineendx linendy\n"
635 " Where ballangle is 0 for north, 90 for east, etc\n",
638 ball.pos.x = atol(argv[1]);
639 ball.pos.y = atol(argv[2]);
640 ball.move.x = roundf(sin(deg_to_rad(atof(argv[3]))) * atol(argv[4]));
641 ball.move.y = roundf(cos(deg_to_rad(atof(argv[3]))) * atol(argv[4]));
642 printf("ball move = %li,%li\n", ball.move.x, ball.move.y);
644 memcpy(lines, border, sizeof(border));
645 lines[ARRAY_SIZE(border)].start.x = atof(argv[5]);
646 lines[ARRAY_SIZE(border)].start.y = atof(argv[6]);
647 lines[ARRAY_SIZE(border)].end.x = atof(argv[7]);
648 lines[ARRAY_SIZE(border)].end.y = atof(argv[8]);
651 if ( SDL_Init(SDL_INIT_VIDEO) < 0 ) {
652 fprintf(stderr, "Couldn't initialize SDL: %s\n",SDL_GetError());
657 videoflags = SDL_SWSURFACE;
659 /* Set 640x480 video mode */
660 if ( (screen=SDL_SetVideoMode(MAX_X,MAX_Y,video_bpp,videoflags)) == NULL ) {
661 errx(1, "Couldn't set %dx%dx%d video mode: %s",
662 MAX_X, MAX_Y,video_bpp, SDL_GetError());
665 /* Set the surface pixels and refresh! */
666 if ( SDL_LockSurface(screen) < 0 ) {
667 errx(1, "Couldn't lock the display surface: %s",
671 memset(screen->pixels, 0xFF, screen->pitch * screen->h);
674 for (i = 0; i < ARRAY_SIZE(lines); i++)
675 thick_line(screen, &lines[i]);
677 SDL_UnlockSurface(screen);
678 SDL_UpdateRect(screen, 0, 0, 0, 0);
680 ballsur = ball_surface(screen);
683 struct coord new, isect, move = ball.move;
684 struct line_segment *best_l;
690 rect.x = ball.pos.x - ballsur->w/2;
691 rect.y = ball.pos.y - ballsur->h/2;
697 best_d = MAX_X + MAX_Y;
699 new.x = ball.pos.x + move.x;
700 new.y = ball.pos.y + move.y;
702 printf("Ball at %li,%li (%li,%li)\n", ball.pos.x, ball.pos.y,
705 for (i = 0; i < ARRAY_SIZE(lines); i++) {
706 if (last_l == &lines[i])
708 if (intersect(&ball.pos, &new, &lines[i], &isect)) {
710 d = dist(&ball.pos, &isect);
718 printf("Ball bouncing off (%lu,%lu %lu,%lu)!\n",
719 best_l->start.x, best_l->start.y,
720 best_l->end.x, best_l->end.y);
721 bounce(&ball, best_l, &move);
722 /* Don't hit the same line twice. */
728 /* Restore what was there before ball. */
730 SDL_BlitSurface(savesur, NULL, screen, &rect);
731 SDL_UpdateRect(screen, rect.x, rect.y, rect.w, rect.h);
733 savesur = sub_surface(screen, ballsur->w, ballsur->h);
736 /* Save away image under new ball. */
739 rect.x = new.x - ballsur->w/2;
740 rect.y = new.y - ballsur->h/2;
741 SDL_BlitSurface(screen, &rect, savesur, NULL);
744 SDL_BlitSurface(ballsur, NULL, screen, &rect);
745 SDL_UpdateRect(screen, rect.x, rect.y, rect.w, rect.h);
746 /* That SDL_BlitSurface can crop rect. */
747 rect.x = new.x - ballsur->w/2;
748 rect.y = new.y - ballsur->h/2;
751 } while (!SDL_PollEvent(&event) || event.type != SDL_QUIT);