navwin.cpp
Go to the documentation of this file.
1 //
2 // simple timing test of the nav fn planner
3 //
4 
5 #include <navfn/navwin.h>
6 #include <string.h>
7 
8 namespace navfn {
9 NavWin::NavWin(int w, int h, const char *name)
10  : Fl_Double_Window(w,h,name)
11 {
12  nw = w;
13  nh = h;
14  dec = 1;
15  maxval = 90*1000;
16  im = NULL;
17  pw = w;
18  ph = h;
19  pce = pne = poe = 0;
20  pc = pn = po = NULL;
21  pathlen = pathbuflen = 0;
22  path = NULL;
23 }
24 
26 {
27 }
28 
29 void
31 {
32  float *pot = nav->potarr;
33  COSTTYPE *cst = nav->costarr;
34  int width = nav->nx;
35  int height = nav->ny;
36 
37  // params
38  pw = width;
39  ph = height;
40 
41  // figure out decimation or expansion to fit
42  dec = 1;
43  inc = 1;
44 
45  if (width >= nw/2)
46  {
47  int ww = width;
48  while (ww > nw)
49  {
50  dec++;
51  ww = width/dec;
52  }
53 
54  int hh = height/dec;
55  while (hh > nh)
56  {
57  dec++;
58  hh = height/dec;
59  }
60 
61  if (im == NULL)
62  im = new uchar[nw*nh*3];
63 
64 
65  // draw potential
66  for (int i=0; i<height-dec+1; i+=dec)
67  {
68  float *pp = pot + i*width;
69  uchar *ii = im + 3*i/dec * nw;
70  for (int j=0; j<width-dec+1; j+=dec, pp+=dec)
71  {
72  int v;
73  v = (int)((*pp/maxval) * 255.0);
74  *ii++ = v;
75  *ii++ = v;
76  *ii++ = v;
77  }
78  }
79 
80 
81  // draw obstacles
82  for (int i=0; i<height-dec+1; i+=dec)
83  {
84  COSTTYPE *pp = cst + i*width;
85  uchar *ii = im + 3*i/dec * nw;
86  for (int j=0; j<width-dec+1; j+=dec, pp+=dec, ii+=3)
87  {
88  if (*pp >= COST_OBS)
89  {
90  *ii = 120;
91  *(ii+1) = 60;
92  *(ii+2) = 60;
93  }
94  }
95  }
96 
97  }
98 
99  else // expand
100  {
101  int ww = width;
102  while (ww < nw/2)
103  {
104  inc++;
105  ww = width*inc;
106  }
107 
108  int hh = height*inc;
109  while (hh < nh/2)
110  {
111  inc++;
112  hh = height*inc;
113  }
114 
115  if (im == NULL)
116  im = new uchar[nw*nh*3];
117 
118  for (int i=0; i<height; i++)
119  {
120  float *pp = pot + i*width;
121  uchar *ii = im + 3*i*inc * nw;
122  for (int j=0; j<width; j++, pp++)
123  {
124  int v;
125  if (*pp > maxval)
126  v = 255;
127  else
128  v = (int)((*pp/maxval) * 255.0);
129  for (int k=0; k<inc; k++)
130  {
131  uchar *iii = ii + 3*j*inc + 3*k*nw;
132  for (int kk=0; kk<inc; kk++)
133  {
134  *iii++ = v;
135  *iii++ = v;
136  *iii++ = v;
137  }
138  }
139  }
140  }
141  }
142 
143 
144  make_current();
145  fl_draw_image(im, 0,0,nw,nh);
146 
147  if (pc)
148  delete [] pc;
149  pce = nav->curPe;
150  pc = new int[pce];
151  memcpy(pc, nav->curP, pce*sizeof(int));
152 
153  if (pn)
154  delete [] pn;
155  pne = nav->nextPe;
156  pn = new int[pne];
157  memcpy(pn, nav->nextP, pne*sizeof(int));
158 
159  if (po)
160  delete [] po;
161  poe = nav->overPe;
162  po = new int[poe];
163  memcpy(po, nav->overP, poe*sizeof(int));
164 
165  // start and goal
166  goal[0] = nav->goal[0];
167  goal[1] = nav->goal[1];
168  start[0] = nav->start[0];
169  start[1] = nav->start[1];
170 
171  // path
172  if (nav->npath > 0)
173  {
174  pathlen = nav->npath;
175  if (pathbuflen < pathlen)
176  {
178  if (path) delete [] path;
179  path = new int[pathbuflen];
180  }
181  for (int i=0; i<pathlen; i++)
182  path[i] = width*(int)(nav->pathy[i])+(int)(nav->pathx[i]);
183  }
184 
185  drawOverlay();
186 
187  redraw();
188 }
189 
190 
191 void
193 {
194  if (inc == 1) // decimation
195  {
196  fl_color(255,0,0);
197  if (pce > 0)
198  for (int i=0; i<pce; i++)
199  {
200  int y = pc[i]/pw;
201  int x = pc[i]%pw;
202  fl_point(x/dec,y/dec);
203  }
204  fl_color(255,0,0);
205  if (pne > 0)
206  for (int i=0; i<pne; i++)
207  {
208  int y = pn[i]/pw;
209  int x = pn[i]%pw;
210  fl_point(x/dec,y/dec);
211  }
212  fl_color(0,255,0);
213  if (poe > 0)
214  for (int i=0; i<poe; i++)
215  {
216  int y = po[i]/pw;
217  int x = po[i]%pw;
218  fl_point(x/dec,y/dec);
219  }
220  }
221  else // expansion
222  {
223  fl_color(255,0,0);
224  if (pce > 0)
225  for (int i=0; i<pce; i++)
226  {
227  int y = pc[i]/pw;
228  int x = pc[i]%pw;
229  fl_rect(x*inc,y*inc,inc,inc);
230  }
231  fl_color(255,0,0);
232  if (pne > 0)
233  for (int i=0; i<pne; i++)
234  {
235  int y = pn[i]/pw;
236  int x = pn[i]%pw;
237  fl_rect(x*inc,y*inc,inc,inc);
238  }
239  fl_color(0,255,0);
240  if (poe > 0)
241  for (int i=0; i<poe; i++)
242  {
243  int y = po[i]/pw;
244  int x = po[i]%pw;
245  fl_rect(x*inc,y*inc,inc,inc);
246  }
247  }
248 
249  // draw the goal
250  fl_color(255,200,0);
251  int x = goal[0];
252  int y = goal[1];
253  if (inc == 1)
254  fl_rectf(x/dec-2,y/dec-2,5,5);
255  else
256  fl_rectf(x*inc-2,y*inc-2,5,5);
257 
258  // draw the start
259  fl_color(0,255,0);
260  x = start[0];
261  y = start[1];
262  if (inc == 1)
263  fl_rectf(x/dec-2,y/dec-2,5,5);
264  else
265  fl_rectf(x*inc-2,y*inc-2,5,5);
266 
267  // draw the path
268  fl_color(0,255,255);
269  if (inc == 1 && pathlen > 0) // decimation or equal pixels
270  {
271  int y = path[0]/pw;
272  int x = path[0]%pw;
273  for (int i=1; i<pathlen; i++)
274  {
275  int y1 = path[i]/pw;
276  int x1 = path[i]%pw;
277  fl_line(x/dec,y/dec,x1/dec,y1/dec);
278  x = x1;
279  y = y1;
280  }
281  }
282 }
283 
284 
286 {
287  if (im)
288  fl_draw_image(im, 0,0,nw,nh);
289  drawOverlay();
290 }
291 };
292 
NavWin(int w, int h, const char *name)
Definition: navwin.cpp:9
int goal[2]
Definition: navwin.h:37
Definition: navfn.h:81
uchar * im
Definition: navwin.h:34
float maxval
Definition: navwin.h:29
int * pc
Definition: navwin.h:35
int npath
Definition: navfn.h:242
int nextPe
Definition: navfn.h:178
int * curP
Definition: navfn.h:177
int overPe
Definition: navfn.h:178
float * pathx
Definition: navfn.h:241
TFSIMD_FORCE_INLINE const tfScalar & y() const
int nx
Definition: navfn.h:124
int pathlen
Definition: navwin.h:40
int * po
Definition: navwin.h:35
float * potarr
Definition: navfn.h:171
COSTTYPE * costarr
Definition: navfn.h:170
int * path
Definition: navwin.h:39
void drawPot(NavFn *nav)
Definition: navwin.cpp:30
Navigation function class. Holds buffers for costmap, navfn map. Maps are pixel-based. Origin is upper left, x is right, y is down.
Definition: navfn.h:106
int * pn
Definition: navwin.h:35
int goal[2]
Definition: navfn.h:197
int start[2]
Definition: navfn.h:198
void drawOverlay()
Definition: navwin.cpp:192
TFSIMD_FORCE_INLINE const tfScalar & x() const
float * pathy
Definition: navfn.h:241
void draw()
Definition: navwin.cpp:285
int start[2]
Definition: navwin.h:38
int pathbuflen
Definition: navwin.h:41
int curPe
Definition: navfn.h:178
int * overP
Definition: navfn.h:177
int * nextP
Definition: navfn.h:177
int ny
Definition: navfn.h:124


navfn
Author(s): Kurt Konolige, Eitan Marder-Eppstein, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:06:04