gridlinetraversal.h
Go to the documentation of this file.
00001 #ifndef GRIDLINETRAVERSAL_H
00002 #define GRIDLINETRAVERSAL_H
00003 
00004 #include <cstdlib>
00005 #include <gmapping/utils/point.h>
00006 
00007 namespace GMapping {
00008 
00009 typedef struct {
00010   int     num_points;
00011   IntPoint*  points;
00012 } GridLineTraversalLine;
00013 
00014 struct GridLineTraversal {
00015   inline static void gridLine( IntPoint start, IntPoint end, GridLineTraversalLine *line ) ;
00016   inline static void gridLineCore( IntPoint start, IntPoint end, GridLineTraversalLine *line ) ;
00017 
00018 };
00019 
00020 void GridLineTraversal::gridLineCore( IntPoint start, IntPoint end, GridLineTraversalLine *line )
00021 {
00022   int dx, dy, incr1, incr2, d, x, y, xend, yend, xdirflag, ydirflag;
00023   int cnt = 0;
00024 
00025   dx = abs(end.x-start.x); dy = abs(end.y-start.y);
00026   
00027   if (dy <= dx) {
00028     d = 2*dy - dx; incr1 = 2 * dy; incr2 = 2 * (dy - dx);
00029     if (start.x > end.x) {
00030       x = end.x; y = end.y;
00031       ydirflag = (-1);
00032       xend = start.x;
00033     } else {
00034       x = start.x; y = start.y;
00035       ydirflag = 1;
00036       xend = end.x;
00037     }
00038     line->points[cnt].x=x;
00039     line->points[cnt].y=y;
00040     cnt++;
00041     if (((end.y - start.y) * ydirflag) > 0) {
00042       while (x < xend) {
00043         x++;
00044         if (d <0) {
00045           d+=incr1;
00046         } else {
00047           y++; d+=incr2;
00048         }
00049         line->points[cnt].x=x;
00050         line->points[cnt].y=y;
00051         cnt++;
00052       }
00053     } else {
00054       while (x < xend) {
00055         x++;
00056         if (d <0) {
00057           d+=incr1;
00058         } else {
00059           y--; d+=incr2;
00060         }
00061         line->points[cnt].x=x;
00062         line->points[cnt].y=y;
00063         cnt++;
00064       }
00065     }           
00066   } else {
00067     d = 2*dx - dy;
00068     incr1 = 2*dx; incr2 = 2 * (dx - dy);
00069     if (start.y > end.y) {
00070       y = end.y; x = end.x;
00071       yend = start.y;
00072       xdirflag = (-1);
00073     } else {
00074       y = start.y; x = start.x;
00075       yend = end.y;
00076       xdirflag = 1;
00077     }
00078     line->points[cnt].x=x;
00079     line->points[cnt].y=y;
00080     cnt++;
00081     if (((end.x - start.x) * xdirflag) > 0) {
00082       while (y < yend) {
00083         y++;
00084         if (d <0) {
00085           d+=incr1;
00086         } else {
00087           x++; d+=incr2;
00088         }
00089         line->points[cnt].x=x;
00090         line->points[cnt].y=y;
00091         cnt++;
00092       }
00093     } else {
00094       while (y < yend) {
00095         y++;
00096         if (d <0) {
00097           d+=incr1;
00098         } else {
00099           x--; d+=incr2;
00100         }
00101         line->points[cnt].x=x;
00102         line->points[cnt].y=y;
00103         cnt++;
00104       }
00105     }
00106   }
00107   line->num_points = cnt;
00108 }
00109 
00110 void GridLineTraversal::gridLine( IntPoint start, IntPoint end, GridLineTraversalLine *line ) {
00111   int i,j;
00112   int half;
00113   IntPoint v;
00114   gridLineCore( start, end, line );
00115   if ( start.x!=line->points[0].x ||
00116        start.y!=line->points[0].y ) {
00117     half = line->num_points/2;
00118     for (i=0,j=line->num_points - 1;i<half; i++,j--) {
00119       v = line->points[i];
00120       line->points[i] = line->points[j];
00121       line->points[j] = v;
00122     }
00123   }
00124 }
00125 
00126 };
00127 
00128 #endif


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Thu Jun 6 2019 19:25:12