├── README.md
├── data
    └── testmap.pgm
├── include
    ├── point.h
    ├── bucketedqueue.h
    └── dynamicvoronoi.h
├── src
    ├── bucketedqueue.cpp
    └── dynamicvoronoi.cpp
├── package.xml
└── CMakeLists.txt


/README.md:
--------------------------------------------------------------------------------
1 | # dynamic_voronoi
2 | 


--------------------------------------------------------------------------------
/data/testmap.pgm:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/frontw/dynamicvoronoi/HEAD/data/testmap.pgm


--------------------------------------------------------------------------------
/include/point.h:
--------------------------------------------------------------------------------
 1 | #ifndef _VOROPOINT_H_
 2 | #define _VOROPOINT_H_
 3 | 
 4 | #define INTPOINT IntPoint
 5 | 
 6 | /*! A light-weight integer point with fields x,y */
 7 | class IntPoint {
 8 | public:
 9 |   IntPoint() : x(0), y(0) {}
10 |   IntPoint(int _x, int _y) : x(_x), y(_y) {}
11 |   int x,y;
12 | };
13 | 
14 | #endif
15 | 


--------------------------------------------------------------------------------
/include/bucketedqueue.h:
--------------------------------------------------------------------------------
 1 | #ifndef _PRIORITYQUEUE2_H_
 2 | #define _PRIORITYQUEUE2_H_
 3 | 
 4 | #define MAXDIST 1000
 5 | #define RESERVE 64
 6 | 
 7 | #include <vector>
 8 | #include <set>
 9 | #include <queue>
10 | #include <assert.h>
11 | #include "point.h"
12 | 
13 | //! Priority queue for integer coordinates with squared distances as priority.
14 | /** A priority queue that uses buckets to group elements with the same priority.
15 |  *  The individual buckets are unsorted, which increases efficiency if these groups are large.
16 |  *  The elements are assumed to be integer coordinates, and the priorities are assumed
17 |  *  to be squared euclidean distances (integers).
18 |  */
19 | class BucketPrioQueue {
20 | 
21 | public:
22 |   //! Standard constructor
23 |   /** Standard constructor. When called for the first time it creates a look up table 
24 |    *  that maps square distanes to bucket numbers, which might take some time... 
25 |    */
26 |   BucketPrioQueue(); 
27 |   //! Checks whether the Queue is empty
28 |   bool empty();
29 |   //! push an element
30 |   void push(int prio, INTPOINT t);
31 |   //! return and pop the element with the lowest squared distance */
32 |   INTPOINT pop();
33 | 
34 | private:
35 |   
36 |   static void initSqrIndices();
37 |   static std::vector<int> sqrIndices;
38 |   static int numBuckets;
39 |   int count;
40 |   int nextBucket;
41 | 
42 |   std::vector<std::queue<INTPOINT> > buckets;
43 | };
44 | 
45 | #endif
46 | 


--------------------------------------------------------------------------------
/src/bucketedqueue.cpp:
--------------------------------------------------------------------------------
 1 | #include "bucketedqueue.h"
 2 | 
 3 | #include "limits.h"
 4 | #include <stdio.h>
 5 | #include <stdlib.h>
 6 | 
 7 | std::vector<int> BucketPrioQueue::sqrIndices;
 8 | int BucketPrioQueue::numBuckets;
 9 | 
10 | 
11 | BucketPrioQueue::BucketPrioQueue() {
12 |   // make sure the index array is created
13 |   if (sqrIndices.size()==0) initSqrIndices();
14 |   nextBucket = INT_MAX;
15 |     
16 |   // now create the buckets
17 |   //    buckets = std::vector<MyQueue2<INTPOINT> >(numBuckets);
18 |   buckets = std::vector<std::queue<INTPOINT> >(numBuckets);
19 | 
20 |   // reset element counter 
21 |   count = 0;
22 | }
23 | 
24 | bool BucketPrioQueue::empty() {
25 |   return (count==0);
26 | }
27 | 
28 | 
29 | void BucketPrioQueue::push(int prio, INTPOINT t) {
30 |   if (prio>=(int)sqrIndices.size()) {
31 |     fprintf(stderr, "error: priority %d is not a valid squared distance x*x+y*y, or x>MAXDIST or y>MAXDIST.\n", prio);
32 |     exit(-1);
33 |   }
34 |   int id = sqrIndices[prio];
35 |   if (id<0) {
36 |     fprintf(stderr, "error: priority %d is not a valid squared distance x*x+y*y, or x>MAXDIST or y>MAXDIST.\n", prio);
37 |     exit(-1);
38 |   }
39 |   buckets[id].push(t);
40 |   //    printf("pushing %d with prio %d into %d. Next: %d\n", t.x, prio, id, nextBucket);
41 |   if (id<nextBucket) nextBucket = id;
42 |   //    printf("push new next is %d\n", nextBucket);
43 |   count++;
44 | }
45 | 
46 | INTPOINT BucketPrioQueue::pop() {
47 |   int i;
48 |   assert(count>0);
49 |   //    printf("next: %d\n", nextBucket);
50 |   for (i = nextBucket; i<(int)buckets.size(); i++) {
51 |     if (!buckets[i].empty()) break;	
52 |   }
53 |   assert(i<(int)buckets.size());
54 |   nextBucket = i;
55 |   //    printf("pop new next %d\n", nextBucket);
56 |   count--;
57 |   INTPOINT p = buckets[i].front();
58 |   buckets[i].pop();
59 |   return p;
60 | }
61 | 
62 | 
63 | void BucketPrioQueue::initSqrIndices() {
64 |   //    std::cout << "BUCKETQUEUE Starting to build the index array...\n";
65 |   //  std::set<int> sqrNumbers;
66 | 
67 |   sqrIndices = std::vector<int>(2*MAXDIST*MAXDIST+1, -1);
68 | 
69 |   int count=0;
70 |   for (int x=0; x<=MAXDIST; x++) {
71 |     for (int y=0; y<=x; y++) {
72 |       int sqr = x*x+y*y;
73 |       sqrIndices[sqr] = count++;
74 |     }
75 |   }
76 |   numBuckets = count;
77 |   //    std::cout << "BUCKETQUEUE Done with building the index arrays.\n";
78 | }
79 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |   <name>dynamicvoronoi</name>
 4 |   <version>0.0.0</version>
 5 |   <description>
 6 | 
 7 | <p>
 8 | This stack provides software to compute and update Euclidean distance maps (DM) and Euclidean Voronoi diagrams (GVD) on 2D grid maps.
 9 | </p>
10 | 
11 | <p>
12 | The program is initialized with a binary occupancy grid map and computes the corresponding DM and GVD. When provided with points that mark newly occupied or freed cells, the DM and GVD can be updated efficiently to reflect the changes in the environment.
13 | </p>
14 | 
15 | <p>
16 | Details on the algorithms can be found in the corresponding paper. Please cite the paper if you use it for scientific work:<br />
17 | B. Lau, C. Sprunk and W. Burgard, Improved Updating of Euclidean Distance Maps and Voronoi Diagrams, IEEE Intl. Conf. on Intelligent Robots and Systems (IROS), Taipei, Taiwan, 2010.<br />
18 | See also <a href="http://www.informatik.uni-freiburg.de/~lau/dynamicvoronoi">http://www.informatik.uni-freiburg.de/~lau/dynamicvoronoi</a>
19 | </p>
20 | 
21 |   </description>
22 | 
23 | 
24 |   <!-- One maintainer tag required, multiple allowed, one person per tag --> 
25 |   <!-- Example:  -->
26 |   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
27 |   <maintainer email="op@todo.todo">op</maintainer>
28 |   <author>Boris Lau, Christoph Sprunk, Wolfram Burgard</author>
29 | 
30 | 
31 |   <!-- One license tag required, multiple allowed, one license per tag -->
32 |   <!-- Commonly used license strings: -->
33 |   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
34 |   <license>BSD</license>
35 | 
36 | 
37 |   <!-- Url tags are optional, but mutiple are allowed, one per tag -->
38 |   <!-- Optional attribute type can be: website, bugtracker, or repository -->
39 |   <!-- Example: -->
40 |   <!-- <url type="website">http://wiki.ros.org/dynamicvoronoi</url> -->
41 | 
42 | 
43 |   <!-- Author tags are optional, mutiple are allowed, one per tag -->
44 |   <!-- Authors do not have to be maintianers, but could be -->
45 |   <!-- Example: -->
46 |   <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
47 | 
48 | 
49 |   <!-- The *_depend tags are used to specify dependencies -->
50 |   <!-- Dependencies can be catkin packages or system dependencies -->
51 |   <!-- Examples: -->
52 |   <!-- Use build_depend for packages you need at compile time: -->
53 |   <!--   <build_depend>message_generation</build_depend> -->
54 |   <!-- Use buildtool_depend for build tool packages: -->
55 |   <!--   <buildtool_depend>catkin</buildtool_depend> -->
56 |   <!-- Use run_depend for packages you need at runtime: -->
57 |   <!--   <run_depend>message_runtime</run_depend> -->
58 |   <!-- Use test_depend for packages you need only for testing: -->
59 |   <!--   <test_depend>gtest</test_depend> -->
60 |   <buildtool_depend>catkin</buildtool_depend>
61 |   <build_depend>roscpp</build_depend>
62 |   <run_depend>roscpp</run_depend>
63 | 
64 | 
65 |   <!-- The export tag contains other, unspecified, tags -->
66 |   <export>
67 |     <!-- Other tools can request additional information be placed here -->
68 | 
69 |   </export>
70 | </package>
71 | 


--------------------------------------------------------------------------------
/include/dynamicvoronoi.h:
--------------------------------------------------------------------------------
  1 | #ifndef _DYNAMICVORONOI_H_
  2 | #define _DYNAMICVORONOI_H_
  3 | 
  4 | 
  5 | #include <stdlib.h>
  6 | #include <stdio.h>
  7 | #include <limits.h>
  8 | #include <queue>
  9 | 
 10 | #include "bucketedqueue.h"
 11 | 
 12 | //! A DynamicVoronoi object computes and updates a distance map and Voronoi diagram.
 13 | class DynamicVoronoi {
 14 |   
 15 | public:
 16 |   
 17 |   DynamicVoronoi();
 18 |   ~DynamicVoronoi();
 19 | 
 20 |   //! Initialization with an empty map
 21 |   void initializeEmpty(int _sizeX, int _sizeY, bool initGridMap=true);
 22 |   //! Initialization with a given binary map (false==free, true==occupied)
 23 |   void initializeMap(int _sizeX, int _sizeY, bool** _gridMap);
 24 | 
 25 |   //! add an obstacle at the specified cell coordinate
 26 |   void occupyCell(int x, int y);
 27 |   //! remove an obstacle at the specified cell coordinate
 28 |   void clearCell(int x, int y);
 29 |   //! remove old dynamic obstacles and add the new ones
 30 |   void exchangeObstacles(std::vector<INTPOINT> newObstacles);
 31 | 
 32 |   //! update distance map and Voronoi diagram to reflect the changes
 33 |   void update(bool updateRealDist=true);
 34 |   //! prune the Voronoi diagram
 35 |   void prune();
 36 |   
 37 |   //! returns the obstacle distance at the specified location
 38 |   float getDistance( int x, int y );
 39 |   //! returns whether the specified cell is part of the (pruned) Voronoi graph
 40 |   bool isVoronoi( int x, int y );
 41 |   //! checks whether the specficied location is occupied
 42 |   bool isOccupied(int x, int y);
 43 |   //! write the current distance map and voronoi diagram as ppm file
 44 |   void visualize(const char* filename="result.ppm");
 45 | 
 46 |   //! returns the horizontal size of the workspace/map
 47 |   unsigned int getSizeX() {return sizeX;}
 48 |   //! returns the vertical size of the workspace/map
 49 |   unsigned int getSizeY() {return sizeY;}
 50 | 
 51 | private:  
 52 |   struct dataCell {
 53 |     float dist;
 54 |     char voronoi;
 55 |     char queueing;
 56 |     int obstX;
 57 |     int obstY;
 58 |     bool needsRaise;
 59 |     int sqdist;
 60 |   };
 61 | 
 62 |   typedef enum {voronoiKeep=-4, freeQueued = -3, voronoiRetry=-2, voronoiPrune=-1, free=0, occupied=1} State;
 63 |   typedef enum {fwNotQueued=1, fwQueued=2, fwProcessed=3, bwQueued=4, bwProcessed=1} QueueingState;
 64 |   typedef enum {invalidObstData = SHRT_MAX/2} ObstDataState;
 65 |   typedef enum {pruned, keep, retry} markerMatchResult;
 66 | 
 67 | 
 68 |   
 69 |   // methods
 70 |   void setObstacle(int x, int y);
 71 |   void removeObstacle(int x, int y);
 72 |   inline void checkVoro(int x, int y, int nx, int ny, dataCell& c, dataCell& nc);
 73 |   void recheckVoro();
 74 |   void commitAndColorize(bool updateRealDist=true);
 75 |   inline void reviveVoroNeighbors(int &x, int &y);
 76 | 
 77 |   inline bool isOccupied(int &x, int &y, dataCell &c);
 78 |   inline markerMatchResult markerMatch(int x, int y);
 79 | 
 80 |   // queues
 81 | 
 82 |   BucketPrioQueue open;
 83 |   std::queue<INTPOINT> pruneQueue;
 84 | 
 85 |   std::vector<INTPOINT> removeList;
 86 |   std::vector<INTPOINT> addList;
 87 |   std::vector<INTPOINT> lastObstacles;
 88 | 
 89 |   // maps
 90 |   int sizeY;
 91 |   int sizeX;
 92 |   dataCell** data;
 93 |   bool** gridMap;
 94 | 
 95 |   // parameters
 96 |   int padding;
 97 |   double doubleThreshold;
 98 | 
 99 |   double sqrt2;
100 | 
101 |   //  dataCell** getData(){ return data; }
102 | };
103 | 
104 | 
105 | #endif
106 | 
107 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
  1 | cmake_minimum_required(VERSION 2.8.3)
  2 | project(dynamicvoronoi)
  3 | 
  4 | ## Find catkin macros and libraries
  5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
  6 | ## is used, also find other catkin packages
  7 | find_package(catkin REQUIRED COMPONENTS
  8 |   roscpp
  9 | )
 10 | 
 11 | ## System dependencies are found with CMake's conventions
 12 | # find_package(Boost REQUIRED COMPONENTS system)
 13 | 
 14 | 
 15 | ## Uncomment this if the package has a setup.py. This macro ensures
 16 | ## modules and global scripts declared therein get installed
 17 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
 18 | # catkin_python_setup()
 19 | 
 20 | ################################################
 21 | ## Declare ROS messages, services and actions ##
 22 | ################################################
 23 | 
 24 | ## To declare and build messages, services or actions from within this
 25 | ## package, follow these steps:
 26 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in
 27 | ##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
 28 | ## * In the file package.xml:
 29 | ##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
 30 | ##   * If MSG_DEP_SET isn't empty the following dependencies might have been
 31 | ##     pulled in transitively but can be declared for certainty nonetheless:
 32 | ##     * add a build_depend tag for "message_generation"
 33 | ##     * add a run_depend tag for "message_runtime"
 34 | ## * In this file (CMakeLists.txt):
 35 | ##   * add "message_generation" and every package in MSG_DEP_SET to
 36 | ##     find_package(catkin REQUIRED COMPONENTS ...)
 37 | ##   * add "message_runtime" and every package in MSG_DEP_SET to
 38 | ##     catkin_package(CATKIN_DEPENDS ...)
 39 | ##   * uncomment the add_*_files sections below as needed
 40 | ##     and list every .msg/.srv/.action file to be processed
 41 | ##   * uncomment the generate_messages entry below
 42 | ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 43 | 
 44 | ## Generate messages in the 'msg' folder
 45 | # add_message_files(
 46 | #   FILES
 47 | #   Message1.msg
 48 | #   Message2.msg
 49 | # )
 50 | 
 51 | ## Generate services in the 'srv' folder
 52 | # add_service_files(
 53 | #   FILES
 54 | #   Service1.srv
 55 | #   Service2.srv
 56 | # )
 57 | 
 58 | ## Generate actions in the 'action' folder
 59 | # add_action_files(
 60 | #   FILES
 61 | #   Action1.action
 62 | #   Action2.action
 63 | # )
 64 | 
 65 | ## Generate added messages and services with any dependencies listed here
 66 | # generate_messages(
 67 | #   DEPENDENCIES
 68 | #   std_msgs  # Or other packages containing msgs
 69 | # )
 70 | 
 71 | ###################################
 72 | ## catkin specific configuration ##
 73 | ###################################
 74 | ## The catkin_package macro generates cmake config files for your package
 75 | ## Declare things to be passed to dependent projects
 76 | ## INCLUDE_DIRS: uncomment this if you package contains header files
 77 | ## LIBRARIES: libraries you create in this project that dependent projects also need
 78 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 79 | ## DEPENDS: system dependencies of this project that dependent projects also need
 80 | catkin_package(
 81 |   INCLUDE_DIRS include
 82 |   LIBRARIES dynamicvoronoi
 83 | #  CATKIN_DEPENDS roscpp
 84 | #  DEPENDS system_lib
 85 | )
 86 | 
 87 | ###########
 88 | ## Build ##
 89 | ###########
 90 | 
 91 | ## Specify additional locations of header files
 92 | ## Your package locations should be listed before other locations
 93 | include_directories(include)
 94 | include_directories(
 95 |   ${catkin_INCLUDE_DIRS}
 96 | )
 97 | 
 98 | ## Declare a cpp library
 99 | add_library(dynamicvoronoi
100 |   src/bucketedqueue.cpp src/dynamicvoronoi.cpp
101 | )
102 | 
103 | ## Declare a cpp executable
104 | # add_executable(dynamicvoronoi_node src/dynamicvoronoi_node.cpp)
105 | 
106 | ## Add cmake target dependencies of the executable/library
107 | ## as an example, message headers may need to be generated before nodes
108 | # add_dependencies(dynamicvoronoi_node dynamicvoronoi_generate_messages_cpp)
109 | 
110 | ## Specify libraries to link a library or executable target against
111 | # target_link_libraries(dynamicvoronoi_node
112 | #   ${catkin_LIBRARIES}
113 | # )
114 | 
115 | #############
116 | ## Install ##
117 | #############
118 | 
119 | # all install targets should use catkin DESTINATION variables
120 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
121 | 
122 | ## Mark executable scripts (Python etc.) for installation
123 | ## in contrast to setup.py, you can choose the destination
124 | # install(PROGRAMS
125 | #   scripts/my_python_script
126 | #   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
127 | # )
128 | 
129 | ## Mark executables and/or libraries for installation
130 | # install(TARGETS dynamicvoronoi dynamicvoronoi_node
131 | #   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
132 | #   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
133 | #   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
134 | # )
135 | 
136 | ## Mark cpp header files for installation
137 | # install(DIRECTORY include/${PROJECT_NAME}/
138 | #   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
139 | #   FILES_MATCHING PATTERN "*.h"
140 | #   PATTERN ".svn" EXCLUDE
141 | # )
142 | 
143 | ## Mark other files for installation (e.g. launch and bag files, etc.)
144 | # install(FILES
145 | #   # myfile1
146 | #   # myfile2
147 | #   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
148 | # )
149 | 
150 | #############
151 | ## Testing ##
152 | #############
153 | 
154 | ## Add gtest based cpp test target and link libraries
155 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_dynamicvoronoi.cpp)
156 | # if(TARGET ${PROJECT_NAME}-test)
157 | #   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
158 | # endif()
159 | 
160 | ## Add folders to be run by python nosetests
161 | # catkin_add_nosetests(test)
162 | 


--------------------------------------------------------------------------------
/src/dynamicvoronoi.cpp:
--------------------------------------------------------------------------------
  1 | #include "dynamicvoronoi.h"
  2 | 
  3 | #include <math.h>
  4 | #include <iostream>
  5 | 
  6 | DynamicVoronoi::DynamicVoronoi() {
  7 |   sqrt2 = sqrt(2.0);
  8 |   data = NULL;
  9 |   gridMap = NULL;
 10 | }
 11 | 
 12 | DynamicVoronoi::~DynamicVoronoi() {
 13 |   if (data) {
 14 |     for (int x=0; x<sizeX; x++) delete[] data[x];
 15 |     delete[] data;
 16 |   }
 17 |   if (gridMap) {
 18 |     for (int x=0; x<sizeX; x++) delete[] gridMap[x];
 19 |     delete[] gridMap;
 20 |   }
 21 | }
 22 | 
 23 | void DynamicVoronoi::initializeEmpty(int _sizeX, int _sizeY, bool initGridMap) {
 24 |   sizeX = _sizeX;
 25 |   sizeY = _sizeY;
 26 |   if (data) {
 27 |     for (int x=0; x<sizeX; x++) delete[] data[x];
 28 |     delete[] data;
 29 |   }
 30 |   data = new dataCell*[sizeX];
 31 |   for (int x=0; x<sizeX; x++) data[x] = new dataCell[sizeY];
 32 | 
 33 |   if (initGridMap) {
 34 |     if (gridMap) {
 35 |       for (int x=0; x<sizeX; x++) delete[] gridMap[x];
 36 |       delete[] gridMap;
 37 |     }
 38 |     gridMap = new bool*[sizeX];
 39 |     for (int x=0; x<sizeX; x++) gridMap[x] = new bool[sizeY];
 40 |   }
 41 |   
 42 |   dataCell c;
 43 |   c.dist = INFINITY;
 44 |   c.sqdist = INT_MAX;
 45 |   c.obstX = invalidObstData;
 46 |   c.obstY = invalidObstData;
 47 |   c.voronoi = free;
 48 |   c.queueing = fwNotQueued;
 49 |   c.needsRaise = false;
 50 | 
 51 |   for (int x=0; x<sizeX; x++)
 52 |     for (int y=0; y<sizeY; y++) data[x][y] = c;
 53 | 
 54 |   if (initGridMap) {
 55 |     for (int x=0; x<sizeX; x++) 
 56 |       for (int y=0; y<sizeY; y++) gridMap[x][y] = 0;
 57 |   }
 58 | }
 59 | 
 60 | void DynamicVoronoi::initializeMap(int _sizeX, int _sizeY, bool** _gridMap) {
 61 |   gridMap = _gridMap;
 62 |   initializeEmpty(_sizeX, _sizeY, false);
 63 | 
 64 |   for (int x=0; x<sizeX; x++) {
 65 |     for (int y=0; y<sizeY; y++) {
 66 |       if (gridMap[x][y]) {
 67 |         dataCell c = data[x][y];
 68 |         if (!isOccupied(x,y,c)) {
 69 |           
 70 |           bool isSurrounded = true;
 71 |           for (int dx=-1; dx<=1; dx++) {
 72 |             int nx = x+dx;
 73 |             if (nx<=0 || nx>=sizeX-1) continue;
 74 |             for (int dy=-1; dy<=1; dy++) {
 75 |               if (dx==0 && dy==0) continue;
 76 |               int ny = y+dy;
 77 |               if (ny<=0 || ny>=sizeY-1) continue;
 78 | 
 79 |               if (!gridMap[nx][ny]) {
 80 |                 isSurrounded = false;
 81 |                 break;
 82 |               }
 83 |             }
 84 |           }
 85 |           if (isSurrounded) {
 86 |             c.obstX = x;
 87 |             c.obstY = y;
 88 |             c.sqdist = 0;
 89 |             c.dist=0;
 90 |             c.voronoi=occupied;
 91 |             c.queueing = fwProcessed;
 92 |             data[x][y] = c;
 93 |           } else setObstacle(x,y);
 94 |         }
 95 |       }
 96 |     }
 97 |   }
 98 | }
 99 | 
100 | void DynamicVoronoi::occupyCell(int x, int y) {
101 |   gridMap[x][y] = 1;
102 |   setObstacle(x,y);
103 | }
104 | void DynamicVoronoi::clearCell(int x, int y) {
105 |   gridMap[x][y] = 0;
106 |   removeObstacle(x,y);
107 | }
108 | 
109 | void DynamicVoronoi::setObstacle(int x, int y) {
110 |   dataCell c = data[x][y];
111 |   if(isOccupied(x,y,c)) return;
112 |   
113 |   addList.push_back(INTPOINT(x,y));
114 |   c.obstX = x;
115 |   c.obstY = y;
116 |   data[x][y] = c;
117 | }
118 | 
119 | void DynamicVoronoi::removeObstacle(int x, int y) {
120 |   dataCell c = data[x][y];
121 |   if(isOccupied(x,y,c) == false) return;
122 | 
123 |   removeList.push_back(INTPOINT(x,y));
124 |   c.obstX = invalidObstData;
125 |   c.obstY  = invalidObstData;    
126 |   c.queueing = bwQueued;
127 |   data[x][y] = c;
128 | }
129 | 
130 | void DynamicVoronoi::exchangeObstacles(std::vector<INTPOINT> points) {
131 | 
132 |   for (unsigned int i=0; i<lastObstacles.size(); i++) {
133 |     int x = lastObstacles[i].x;
134 |     int y = lastObstacles[i].y;
135 | 
136 |     bool v = gridMap[x][y];
137 |     if (v) continue;
138 |     removeObstacle(x,y);
139 |   }  
140 | 
141 |   lastObstacles.clear();
142 | 
143 |   for (unsigned int i=0; i<points.size(); i++) {
144 |     int x = points[i].x;
145 |     int y = points[i].y;
146 |     bool v = gridMap[x][y];
147 |     if (v) continue;
148 |     setObstacle(x,y);
149 |     lastObstacles.push_back(points[i]);
150 |   }  
151 | }
152 | 
153 | void DynamicVoronoi::update(bool updateRealDist) {
154 | 
155 |   commitAndColorize(updateRealDist);
156 | 
157 |   while (!open.empty()) {
158 |     INTPOINT p = open.pop();
159 |     int x = p.x;
160 |     int y = p.y;
161 |     dataCell c = data[x][y];
162 | 
163 |     if(c.queueing==fwProcessed) continue; 
164 | 
165 |     if (c.needsRaise) {
166 |       // RAISE
167 |       for (int dx=-1; dx<=1; dx++) {
168 |         int nx = x+dx;
169 |         if (nx<=0 || nx>=sizeX-1) continue;
170 |         for (int dy=-1; dy<=1; dy++) {
171 |           if (dx==0 && dy==0) continue;
172 |           int ny = y+dy;
173 |           if (ny<=0 || ny>=sizeY-1) continue;
174 |           dataCell nc = data[nx][ny];
175 |           if (nc.obstX!=invalidObstData && !nc.needsRaise) {
176 |             if(!isOccupied(nc.obstX,nc.obstY,data[nc.obstX][nc.obstY])) {
177 |               open.push(nc.sqdist, INTPOINT(nx,ny));
178 |               nc.queueing = fwQueued;
179 |               nc.needsRaise = true;
180 |               nc.obstX = invalidObstData;
181 |               nc.obstY = invalidObstData;
182 |               if (updateRealDist) nc.dist = INFINITY;
183 |               nc.sqdist = INT_MAX;
184 |               data[nx][ny] = nc;
185 |             } else {
186 |               if(nc.queueing != fwQueued){
187 |                 open.push(nc.sqdist, INTPOINT(nx,ny));
188 |                 nc.queueing = fwQueued;
189 |                 data[nx][ny] = nc;
190 |               }
191 |             }      
192 |           }
193 |         }
194 |       }
195 |       c.needsRaise = false;
196 |       c.queueing = bwProcessed;
197 |       data[x][y] = c;
198 |     }
199 |     else if (c.obstX != invalidObstData && isOccupied(c.obstX,c.obstY,data[c.obstX][c.obstY])) {
200 | 
201 |       // LOWER
202 |       c.queueing = fwProcessed;
203 |       c.voronoi = occupied;
204 | 
205 |       for (int dx=-1; dx<=1; dx++) {
206 |         int nx = x+dx;
207 |         if (nx<=0 || nx>=sizeX-1) continue;
208 |         for (int dy=-1; dy<=1; dy++) {
209 |           if (dx==0 && dy==0) continue;
210 |           int ny = y+dy;
211 |           if (ny<=0 || ny>=sizeY-1) continue;
212 |           dataCell nc = data[nx][ny];
213 |           if(!nc.needsRaise) {
214 |             int distx = nx-c.obstX;
215 |             int disty = ny-c.obstY;
216 |             int newSqDistance = distx*distx + disty*disty;		
217 |             bool overwrite =  (newSqDistance < nc.sqdist);
218 |             if(!overwrite && newSqDistance==nc.sqdist) { 
219 |               if (nc.obstX == invalidObstData || isOccupied(nc.obstX,nc.obstY,data[nc.obstX][nc.obstY])==false) overwrite = true;
220 |             }
221 |             if (overwrite) {
222 |               open.push(newSqDistance, INTPOINT(nx,ny));
223 |               nc.queueing = fwQueued;
224 |               if (updateRealDist) {
225 |                 nc.dist = sqrt((double) newSqDistance);
226 |               }
227 |               nc.sqdist = newSqDistance;
228 |               nc.obstX = c.obstX;
229 |               nc.obstY = c.obstY;
230 |             } else { 
231 |               checkVoro(x,y,nx,ny,c,nc);
232 |             }
233 |             data[nx][ny] = nc;
234 |           }
235 |         }
236 |       }
237 |     }
238 |     data[x][y] = c;
239 |   }
240 | }
241 | 
242 | float DynamicVoronoi::getDistance( int x, int y ) {
243 |   if( (x>0) && (x<sizeX) && (y>0) && (y<sizeY)) return data[x][y].dist; 
244 |   else return -INFINITY;
245 | }
246 | 
247 | bool DynamicVoronoi::isVoronoi( int x, int y ) {
248 |   dataCell c = data[x][y];
249 |   return (c.voronoi==free || c.voronoi==voronoiKeep);
250 | }
251 | 
252 | 
253 | void DynamicVoronoi::commitAndColorize(bool updateRealDist) {
254 |   // ADD NEW OBSTACLES
255 |   for (unsigned int i=0; i<addList.size(); i++) {
256 |     INTPOINT p = addList[i];
257 |     int x = p.x;
258 |     int y = p.y;
259 |     dataCell c = data[x][y];
260 | 
261 |     if(c.queueing != fwQueued){
262 |       if (updateRealDist) c.dist = 0;
263 |       c.sqdist = 0;
264 |       c.obstX = x;
265 |       c.obstY = y;
266 |       c.queueing = fwQueued;
267 |       c.voronoi = occupied;
268 |       data[x][y] = c;
269 |       open.push(0, INTPOINT(x,y));
270 |     }
271 |   }
272 | 
273 |   // REMOVE OLD OBSTACLES
274 |   for (unsigned int i=0; i<removeList.size(); i++) {
275 |     INTPOINT p = removeList[i];
276 |     int x = p.x;
277 |     int y = p.y;
278 |     dataCell c = data[x][y];
279 | 
280 |     if (isOccupied(x,y,c)==true) continue; // obstacle was removed and reinserted
281 |     open.push(0, INTPOINT(x,y));
282 |     if (updateRealDist) c.dist  = INFINITY;
283 |     c.sqdist = INT_MAX;
284 |     c.needsRaise = true;
285 |     data[x][y] = c;
286 |   }
287 |   removeList.clear();
288 |   addList.clear();
289 | }
290 | 
291 | 
292 | void DynamicVoronoi::checkVoro(int x, int y, int nx, int ny, dataCell& c, dataCell& nc) {
293 | 
294 |   if ((c.sqdist>1 || nc.sqdist>1) && nc.obstX!=invalidObstData) { 
295 |     if (abs(c.obstX-nc.obstX) > 1 || abs(c.obstY-nc.obstY) > 1) {
296 |       //compute dist from x,y to obstacle of nx,ny	 
297 |       int dxy_x = x-nc.obstX;
298 |       int dxy_y = y-nc.obstY;
299 |       int sqdxy = dxy_x*dxy_x + dxy_y*dxy_y;
300 |       int stability_xy = sqdxy - c.sqdist;
301 |       if (sqdxy - c.sqdist<0) return;
302 | 
303 |       //compute dist from nx,ny to obstacle of x,y
304 |       int dnxy_x = nx - c.obstX;
305 |       int dnxy_y = ny - c.obstY;
306 |       int sqdnxy = dnxy_x*dnxy_x + dnxy_y*dnxy_y;
307 |       int stability_nxy = sqdnxy - nc.sqdist;
308 |       if (sqdnxy - nc.sqdist <0) return;
309 | 
310 |       //which cell is added to the Voronoi diagram?
311 |       if(stability_xy <= stability_nxy && c.sqdist>2) {
312 |         if (c.voronoi != free) {
313 |           c.voronoi = free;
314 |           reviveVoroNeighbors(x,y);
315 |           pruneQueue.push(INTPOINT(x,y));
316 |         }
317 |       }
318 |       if(stability_nxy <= stability_xy && nc.sqdist>2) {
319 |         if (nc.voronoi != free) {
320 |           nc.voronoi = free;
321 |           reviveVoroNeighbors(nx,ny);
322 |           pruneQueue.push(INTPOINT(nx,ny));
323 |         }
324 |       }
325 |     }
326 |   }
327 | }
328 | 
329 | 
330 | void DynamicVoronoi::reviveVoroNeighbors(int &x, int &y) {
331 |   for (int dx=-1; dx<=1; dx++) {
332 |     int nx = x+dx;
333 |     if (nx<=0 || nx>=sizeX-1) continue;
334 |     for (int dy=-1; dy<=1; dy++) {
335 |       if (dx==0 && dy==0) continue;
336 |       int ny = y+dy;
337 |       if (ny<=0 || ny>=sizeY-1) continue;
338 |       dataCell nc = data[nx][ny];
339 |       if (nc.sqdist != INT_MAX && !nc.needsRaise && (nc.voronoi == voronoiKeep || nc.voronoi == voronoiPrune)) {
340 |         nc.voronoi = free;
341 |         data[nx][ny] = nc;
342 |         pruneQueue.push(INTPOINT(nx,ny));
343 |       }
344 |     }
345 |   }
346 | }
347 | 
348 | 
349 | bool DynamicVoronoi::isOccupied(int x, int y) {
350 |   dataCell c = data[x][y];
351 |   return (c.obstX==x && c.obstY==y);
352 | }
353 | 
354 | bool DynamicVoronoi::isOccupied(int &x, int &y, dataCell &c) { 
355 |   return (c.obstX==x && c.obstY==y);
356 | }
357 | 
358 | void DynamicVoronoi::visualize(const char *filename) {
359 |   // write pgm files
360 | 
361 |   FILE* F = fopen(filename, "w");
362 |   if (!F) {
363 |     std::cerr << "could not open 'result.pgm' for writing!\n";
364 |     return;
365 |   }
366 |   fprintf(F, "P6\n");
367 |   fprintf(F, "%d %d 255\n", sizeX, sizeY);
368 | 
369 |   for(int y = sizeY-1; y >=0; y--){      
370 |     for(int x = 0; x<sizeX; x++){	
371 |       unsigned char c = 0;
372 |       if (isVoronoi(x,y)) {
373 |         fputc( 255, F );
374 |         fputc( 0, F );
375 |         fputc( 0, F );
376 |       } else if (data[x][y].sqdist==0) {
377 |         fputc( 0, F );
378 |         fputc( 0, F );
379 |         fputc( 0, F );
380 |       } else {
381 |         float f = 80+(data[x][y].dist*5);
382 |         if (f>255) f=255;
383 |         if (f<0) f=0;
384 |         c = (unsigned char)f;
385 |         fputc( c, F );
386 |         fputc( c, F );
387 |         fputc( c, F );
388 |       }
389 |     }
390 |   }
391 |   fclose(F);
392 | }
393 | 
394 | 
395 | void DynamicVoronoi::prune() {
396 |   // filler
397 |   while(!pruneQueue.empty()) {
398 |     INTPOINT p = pruneQueue.front();
399 |     pruneQueue.pop();
400 |     int x = p.x;
401 |     int y = p.y;
402 | 
403 |     if (data[x][y].voronoi==occupied) continue;
404 |     if (data[x][y].voronoi==freeQueued) continue;
405 | 
406 |     data[x][y].voronoi = freeQueued;
407 |     open.push(data[x][y].sqdist, p);
408 | 
409 |     /* tl t tr
410 |        l c r
411 |        bl b br */
412 | 
413 |     dataCell tr,tl,br,bl;
414 |     tr = data[x+1][y+1];
415 |     tl = data[x-1][y+1];
416 |     br = data[x+1][y-1];
417 |     bl = data[x-1][y-1];
418 | 
419 |     dataCell r,b,t,l;
420 |     r = data[x+1][y];
421 |     l = data[x-1][y];
422 |     t = data[x][y+1];
423 |     b = data[x][y-1];
424 | 
425 |     if (x+2<sizeX && r.voronoi==occupied) { 
426 |       // fill to the right
427 |       if (tr.voronoi!=occupied && br.voronoi!=occupied && data[x+2][y].voronoi!=occupied) {
428 |         r.voronoi = freeQueued;
429 |         open.push(r.sqdist, INTPOINT(x+1,y));
430 |         data[x+1][y] = r;
431 |       }
432 |     } 
433 |     if (x-2>=0 && l.voronoi==occupied) { 
434 |       // fill to the left
435 |       if (tl.voronoi!=occupied && bl.voronoi!=occupied && data[x-2][y].voronoi!=occupied) {
436 |         l.voronoi = freeQueued;
437 |         open.push(l.sqdist, INTPOINT(x-1,y));
438 |         data[x-1][y] = l;
439 |       }
440 |     } 
441 |     if (y+2<sizeY && t.voronoi==occupied) { 
442 |       // fill to the top
443 |       if (tr.voronoi!=occupied && tl.voronoi!=occupied && data[x][y+2].voronoi!=occupied) {
444 |         t.voronoi = freeQueued;
445 |         open.push(t.sqdist, INTPOINT(x,y+1));
446 |         data[x][y+1] = t;
447 |       }
448 |     } 
449 |     if (y-2>=0 && b.voronoi==occupied) { 
450 |       // fill to the bottom
451 |       if (br.voronoi!=occupied && bl.voronoi!=occupied && data[x][y-2].voronoi!=occupied) {
452 |         b.voronoi = freeQueued;
453 |         open.push(b.sqdist, INTPOINT(x,y-1));
454 |         data[x][y-1] = b;
455 |       }
456 |     } 
457 |   }
458 | 
459 | 
460 |   while(!open.empty()) {
461 |     INTPOINT p = open.pop();
462 |     dataCell c = data[p.x][p.y];
463 |     int v = c.voronoi;
464 |     if (v!=freeQueued && v!=voronoiRetry) { // || v>free || v==voronoiPrune || v==voronoiKeep) {
465 |       //      assert(v!=retry);
466 |       continue;
467 |     }
468 | 
469 |     markerMatchResult r = markerMatch(p.x,p.y);
470 |     if (r==pruned) c.voronoi = voronoiPrune;
471 |     else if (r==keep) c.voronoi = voronoiKeep;
472 |     else { // r==retry
473 |       c.voronoi = voronoiRetry;
474 |       //      printf("RETRY %d %d\n", x, sizeY-1-y);
475 |       pruneQueue.push(p);
476 |     }
477 |     data[p.x][p.y] = c;
478 | 
479 |     if (open.empty()) {
480 |       while (!pruneQueue.empty()) {
481 |         INTPOINT p = pruneQueue.front();
482 |         pruneQueue.pop();
483 |         open.push(data[p.x][p.y].sqdist, p);
484 |       }
485 |     }
486 |   }
487 |   //  printf("match: %d\nnomat: %d\n", matchCount, noMatchCount);
488 | }
489 | 
490 | 
491 | DynamicVoronoi::markerMatchResult DynamicVoronoi::markerMatch(int x, int y) {
492 |   // implementation of connectivity patterns
493 |   bool f[8];
494 | 
495 |   int nx, ny;
496 |   int dx, dy;
497 | 
498 |   int i=0;
499 |   int count=0;
500 |   //  int obstacleCount=0;
501 |   int voroCount=0;
502 |   int voroCountFour=0;
503 | 
504 |   for (dy=1; dy>=-1; dy--) {
505 |     ny = y+dy;
506 |     for (dx=-1; dx<=1; dx++) {
507 |       if (dx || dy) {
508 |         nx = x+dx;
509 |         dataCell nc = data[nx][ny];
510 |         int v = nc.voronoi;
511 |         bool b = (v<=free && v!=voronoiPrune); 
512 |         //	if (v==occupied) obstacleCount++;
513 |         f[i] = b;
514 |         if (b) {
515 |           voroCount++;
516 |           if (!(dx && dy)) voroCountFour++;
517 |         }
518 |         if (b && !(dx && dy) ) count++;
519 |         //	if (v<=free && !(dx && dy)) voroCount++;
520 |         i++;
521 |       }
522 |     }
523 |   }
524 |   if (voroCount<3 && voroCountFour==1 && (f[1] || f[3] || f[4] || f[6])) {
525 |     //    assert(voroCount<2);
526 |     //    if (voroCount>=2) printf("voro>2 %d %d\n", x, y);
527 |     return keep;
528 |   }
529 | 
530 |   // 4-connected
531 |   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;
532 |   if ((f[3] && f[4] && !f[1] && !f[6]) || (f[1] && f[6] && !f[3] && !f[4])) return keep;
533 |   
534 | 
535 | 
536 |   // keep voro cells inside of blocks and retry later
537 |   if (voroCount>=5 && voroCountFour>=3 && data[x][y].voronoi!=voronoiRetry) {
538 |     return retry;
539 |   }
540 | 
541 |   return pruned;
542 | }
543 | 


--------------------------------------------------------------------------------