gridlinetraversal.h
Go to the documentation of this file.
1 #ifndef GRIDLINETRAVERSAL_H
2 #define GRIDLINETRAVERSAL_H
3 
4 #include <cstdlib>
5 #include <gmapping/utils/point.h>
6 
7 namespace GMapping {
8 
9 typedef struct {
13 
15  inline static void gridLine( IntPoint start, IntPoint end, GridLineTraversalLine *line ) ;
16  inline static void gridLineCore( IntPoint start, IntPoint end, GridLineTraversalLine *line ) ;
17 
18 };
19 
21 {
22  int dx, dy, incr1, incr2, d, x, y, xend, yend, xdirflag, ydirflag;
23  int cnt = 0;
24 
25  dx = abs(end.x-start.x); dy = abs(end.y-start.y);
26 
27  if (dy <= dx) {
28  d = 2*dy - dx; incr1 = 2 * dy; incr2 = 2 * (dy - dx);
29  if (start.x > end.x) {
30  x = end.x; y = end.y;
31  ydirflag = (-1);
32  xend = start.x;
33  } else {
34  x = start.x; y = start.y;
35  ydirflag = 1;
36  xend = end.x;
37  }
38  line->points[cnt].x=x;
39  line->points[cnt].y=y;
40  cnt++;
41  if (((end.y - start.y) * ydirflag) > 0) {
42  while (x < xend) {
43  x++;
44  if (d <0) {
45  d+=incr1;
46  } else {
47  y++; d+=incr2;
48  }
49  line->points[cnt].x=x;
50  line->points[cnt].y=y;
51  cnt++;
52  }
53  } else {
54  while (x < xend) {
55  x++;
56  if (d <0) {
57  d+=incr1;
58  } else {
59  y--; d+=incr2;
60  }
61  line->points[cnt].x=x;
62  line->points[cnt].y=y;
63  cnt++;
64  }
65  }
66  } else {
67  d = 2*dx - dy;
68  incr1 = 2*dx; incr2 = 2 * (dx - dy);
69  if (start.y > end.y) {
70  y = end.y; x = end.x;
71  yend = start.y;
72  xdirflag = (-1);
73  } else {
74  y = start.y; x = start.x;
75  yend = end.y;
76  xdirflag = 1;
77  }
78  line->points[cnt].x=x;
79  line->points[cnt].y=y;
80  cnt++;
81  if (((end.x - start.x) * xdirflag) > 0) {
82  while (y < yend) {
83  y++;
84  if (d <0) {
85  d+=incr1;
86  } else {
87  x++; d+=incr2;
88  }
89  line->points[cnt].x=x;
90  line->points[cnt].y=y;
91  cnt++;
92  }
93  } else {
94  while (y < yend) {
95  y++;
96  if (d <0) {
97  d+=incr1;
98  } else {
99  x--; d+=incr2;
100  }
101  line->points[cnt].x=x;
102  line->points[cnt].y=y;
103  cnt++;
104  }
105  }
106  }
107  line->num_points = cnt;
108 }
109 
111  int i,j;
112  int half;
113  IntPoint v;
114  gridLineCore( start, end, line );
115  if ( start.x!=line->points[0].x ||
116  start.y!=line->points[0].y ) {
117  half = line->num_points/2;
118  for (i=0,j=line->num_points - 1;i<half; i++,j--) {
119  v = line->points[i];
120  line->points[i] = line->points[j];
121  line->points[j] = v;
122  }
123  }
124 }
125 
126 };
127 
128 #endif
static void gridLineCore(IntPoint start, IntPoint end, GridLineTraversalLine *line)
static void gridLine(IntPoint start, IntPoint end, GridLineTraversalLine *line)


openslam_gmapping
Author(s): Cyrill Stachniss, Udo Frese, Giorgio Grisetti, Wolfram Burgard
autogenerated on Mon Feb 28 2022 22:59:20