00001
00002
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
00038 pw = width;
00039 ph = height;
00040
00041
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
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
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
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
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
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)
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
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
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
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
00268 fl_color(0,255,255);
00269 if (inc == 1 && pathlen > 0)
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