navfn: navfn: navwin.cpp Source 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               if (*pp > maxval)
00074                 v = 255;
00075               else
00076                 v = (int)((*pp/maxval) * 255.0);
00077               *ii++ = v;
00078               *ii++ = v;
00079               *ii++ = v;
00080             }
00081         }
00082 
00083 
00084       // draw obstacles
00085       for (int i=0; i<height-dec+1; i+=dec)
00086         {
00087           COSTTYPE *pp = cst + i*width;
00088           uchar *ii = im + 3*i/dec * nw;
00089           for (int j=0; j<width-dec+1; j+=dec, pp+=dec, ii+=3)
00090             {
00091               if (*pp >= COST_OBS)
00092                 {
00093                   *ii =     120;
00094                   *(ii+1) = 60;
00095                   *(ii+2) = 60;
00096                 }
00097             }
00098         }
00099 
00100     }
00101 
00102   else                          // expand
00103     {
00104       int ww = width; 
00105       while (ww < nw/2)
00106         {
00107           inc++;
00108           ww = width*inc;
00109         }
00110 
00111       int hh = height*inc;
00112       while (hh < nh/2)
00113         {
00114           inc++;
00115           hh = height*inc;
00116         }
00117 
00118       if (im == NULL)
00119         im = new uchar[nw*nh*3];
00120 
00121       for (int i=0; i<height; i++)
00122         {
00123           float *pp = pot + i*width;
00124           uchar *ii = im + 3*i*inc * nw;
00125           for (int j=0; j<width; j++, pp++)
00126             {
00127               int v;
00128               if (*pp > maxval)
00129                 v = 255;
00130               else
00131                 v = (int)((*pp/maxval) * 255.0);
00132               for (int k=0; k<inc; k++)
00133                 {
00134                   uchar *iii = ii + 3*j*inc + 3*k*nw;
00135                   for (int kk=0; kk<inc; kk++)
00136                     {
00137                       *iii++ = v;
00138                       *iii++ = v;
00139                       *iii++ = v;
00140                     }
00141                 }
00142             }
00143         }
00144     }
00145 
00146 
00147   make_current();
00148   fl_draw_image(im, 0,0,nw,nh);
00149 
00150   if (pc)
00151     delete [] pc;
00152   pce = nav->curPe;
00153   pc = new int[pce];
00154   memcpy(pc, nav->curP, pce*sizeof(int));
00155 
00156   if (pn)
00157     delete [] pn;
00158   pne = nav->nextPe;
00159   pn = new int[pne];
00160   memcpy(pn, nav->nextP, pne*sizeof(int));
00161 
00162   if (po)
00163     delete [] po;
00164   poe = nav->overPe;
00165   po = new int[poe];
00166   memcpy(po, nav->overP, poe*sizeof(int));
00167 
00168   // start and goal
00169   goal[0] = nav->goal[0];
00170   goal[1] = nav->goal[1];
00171   start[0] = nav->start[0];
00172   start[1] = nav->start[1];
00173 
00174   // path
00175   if (nav->npath > 0)
00176     {
00177       pathlen = nav->npath;
00178       if (pathbuflen < pathlen)
00179         {
00180           pathbuflen = pathlen;
00181           if (path) delete [] path;
00182           path = new int[pathbuflen];
00183         }
00184       for (int i=0; i<pathlen; i++)
00185         path[i] = width*(int)(nav->pathy[i])+(int)(nav->pathx[i]);
00186     }
00187 
00188   drawOverlay();
00189 
00190   redraw();
00191 }
00192 
00193 
00194 void 
00195 NavWin::drawOverlay()
00196 {
00197   if (inc == 1)                 // decimation
00198     {
00199       fl_color(255,0,0);
00200       if (pce > 0)
00201         for (int i=0; i<pce; i++)
00202           {
00203             int y = pc[i]/pw;
00204             int x = pc[i]%pw;
00205             fl_point(x/dec,y/dec);
00206           }
00207       fl_color(255,0,0);
00208       if (pne > 0)
00209         for (int i=0; i<pne; i++)
00210           {
00211             int y = pn[i]/pw;
00212             int x = pn[i]%pw;
00213             fl_point(x/dec,y/dec);
00214           }
00215       fl_color(0,255,0);
00216       if (poe > 0)
00217         for (int i=0; i<poe; i++)
00218           {
00219             int y = po[i]/pw;
00220             int x = po[i]%pw;
00221             fl_point(x/dec,y/dec);
00222           }
00223     }
00224   else                          // expansion
00225     {
00226       fl_color(255,0,0);
00227       if (pce > 0)
00228         for (int i=0; i<pce; i++)
00229           {
00230             int y = pc[i]/pw;
00231             int x = pc[i]%pw;
00232             fl_rect(x*inc,y*inc,inc,inc);
00233           }
00234       fl_color(255,0,0);
00235       if (pne > 0)
00236         for (int i=0; i<pne; i++)
00237           {
00238             int y = pn[i]/pw;
00239             int x = pn[i]%pw;
00240             fl_rect(x*inc,y*inc,inc,inc);
00241           }
00242       fl_color(0,255,0);
00243       if (poe > 0)
00244         for (int i=0; i<poe; i++)
00245           {
00246             int y = po[i]/pw;
00247             int x = po[i]%pw;
00248             fl_rect(x*inc,y*inc,inc,inc);
00249           }
00250     }
00251 
00252   // draw the goal
00253   fl_color(255,200,0);
00254   int x = goal[0];
00255   int y = goal[1];
00256   if (inc == 1)
00257     fl_rectf(x/dec-2,y/dec-2,5,5);      
00258   else
00259     fl_rectf(x*inc-2,y*inc-2,5,5);      
00260 
00261   // draw the start
00262   fl_color(0,255,0);
00263   x = start[0];
00264   y = start[1];
00265   if (inc == 1)
00266     fl_rectf(x/dec-2,y/dec-2,5,5);      
00267   else
00268     fl_rectf(x*inc-2,y*inc-2,5,5);      
00269 
00270   // draw the path
00271   fl_color(0,255,255);
00272   if (inc == 1 && pathlen > 0)  // decimation or equal pixels
00273     {
00274       int y = path[0]/pw;
00275       int x = path[0]%pw;
00276       for (int i=1; i<pathlen; i++)
00277         {
00278           int y1 = path[i]/pw;
00279           int x1 = path[i]%pw;
00280           fl_line(x/dec,y/dec,x1/dec,y1/dec);
00281           x = x1;
00282           y = y1;
00283         }
00284     }
00285 }
00286 
00287 
00288 void NavWin::draw()
00289 {
00290   if (im)
00291     fl_draw_image(im, 0,0,nw,nh);
00292   drawOverlay();
00293 }
00294 };
00295