dynamicvoronoi.cpp
Go to the documentation of this file.
00001 #include "dynamicvoronoi.h"
00002 
00003 #include <math.h>
00004 #include <iostream>
00005 
00006 DynamicVoronoi::DynamicVoronoi() {
00007   sqrt2 = sqrt(2.0);
00008   data = NULL;
00009   gridMap = NULL;
00010 }
00011 
00012 DynamicVoronoi::~DynamicVoronoi() {
00013   if (data) {
00014     for (int x=0; x<sizeX; x++) delete[] data[x];
00015     delete[] data;
00016   }
00017   if (gridMap) {
00018     for (int x=0; x<sizeX; x++) delete[] gridMap[x];
00019     delete[] gridMap;
00020   }
00021 }
00022 
00023 void DynamicVoronoi::initializeEmpty(int _sizeX, int _sizeY, bool initGridMap) {
00024   sizeX = _sizeX;
00025   sizeY = _sizeY;
00026   if (data) {
00027     for (int x=0; x<sizeX; x++) delete[] data[x];
00028     delete[] data;
00029   }
00030   data = new dataCell*[sizeX];
00031   for (int x=0; x<sizeX; x++) data[x] = new dataCell[sizeY];
00032 
00033   if (initGridMap) {
00034     if (gridMap) {
00035       for (int x=0; x<sizeX; x++) delete[] gridMap[x];
00036       delete[] gridMap;
00037     }
00038     gridMap = new bool*[sizeX];
00039     for (int x=0; x<sizeX; x++) gridMap[x] = new bool[sizeY];
00040   }
00041   
00042   dataCell c;
00043   c.dist = INFINITY;
00044   c.sqdist = INT_MAX;
00045   c.obstX = invalidObstData;
00046   c.obstY = invalidObstData;
00047   c.voronoi = free;
00048   c.queueing = fwNotQueued;
00049   c.needsRaise = false;
00050 
00051   for (int x=0; x<sizeX; x++)
00052     for (int y=0; y<sizeY; y++) data[x][y] = c;
00053 
00054   if (initGridMap) {
00055     for (int x=0; x<sizeX; x++) 
00056       for (int y=0; y<sizeY; y++) gridMap[x][y] = 0;
00057   }
00058 }
00059 
00060 void DynamicVoronoi::initializeMap(int _sizeX, int _sizeY, bool** _gridMap) {
00061   gridMap = _gridMap;
00062   initializeEmpty(_sizeX, _sizeY, false);
00063 
00064   for (int x=0; x<sizeX; x++) {
00065     for (int y=0; y<sizeY; y++) {
00066       if (gridMap[x][y]) {
00067         dataCell c = data[x][y];
00068         if (!isOccupied(x,y,c)) {
00069           
00070           bool isSurrounded = true;
00071           for (int dx=-1; dx<=1; dx++) {
00072             int nx = x+dx;
00073             if (nx<=0 || nx>=sizeX-1) continue;
00074             for (int dy=-1; dy<=1; dy++) {
00075               if (dx==0 && dy==0) continue;
00076               int ny = y+dy;
00077               if (ny<=0 || ny>=sizeY-1) continue;
00078 
00079               if (!gridMap[nx][ny]) {
00080                 isSurrounded = false;
00081                 break;
00082               }
00083             }
00084           }
00085           if (isSurrounded) {
00086             c.obstX = x;
00087             c.obstY = y;
00088             c.sqdist = 0;
00089             c.dist=0;
00090             c.voronoi=occupied;
00091             c.queueing = fwProcessed;
00092             data[x][y] = c;
00093           } else setObstacle(x,y);
00094         }
00095       }
00096     }
00097   }
00098 }
00099 
00100 void DynamicVoronoi::occupyCell(int x, int y) {
00101   gridMap[x][y] = 1;
00102   setObstacle(x,y);
00103 }
00104 void DynamicVoronoi::clearCell(int x, int y) {
00105   gridMap[x][y] = 0;
00106   removeObstacle(x,y);
00107 }
00108 
00109 void DynamicVoronoi::setObstacle(int x, int y) {
00110   dataCell c = data[x][y];
00111   if(isOccupied(x,y,c)) return;
00112   
00113   addList.push_back(INTPOINT(x,y));
00114   c.obstX = x;
00115   c.obstY = y;
00116   data[x][y] = c;
00117 }
00118 
00119 void DynamicVoronoi::removeObstacle(int x, int y) {
00120   dataCell c = data[x][y];
00121   if(isOccupied(x,y,c) == false) return;
00122 
00123   removeList.push_back(INTPOINT(x,y));
00124   c.obstX = invalidObstData;
00125   c.obstY  = invalidObstData;    
00126   c.queueing = bwQueued;
00127   data[x][y] = c;
00128 }
00129 
00130 void DynamicVoronoi::exchangeObstacles(std::vector<INTPOINT> points) {
00131 
00132   for (unsigned int i=0; i<lastObstacles.size(); i++) {
00133     int x = lastObstacles[i].x;
00134     int y = lastObstacles[i].y;
00135 
00136     bool v = gridMap[x][y];
00137     if (v) continue;
00138     removeObstacle(x,y);
00139   }  
00140 
00141   lastObstacles.clear();
00142 
00143   for (unsigned int i=0; i<points.size(); i++) {
00144     int x = points[i].x;
00145     int y = points[i].y;
00146     bool v = gridMap[x][y];
00147     if (v) continue;
00148     setObstacle(x,y);
00149     lastObstacles.push_back(points[i]);
00150   }  
00151 }
00152 
00153 void DynamicVoronoi::update(bool updateRealDist) {
00154 
00155   commitAndColorize(updateRealDist);
00156 
00157   while (!open.empty()) {
00158     INTPOINT p = open.pop();
00159     int x = p.x;
00160     int y = p.y;
00161     dataCell c = data[x][y];
00162 
00163     if(c.queueing==fwProcessed) continue; 
00164 
00165     if (c.needsRaise) {
00166       // RAISE
00167       for (int dx=-1; dx<=1; dx++) {
00168         int nx = x+dx;
00169         if (nx<=0 || nx>=sizeX-1) continue;
00170         for (int dy=-1; dy<=1; dy++) {
00171           if (dx==0 && dy==0) continue;
00172           int ny = y+dy;
00173           if (ny<=0 || ny>=sizeY-1) continue;
00174           dataCell nc = data[nx][ny];
00175           if (nc.obstX!=invalidObstData && !nc.needsRaise) {
00176             if(!isOccupied(nc.obstX,nc.obstY,data[nc.obstX][nc.obstY])) {
00177               open.push(nc.sqdist, INTPOINT(nx,ny));
00178               nc.queueing = fwQueued;
00179               nc.needsRaise = true;
00180               nc.obstX = invalidObstData;
00181               nc.obstY = invalidObstData;
00182               if (updateRealDist) nc.dist = INFINITY;
00183               nc.sqdist = INT_MAX;
00184               data[nx][ny] = nc;
00185             } else {
00186               if(nc.queueing != fwQueued){
00187                 open.push(nc.sqdist, INTPOINT(nx,ny));
00188                 nc.queueing = fwQueued;
00189                 data[nx][ny] = nc;
00190               }
00191             }      
00192           }
00193         }
00194       }
00195       c.needsRaise = false;
00196       c.queueing = bwProcessed;
00197       data[x][y] = c;
00198     }
00199     else if (c.obstX != invalidObstData && isOccupied(c.obstX,c.obstY,data[c.obstX][c.obstY])) {
00200 
00201       // LOWER
00202       c.queueing = fwProcessed;
00203       c.voronoi = occupied;
00204 
00205       for (int dx=-1; dx<=1; dx++) {
00206         int nx = x+dx;
00207         if (nx<=0 || nx>=sizeX-1) continue;
00208         for (int dy=-1; dy<=1; dy++) {
00209           if (dx==0 && dy==0) continue;
00210           int ny = y+dy;
00211           if (ny<=0 || ny>=sizeY-1) continue;
00212           dataCell nc = data[nx][ny];
00213           if(!nc.needsRaise) {
00214             int distx = nx-c.obstX;
00215             int disty = ny-c.obstY;
00216             int newSqDistance = distx*distx + disty*disty;              
00217             bool overwrite =  (newSqDistance < nc.sqdist);
00218             if(!overwrite && newSqDistance==nc.sqdist) { 
00219               if (nc.obstX == invalidObstData || isOccupied(nc.obstX,nc.obstY,data[nc.obstX][nc.obstY])==false) overwrite = true;
00220             }
00221             if (overwrite) {
00222               open.push(newSqDistance, INTPOINT(nx,ny));
00223               nc.queueing = fwQueued;
00224               if (updateRealDist) {
00225                 nc.dist = sqrt((double) newSqDistance);
00226               }
00227               nc.sqdist = newSqDistance;
00228               nc.obstX = c.obstX;
00229               nc.obstY = c.obstY;
00230             } else { 
00231               checkVoro(x,y,nx,ny,c,nc);
00232             }
00233             data[nx][ny] = nc;
00234           }
00235         }
00236       }
00237     }
00238     data[x][y] = c;
00239   }
00240 }
00241 
00242 float DynamicVoronoi::getDistance( int x, int y ) {
00243   if( (x>0) && (x<sizeX) && (y>0) && (y<sizeY)) return data[x][y].dist; 
00244   else return -INFINITY;
00245 }
00246 
00247 bool DynamicVoronoi::isVoronoi( int x, int y ) {
00248   dataCell c = data[x][y];
00249   return (c.voronoi==free || c.voronoi==voronoiKeep);
00250 }
00251 
00252 
00253 void DynamicVoronoi::commitAndColorize(bool updateRealDist) {
00254   // ADD NEW OBSTACLES
00255   for (unsigned int i=0; i<addList.size(); i++) {
00256     INTPOINT p = addList[i];
00257     int x = p.x;
00258     int y = p.y;
00259     dataCell c = data[x][y];
00260 
00261     if(c.queueing != fwQueued){
00262       if (updateRealDist) c.dist = 0;
00263       c.sqdist = 0;
00264       c.obstX = x;
00265       c.obstY = y;
00266       c.queueing = fwQueued;
00267       c.voronoi = occupied;
00268       data[x][y] = c;
00269       open.push(0, INTPOINT(x,y));
00270     }
00271   }
00272 
00273   // REMOVE OLD OBSTACLES
00274   for (unsigned int i=0; i<removeList.size(); i++) {
00275     INTPOINT p = removeList[i];
00276     int x = p.x;
00277     int y = p.y;
00278     dataCell c = data[x][y];
00279 
00280     if (isOccupied(x,y,c)==true) continue; // obstacle was removed and reinserted
00281     open.push(0, INTPOINT(x,y));
00282     if (updateRealDist) c.dist  = INFINITY;
00283     c.sqdist = INT_MAX;
00284     c.needsRaise = true;
00285     data[x][y] = c;
00286   }
00287   removeList.clear();
00288   addList.clear();
00289 }
00290 
00291 
00292 void DynamicVoronoi::checkVoro(int x, int y, int nx, int ny, dataCell& c, dataCell& nc) {
00293 
00294   if ((c.sqdist>1 || nc.sqdist>1) && nc.obstX!=invalidObstData) { 
00295     if (abs(c.obstX-nc.obstX) > 1 || abs(c.obstY-nc.obstY) > 1) {
00296       //compute dist from x,y to obstacle of nx,ny       
00297       int dxy_x = x-nc.obstX;
00298       int dxy_y = y-nc.obstY;
00299       int sqdxy = dxy_x*dxy_x + dxy_y*dxy_y;
00300       int stability_xy = sqdxy - c.sqdist;
00301       if (sqdxy - c.sqdist<0) return;
00302 
00303       //compute dist from nx,ny to obstacle of x,y
00304       int dnxy_x = nx - c.obstX;
00305       int dnxy_y = ny - c.obstY;
00306       int sqdnxy = dnxy_x*dnxy_x + dnxy_y*dnxy_y;
00307       int stability_nxy = sqdnxy - nc.sqdist;
00308       if (sqdnxy - nc.sqdist <0) return;
00309 
00310       //which cell is added to the Voronoi diagram?
00311       if(stability_xy <= stability_nxy && c.sqdist>2) {
00312         if (c.voronoi != free) {
00313           c.voronoi = free;
00314           reviveVoroNeighbors(x,y);
00315           pruneQueue.push(INTPOINT(x,y));
00316         }
00317       }
00318       if(stability_nxy <= stability_xy && nc.sqdist>2) {
00319         if (nc.voronoi != free) {
00320           nc.voronoi = free;
00321           reviveVoroNeighbors(nx,ny);
00322           pruneQueue.push(INTPOINT(nx,ny));
00323         }
00324       }
00325     }
00326   }
00327 }
00328 
00329 
00330 void DynamicVoronoi::reviveVoroNeighbors(int &x, int &y) {
00331   for (int dx=-1; dx<=1; dx++) {
00332     int nx = x+dx;
00333     if (nx<=0 || nx>=sizeX-1) continue;
00334     for (int dy=-1; dy<=1; dy++) {
00335       if (dx==0 && dy==0) continue;
00336       int ny = y+dy;
00337       if (ny<=0 || ny>=sizeY-1) continue;
00338       dataCell nc = data[nx][ny];
00339       if (nc.sqdist != INT_MAX && !nc.needsRaise && (nc.voronoi == voronoiKeep || nc.voronoi == voronoiPrune)) {
00340         nc.voronoi = free;
00341         data[nx][ny] = nc;
00342         pruneQueue.push(INTPOINT(nx,ny));
00343       }
00344     }
00345   }
00346 }
00347 
00348 
00349 bool DynamicVoronoi::isOccupied(int x, int y) {
00350   dataCell c = data[x][y];
00351   return (c.obstX==x && c.obstY==y);
00352 }
00353 
00354 bool DynamicVoronoi::isOccupied(int &x, int &y, dataCell &c) { 
00355   return (c.obstX==x && c.obstY==y);
00356 }
00357 
00358 void DynamicVoronoi::visualize(const char *filename) {
00359   // write pgm files
00360 
00361   FILE* F = fopen(filename, "w");
00362   if (!F) {
00363     std::cerr << "could not open 'result.pgm' for writing!\n";
00364     return;
00365   }
00366   fprintf(F, "P6\n");
00367   fprintf(F, "%d %d 255\n", sizeX, sizeY);
00368 
00369   for(int y = sizeY-1; y >=0; y--){      
00370     for(int x = 0; x<sizeX; x++){       
00371       unsigned char c = 0;
00372       if (isVoronoi(x,y)) {
00373         fputc( 255, F );
00374         fputc( 0, F );
00375         fputc( 0, F );
00376       } else if (data[x][y].sqdist==0) {
00377         fputc( 0, F );
00378         fputc( 0, F );
00379         fputc( 0, F );
00380       } else {
00381         float f = 80+(data[x][y].dist*5);
00382         if (f>255) f=255;
00383         if (f<0) f=0;
00384         c = (unsigned char)f;
00385         fputc( c, F );
00386         fputc( c, F );
00387         fputc( c, F );
00388       }
00389     }
00390   }
00391   fclose(F);
00392 }
00393 
00394 
00395 void DynamicVoronoi::prune() {
00396   // filler
00397   while(!pruneQueue.empty()) {
00398     INTPOINT p = pruneQueue.front();
00399     pruneQueue.pop();
00400     int x = p.x;
00401     int y = p.y;
00402 
00403     if (data[x][y].voronoi==occupied) continue;
00404     if (data[x][y].voronoi==freeQueued) continue;
00405 
00406     data[x][y].voronoi = freeQueued;
00407     open.push(data[x][y].sqdist, p);
00408 
00409     /* tl t tr
00410        l c r
00411        bl b br */
00412 
00413     dataCell tr,tl,br,bl;
00414     tr = data[x+1][y+1];
00415     tl = data[x-1][y+1];
00416     br = data[x+1][y-1];
00417     bl = data[x-1][y-1];
00418 
00419     dataCell r,b,t,l;
00420     r = data[x+1][y];
00421     l = data[x-1][y];
00422     t = data[x][y+1];
00423     b = data[x][y-1];
00424 
00425     if (x+2<sizeX && r.voronoi==occupied) { 
00426       // fill to the right
00427       if (tr.voronoi!=occupied && br.voronoi!=occupied && data[x+2][y].voronoi!=occupied) {
00428         r.voronoi = freeQueued;
00429         open.push(r.sqdist, INTPOINT(x+1,y));
00430         data[x+1][y] = r;
00431       }
00432     } 
00433     if (x-2>=0 && l.voronoi==occupied) { 
00434       // fill to the left
00435       if (tl.voronoi!=occupied && bl.voronoi!=occupied && data[x-2][y].voronoi!=occupied) {
00436         l.voronoi = freeQueued;
00437         open.push(l.sqdist, INTPOINT(x-1,y));
00438         data[x-1][y] = l;
00439       }
00440     } 
00441     if (y+2<sizeY && t.voronoi==occupied) { 
00442       // fill to the top
00443       if (tr.voronoi!=occupied && tl.voronoi!=occupied && data[x][y+2].voronoi!=occupied) {
00444         t.voronoi = freeQueued;
00445         open.push(t.sqdist, INTPOINT(x,y+1));
00446         data[x][y+1] = t;
00447       }
00448     } 
00449     if (y-2>=0 && b.voronoi==occupied) { 
00450       // fill to the bottom
00451       if (br.voronoi!=occupied && bl.voronoi!=occupied && data[x][y-2].voronoi!=occupied) {
00452         b.voronoi = freeQueued;
00453         open.push(b.sqdist, INTPOINT(x,y-1));
00454         data[x][y-1] = b;
00455       }
00456     } 
00457   }
00458 
00459 
00460   while(!open.empty()) {
00461     INTPOINT p = open.pop();
00462     dataCell c = data[p.x][p.y];
00463     int v = c.voronoi;
00464     if (v!=freeQueued && v!=voronoiRetry) { // || v>free || v==voronoiPrune || v==voronoiKeep) {
00465       //      assert(v!=retry);
00466       continue;
00467     }
00468 
00469     markerMatchResult r = markerMatch(p.x,p.y);
00470     if (r==pruned) c.voronoi = voronoiPrune;
00471     else if (r==keep) c.voronoi = voronoiKeep;
00472     else { // r==retry
00473       c.voronoi = voronoiRetry;
00474       //      printf("RETRY %d %d\n", x, sizeY-1-y);
00475       pruneQueue.push(p);
00476     }
00477     data[p.x][p.y] = c;
00478 
00479     if (open.empty()) {
00480       while (!pruneQueue.empty()) {
00481         INTPOINT p = pruneQueue.front();
00482         pruneQueue.pop();
00483         open.push(data[p.x][p.y].sqdist, p);
00484       }
00485     }
00486   }
00487   //  printf("match: %d\nnomat: %d\n", matchCount, noMatchCount);
00488 }
00489 
00490 
00491 DynamicVoronoi::markerMatchResult DynamicVoronoi::markerMatch(int x, int y) {
00492   // implementation of connectivity patterns
00493   bool f[8];
00494 
00495   int nx, ny;
00496   int dx, dy;
00497 
00498   int i=0;
00499   int count=0;
00500   //  int obstacleCount=0;
00501   int voroCount=0;
00502   int voroCountFour=0;
00503 
00504   for (dy=1; dy>=-1; dy--) {
00505     ny = y+dy;
00506     for (dx=-1; dx<=1; dx++) {
00507       if (dx || dy) {
00508         nx = x+dx;
00509         dataCell nc = data[nx][ny];
00510         int v = nc.voronoi;
00511         bool b = (v<=free && v!=voronoiPrune); 
00512         //      if (v==occupied) obstacleCount++;
00513         f[i] = b;
00514         if (b) {
00515           voroCount++;
00516           if (!(dx && dy)) voroCountFour++;
00517         }
00518         if (b && !(dx && dy) ) count++;
00519         //      if (v<=free && !(dx && dy)) voroCount++;
00520         i++;
00521       }
00522     }
00523   }
00524   if (voroCount<3 && voroCountFour==1 && (f[1] || f[3] || f[4] || f[6])) {
00525     //    assert(voroCount<2);
00526     //    if (voroCount>=2) printf("voro>2 %d %d\n", x, y);
00527     return keep;
00528   }
00529 
00530   // 4-connected
00531   if ((!f[0] && f[1] && f[3]) || (!f[2] && f[1] && f[4]) || (!f[5] && f[3] && f[6]) || (!f[7] && f[6] && f[4])) return keep;
00532   if ((f[3] && f[4] && !f[1] && !f[6]) || (f[1] && f[6] && !f[3] && !f[4])) return keep;
00533   
00534 
00535 
00536   // keep voro cells inside of blocks and retry later
00537   if (voroCount>=5 && voroCountFour>=3 && data[x][y].voronoi!=voronoiRetry) {
00538     return retry;
00539   }
00540 
00541   return pruned;
00542 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


dynamicvoronoi
Author(s): Boris Lau, Christoph Sprunk, Wolfram Burgard
autogenerated on Wed Dec 26 2012 16:35:03