navwin.cpp
Go to the documentation of this file.
00001 //
00002 // simple timing test of the nav fn planner
00003 // 
00004 
00005 #include <navfn/navwin.h>
00006 #include <string.h>
00007 
00008 namespace navfn {
00009 NavWin::NavWin(int w, int h, const char *name)
00010   : Fl_Double_Window(w,h,name)
00011 {
00012   nw = w;
00013   nh = h;
00014   dec = 1;
00015   maxval = 90*1000;
00016   im = NULL;
00017   pw = w;
00018   ph = h;
00019   pce = pne = poe = 0;
00020   pc = pn = po = NULL;
00021   pathlen = pathbuflen = 0;
00022   path = NULL;
00023 }
00024 
00025 NavWin::~NavWin()
00026 {
00027 }
00028 
00029 void
00030 NavWin::drawPot(NavFn *nav)
00031 {
00032   float *pot = nav->potarr;
00033   COSTTYPE *cst = nav->costarr;
00034   int width = nav->nx;
00035   int height = nav->ny;
00036 
00037   // params
00038   pw = width;
00039   ph = height;
00040 
00041   // figure out decimation or expansion to fit
00042   dec = 1;
00043   inc = 1;
00044 
00045   if (width >= nw/2)
00046     {
00047       int ww = width; 
00048       while (ww > nw)
00049         {
00050           dec++;
00051           ww = width/dec;
00052         }
00053 
00054       int hh = height/dec;
00055       while (hh > nh)
00056         {
00057           dec++;
00058           hh = height/dec;
00059         }
00060 
00061       if (im == NULL)
00062         im = new uchar[nw*nh*3];
00063 
00064 
00065       // draw potential
00066       for (int i=0; i<height-dec+1; i+=dec)
00067         {
00068           float *pp = pot + i*width;
00069           uchar *ii = im + 3*i/dec * nw;
00070           for (int j=0; j<width-dec+1; j+=dec, pp+=dec)
00071             {
00072               int v;
00073               v = (int)((*pp/maxval) * 255.0);
00074               *ii++ = v;
00075               *ii++ = v;
00076               *ii++ = v;
00077             }
00078         }
00079 
00080 
00081       // draw obstacles
00082       for (int i=0; i<height-dec+1; i+=dec)
00083         {
00084           COSTTYPE *pp = cst + i*width;
00085           uchar *ii = im + 3*i/dec * nw;
00086           for (int j=0; j<width-dec+1; j+=dec, pp+=dec, ii+=3)
00087             {
00088               if (*pp >= COST_OBS)
00089                 {
00090                   *ii =     120;
00091                   *(ii+1) = 60;
00092                   *(ii+2) = 60;
00093                 }
00094             }
00095         }
00096 
00097     }
00098 
00099   else                          // expand
00100     {
00101       int ww = width; 
00102       while (ww < nw/2)
00103         {
00104           inc++;
00105           ww = width*inc;
00106         }
00107 
00108       int hh = height*inc;
00109       while (hh < nh/2)
00110         {
00111           inc++;
00112           hh = height*inc;
00113         }
00114 
00115       if (im == NULL)
00116         im = new uchar[nw*nh*3];
00117 
00118       for (int i=0; i<height; i++)
00119         {
00120           float *pp = pot + i*width;
00121           uchar *ii = im + 3*i*inc * nw;
00122           for (int j=0; j<width; j++, pp++)
00123             {
00124               int v;
00125               if (*pp > maxval)
00126                 v = 255;
00127               else
00128                 v = (int)((*pp/maxval) * 255.0);
00129               for (int k=0; k<inc; k++)
00130                 {
00131                   uchar *iii = ii + 3*j*inc + 3*k*nw;
00132                   for (int kk=0; kk<inc; kk++)
00133                     {
00134                       *iii++ = v;
00135                       *iii++ = v;
00136                       *iii++ = v;
00137                     }
00138                 }
00139             }
00140         }
00141     }
00142 
00143 
00144   make_current();
00145   fl_draw_image(im, 0,0,nw,nh);
00146 
00147   if (pc)
00148     delete [] pc;
00149   pce = nav->curPe;
00150   pc = new int[pce];
00151   memcpy(pc, nav->curP, pce*sizeof(int));
00152 
00153   if (pn)
00154     delete [] pn;
00155   pne = nav->nextPe;
00156   pn = new int[pne];
00157   memcpy(pn, nav->nextP, pne*sizeof(int));
00158 
00159   if (po)
00160     delete [] po;
00161   poe = nav->overPe;
00162   po = new int[poe];
00163   memcpy(po, nav->overP, poe*sizeof(int));
00164 
00165   // start and goal
00166   goal[0] = nav->goal[0];
00167   goal[1] = nav->goal[1];
00168   start[0] = nav->start[0];
00169   start[1] = nav->start[1];
00170 
00171   // path
00172   if (nav->npath > 0)
00173     {
00174       pathlen = nav->npath;
00175       if (pathbuflen < pathlen)
00176         {
00177           pathbuflen = pathlen;
00178           if (path) delete [] path;
00179           path = new int[pathbuflen];
00180         }
00181       for (int i=0; i<pathlen; i++)
00182         path[i] = width*(int)(nav->pathy[i])+(int)(nav->pathx[i]);
00183     }
00184 
00185   drawOverlay();
00186 
00187   redraw();
00188 }
00189 
00190 
00191 void 
00192 NavWin::drawOverlay()
00193 {
00194   if (inc == 1)                 // decimation
00195     {
00196       fl_color(255,0,0);
00197       if (pce > 0)
00198         for (int i=0; i<pce; i++)
00199           {
00200             int y = pc[i]/pw;
00201             int x = pc[i]%pw;
00202             fl_point(x/dec,y/dec);
00203           }
00204       fl_color(255,0,0);
00205       if (pne > 0)
00206         for (int i=0; i<pne; i++)
00207           {
00208             int y = pn[i]/pw;
00209             int x = pn[i]%pw;
00210             fl_point(x/dec,y/dec);
00211           }
00212       fl_color(0,255,0);
00213       if (poe > 0)
00214         for (int i=0; i<poe; i++)
00215           {
00216             int y = po[i]/pw;
00217             int x = po[i]%pw;
00218             fl_point(x/dec,y/dec);
00219           }
00220     }
00221   else                          // expansion
00222     {
00223       fl_color(255,0,0);
00224       if (pce > 0)
00225         for (int i=0; i<pce; i++)
00226           {
00227             int y = pc[i]/pw;
00228             int x = pc[i]%pw;
00229             fl_rect(x*inc,y*inc,inc,inc);
00230           }
00231       fl_color(255,0,0);
00232       if (pne > 0)
00233         for (int i=0; i<pne; i++)
00234           {
00235             int y = pn[i]/pw;
00236             int x = pn[i]%pw;
00237             fl_rect(x*inc,y*inc,inc,inc);
00238           }
00239       fl_color(0,255,0);
00240       if (poe > 0)
00241         for (int i=0; i<poe; i++)
00242           {
00243             int y = po[i]/pw;
00244             int x = po[i]%pw;
00245             fl_rect(x*inc,y*inc,inc,inc);
00246           }
00247     }
00248 
00249   // draw the goal
00250   fl_color(255,200,0);
00251   int x = goal[0];
00252   int y = goal[1];
00253   if (inc == 1)
00254     fl_rectf(x/dec-2,y/dec-2,5,5);      
00255   else
00256     fl_rectf(x*inc-2,y*inc-2,5,5);      
00257 
00258   // draw the start
00259   fl_color(0,255,0);
00260   x = start[0];
00261   y = start[1];
00262   if (inc == 1)
00263     fl_rectf(x/dec-2,y/dec-2,5,5);      
00264   else
00265     fl_rectf(x*inc-2,y*inc-2,5,5);      
00266 
00267   // draw the path
00268   fl_color(0,255,255);
00269   if (inc == 1 && pathlen > 0)  // decimation or equal pixels
00270     {
00271       int y = path[0]/pw;
00272       int x = path[0]%pw;
00273       for (int i=1; i<pathlen; i++)
00274         {
00275           int y1 = path[i]/pw;
00276           int x1 = path[i]%pw;
00277           fl_line(x/dec,y/dec,x1/dec,y1/dec);
00278           x = x1;
00279           y = y1;
00280         }
00281     }
00282 }
00283 
00284 
00285 void NavWin::draw()
00286 {
00287   if (im)
00288     fl_draw_image(im, 0,0,nw,nh);
00289   drawOverlay();
00290 }
00291 };
00292 


navfn
Author(s): Kurt Konolige, Eitan Marder-Eppstein, contradict@gmail.com
autogenerated on Thu Aug 27 2015 14:07:27