dynamicEDT3D.cpp
Go to the documentation of this file.
1 
9 /*
10  * Copyright (c) 2011-2012, C. Sprunk, B. Lau, W. Burgard, University of Freiburg
11  * All rights reserved.
12  *
13  * Redistribution and use in source and binary forms, with or without
14  * modification, are permitted provided that the following conditions are met:
15  *
16  * * Redistributions of source code must retain the above copyright
17  * notice, this list of conditions and the following disclaimer.
18  * * Redistributions in binary form must reproduce the above copyright
19  * notice, this list of conditions and the following disclaimer in the
20  * documentation and/or other materials provided with the distribution.
21  * * Neither the name of the University of Freiburg nor the names of its
22  * contributors may be used to endorse or promote products derived from
23  * this software without specific prior written permission.
24  *
25  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
26  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
27  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
28  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
29  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
30  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
31  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
32  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
33  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
34  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  */
37 
38 #include <dynamicEDT3D/dynamicEDT3D.h>
39 
40 #include <math.h>
41 #include <stdlib.h>
42 
43 #define FOR_EACH_NEIGHBOR_WITH_CHECK(function, p, ...) \
44  int x=p.x;\
45  int y=p.y;\
46  int z=p.z;\
47  int xp1 = x+1;\
48  int xm1 = x-1;\
49  int yp1 = y+1;\
50  int ym1 = y-1;\
51  int zp1 = z+1;\
52  int zm1 = z-1;\
53 \
54  if(z<sizeZm1) function(x, y, zp1, ##__VA_ARGS__);\
55  if(z>0) function(x, y, zm1, ##__VA_ARGS__);\
56 \
57  if(y<sizeYm1){\
58  function(x, yp1, z, ##__VA_ARGS__);\
59  if(z<sizeZm1) function(x, yp1, zp1, ##__VA_ARGS__);\
60  if(z>0) function(x, yp1, zm1, ##__VA_ARGS__);\
61  }\
62 \
63  if(y>0){\
64  function(x, ym1, z, ##__VA_ARGS__);\
65  if(z<sizeZm1) function(x, ym1, zp1, ##__VA_ARGS__);\
66  if(z>0) function(x, ym1, zm1, ##__VA_ARGS__);\
67  }\
68 \
69 \
70  if(x<sizeXm1){\
71  function(xp1, y, z, ##__VA_ARGS__);\
72  if(z<sizeZm1) function(xp1, y, zp1, ##__VA_ARGS__);\
73  if(z>0) function(xp1, y, zm1, ##__VA_ARGS__);\
74 \
75  if(y<sizeYm1){\
76  function(xp1, yp1, z, ##__VA_ARGS__);\
77  if(z<sizeZm1) function(xp1, yp1, zp1, ##__VA_ARGS__);\
78  if(z>0) function(xp1, yp1, zm1, ##__VA_ARGS__);\
79  }\
80 \
81  if(y>0){\
82  function(xp1, ym1, z, ##__VA_ARGS__);\
83  if(z<sizeZm1) function(xp1, ym1, zp1, ##__VA_ARGS__);\
84  if(z>0) function(xp1, ym1, zm1, ##__VA_ARGS__);\
85  }\
86  }\
87 \
88  if(x>0){\
89  function(xm1, y, z, ##__VA_ARGS__);\
90  if(z<sizeZm1) function(xm1, y, zp1, ##__VA_ARGS__);\
91  if(z>0) function(xm1, y, zm1, ##__VA_ARGS__);\
92 \
93  if(y<sizeYm1){\
94  function(xm1, yp1, z, ##__VA_ARGS__);\
95  if(z<sizeZm1) function(xm1, yp1, zp1, ##__VA_ARGS__);\
96  if(z>0) function(xm1, yp1, zm1, ##__VA_ARGS__);\
97  }\
98 \
99  if(y>0){\
100  function(xm1, ym1, z, ##__VA_ARGS__);\
101  if(z<sizeZm1) function(xm1, ym1, zp1, ##__VA_ARGS__);\
102  if(z>0) function(xm1, ym1, zm1, ##__VA_ARGS__);\
103  }\
104  }
105 
106 float DynamicEDT3D::distanceValue_Error = -1.0;
107 int DynamicEDT3D::distanceInCellsValue_Error = -1;
108 
109 DynamicEDT3D::DynamicEDT3D(int _maxdist_squared) {
110  sqrt2 = sqrt(2.0);
111  maxDist_squared = _maxdist_squared;
112  maxDist = sqrt((double) maxDist_squared);
113  data = NULL;
114  gridMap = NULL;
115 }
116 
117 DynamicEDT3D::~DynamicEDT3D() {
118  if (data) {
119  for (int x=0; x<sizeX; x++){
120  for (int y=0; y<sizeY; y++)
121  delete[] data[x][y];
122 
123  delete[] data[x];
124  }
125  delete[] data;
126  }
127 
128  if (gridMap) {
129  for (int x=0; x<sizeX; x++){
130  for (int y=0; y<sizeY; y++)
131  delete[] gridMap[x][y];
132 
133  delete[] gridMap[x];
134  }
135  delete[] gridMap;
136  }
137 }
138 
139 void DynamicEDT3D::initializeEmpty(int _sizeX, int _sizeY, int _sizeZ, bool initGridMap) {
140  sizeX = _sizeX;
141  sizeY = _sizeY;
142  sizeZ = _sizeZ;
143 
144  sizeXm1 = sizeX-1;
145  sizeYm1 = sizeY-1;
146  sizeZm1 = sizeZ-1;
147 
148  if (data) {
149  for (int x=0; x<sizeX; x++){
150  for (int y=0; y<sizeY; y++)
151  delete[] data[x][y];
152 
153  delete[] data[x];
154  }
155  delete[] data;
156  }
157 
158  data = new dataCell**[sizeX];
159  for (int x=0; x<sizeX; x++){
160  data[x] = new dataCell*[sizeY];
161  for(int y=0; y<sizeY; y++)
162  data[x][y] = new dataCell[sizeZ];
163  }
164 
165  if (initGridMap) {
166  if (gridMap) {
167  for (int x=0; x<sizeX; x++){
168  for (int y=0; y<sizeY; y++)
169  delete[] gridMap[x][y];
170 
171  delete[] gridMap[x];
172  }
173  delete[] gridMap;
174  }
175 
176  gridMap = new bool**[sizeX];
177  for (int x=0; x<sizeX; x++){
178  gridMap[x] = new bool*[sizeY];
179  for (int y=0; y<sizeY; y++)
180  gridMap[x][y] = new bool[sizeZ];
181  }
182  }
183 
184  dataCell c;
185  c.dist = maxDist;
186  c.sqdist = maxDist_squared;
187  c.obstX = invalidObstData;
188  c.obstY = invalidObstData;
189  c.obstZ = invalidObstData;
190  c.queueing = fwNotQueued;
191  c.needsRaise = false;
192 
193  for (int x=0; x<sizeX; x++){
194  for (int y=0; y<sizeY; y++){
195  for (int z=0; z<sizeZ; z++){
196  data[x][y][z] = c;
197  }
198  }
199  }
200 
201  if (initGridMap) {
202  for (int x=0; x<sizeX; x++)
203  for (int y=0; y<sizeY; y++)
204  for (int z=0; z<sizeZ; z++)
205  gridMap[x][y][z] = 0;
206  }
207 }
208 
209 void DynamicEDT3D::initializeMap(int _sizeX, int _sizeY, int _sizeZ, bool*** _gridMap) {
210  gridMap = _gridMap;
211  initializeEmpty(_sizeX, _sizeY, _sizeZ, false);
212 
213  for (int x=0; x<sizeX; x++) {
214  for (int y=0; y<sizeY; y++) {
215  for (int z=0; z<sizeZ; z++) {
216  if (gridMap[x][y][z]) {
217  dataCell c = data[x][y][z];
218  if (!isOccupied(x,y,z,c)) {
219 
220  bool isSurrounded = true;
221  for (int dx=-1; dx<=1; dx++) {
222  int nx = x+dx;
223  if (nx<0 || nx>sizeX-1) continue;
224  for (int dy=-1; dy<=1; dy++) {
225  int ny = y+dy;
226  if (ny<0 || ny>sizeY-1) continue;
227  for (int dz=-1; dz<=1; dz++) {
228  if (dx==0 && dy==0 && dz==0) continue;
229  int nz = z+dz;
230  if (nz<0 || nz>sizeZ-1) continue;
231 
232  if (!gridMap[nx][ny][nz]) {
233  isSurrounded = false;
234  break;
235  }
236  }
237  }
238  }
239  if (isSurrounded) {
240  c.obstX = x;
241  c.obstY = y;
242  c.obstZ = z;
243  c.sqdist = 0;
244  c.dist = 0;
245  c.queueing = fwProcessed;
246  data[x][y][z] = c;
247  } else setObstacle(x,y,z);
248  }
249  }
250  }
251  }
252  }
253 }
254 
255 void DynamicEDT3D::occupyCell(int x, int y, int z) {
256  gridMap[x][y][z] = 1;
257  setObstacle(x,y,z);
258 }
259 
260 void DynamicEDT3D::clearCell(int x, int y, int z) {
261  gridMap[x][y][z] = 0;
262  removeObstacle(x,y,z);
263 }
264 
265 void DynamicEDT3D::setObstacle(int x, int y, int z) {
266  dataCell c = data[x][y][z];
267  if(isOccupied(x,y,z,c)) return;
268 
269  addList.push_back(INTPOINT3D(x,y,z));
270  c.obstX = x;
271  c.obstY = y;
272  c.obstZ = z;
273  data[x][y][z] = c;
274 }
275 
276 void DynamicEDT3D::removeObstacle(int x, int y, int z) {
277  dataCell c = data[x][y][z];
278  if(isOccupied(x,y,z,c) == false) return;
279 
280  removeList.push_back(INTPOINT3D(x,y,z));
281  c.obstX = invalidObstData;
282  c.obstY = invalidObstData;
283  c.obstZ = invalidObstData;
284  c.queueing = bwQueued;
285  data[x][y][z] = c;
286 }
287 
288 void DynamicEDT3D::exchangeObstacles(std::vector<INTPOINT3D> points) {
289 
290  for (unsigned int i=0; i<lastObstacles.size(); i++) {
291  int x = lastObstacles[i].x;
292  int y = lastObstacles[i].y;
293  int z = lastObstacles[i].z;
294 
295  bool v = gridMap[x][y][z];
296  if (v) continue;
297  removeObstacle(x,y,z);
298  }
299 
300  lastObstacles.clear();
301 
302  for (unsigned int i=0; i<points.size(); i++) {
303  int x = points[i].x;
304  int y = points[i].y;
305  int z = points[i].z;
306  bool v = gridMap[x][y][z];
307  if (v) continue;
308  setObstacle(x,y,z);
309  lastObstacles.push_back(points[i]);
310  }
311 }
312 
313 void DynamicEDT3D::update(bool updateRealDist) {
314  commitAndColorize(updateRealDist);
315 
316  while (!open.empty()) {
317  INTPOINT3D p = open.pop();
318  int x = p.x;
319  int y = p.y;
320  int z = p.z;
321  dataCell c = data[x][y][z];
322 
323  if(c.queueing==fwProcessed) continue;
324 
325  if (c.needsRaise) {
326  // RAISE
327  raiseCell(p, c, updateRealDist);
328  data[x][y][z] = c;
329  }
330  else if (c.obstX != invalidObstData && isOccupied(c.obstX,c.obstY,c.obstZ,data[c.obstX][c.obstY][c.obstZ])) {
331  // LOWER
332  propagateCell(p, c, updateRealDist);
333  data[x][y][z] = c;
334  }
335  }
336 }
337 
338 void DynamicEDT3D::raiseCell(INTPOINT3D &p, dataCell &c, bool updateRealDist){
339  /*
340  for (int dx=-1; dx<=1; dx++) {
341  int nx = p.x+dx;
342  if (nx<0 || nx>sizeX-1) continue;
343  for (int dy=-1; dy<=1; dy++) {
344  int ny = p.y+dy;
345  if (ny<0 || ny>sizeY-1) continue;
346  for (int dz=-1; dz<=1; dz++) {
347  if (dx==0 && dy==0 && dz==0) continue;
348  int nz = p.z+dz;
349  if (nz<0 || nz>sizeZ-1) continue;
350 
351  inspectCellRaise(nx,ny,nz, updateRealDist);
352  }
353  }
354  }
355 */
356  FOR_EACH_NEIGHBOR_WITH_CHECK(inspectCellRaise,p, updateRealDist)
357 
358  c.needsRaise = false;
359  c.queueing = bwProcessed;
360 }
361 
362 void DynamicEDT3D::inspectCellRaise(int &nx, int &ny, int &nz, bool updateRealDist){
363  dataCell nc = data[nx][ny][nz];
364  if (nc.obstX!=invalidObstData && !nc.needsRaise) {
365  if(!isOccupied(nc.obstX,nc.obstY,nc.obstZ,data[nc.obstX][nc.obstY][nc.obstZ])) {
366  open.push(nc.sqdist, INTPOINT3D(nx,ny,nz));
367  nc.queueing = fwQueued;
368  nc.needsRaise = true;
369  nc.obstX = invalidObstData;
370  nc.obstY = invalidObstData;
371  nc.obstZ = invalidObstData;
372  if (updateRealDist) nc.dist = maxDist;
373  nc.sqdist = maxDist_squared;
374  data[nx][ny][nz] = nc;
375  } else {
376  if(nc.queueing != fwQueued){
377  open.push(nc.sqdist, INTPOINT3D(nx,ny,nz));
378  nc.queueing = fwQueued;
379  data[nx][ny][nz] = nc;
380  }
381  }
382  }
383 }
384 
385 void DynamicEDT3D::propagateCell(INTPOINT3D &p, dataCell &c, bool updateRealDist){
386  c.queueing = fwProcessed;
387  /*
388  for (int dx=-1; dx<=1; dx++) {
389  int nx = p.x+dx;
390  if (nx<0 || nx>sizeX-1) continue;
391  for (int dy=-1; dy<=1; dy++) {
392  int ny = p.y+dy;
393  if (ny<0 || ny>sizeY-1) continue;
394  for (int dz=-1; dz<=1; dz++) {
395  if (dx==0 && dy==0 && dz==0) continue;
396  int nz = p.z+dz;
397  if (nz<0 || nz>sizeZ-1) continue;
398 
399  inspectCellPropagate(nx, ny, nz, c, updateRealDist);
400  }
401  }
402  }
403  */
404 
405  if(c.sqdist==0){
406  FOR_EACH_NEIGHBOR_WITH_CHECK(inspectCellPropagate, p, c, updateRealDist)
407  } else {
408  int x=p.x;
409  int y=p.y;
410  int z=p.z;
411  int xp1 = x+1;
412  int xm1 = x-1;
413  int yp1 = y+1;
414  int ym1 = y-1;
415  int zp1 = z+1;
416  int zm1 = z-1;
417 
418  int dpx = (x - c.obstX);
419  int dpy = (y - c.obstY);
420  int dpz = (z - c.obstZ);
421 
422  // dpy=0;
423  // dpz=0;
424 
425 
426  if(dpz >=0 && z<sizeZm1) inspectCellPropagate(x, y, zp1, c, updateRealDist);
427  if(dpz <=0 && z>0) inspectCellPropagate(x, y, zm1, c, updateRealDist);
428 
429  if(dpy>=0 && y<sizeYm1){
430  inspectCellPropagate(x, yp1, z, c, updateRealDist);
431  if(dpz >=0 && z<sizeZm1) inspectCellPropagate(x, yp1, zp1, c, updateRealDist);
432  if(dpz <=0 && z>0) inspectCellPropagate(x, yp1, zm1, c, updateRealDist);
433  }
434 
435  if(dpy<=0 && y>0){
436  inspectCellPropagate(x, ym1, z, c, updateRealDist);
437  if(dpz >=0 && z<sizeZm1) inspectCellPropagate(x, ym1, zp1, c, updateRealDist);
438  if(dpz <=0 && z>0) inspectCellPropagate(x, ym1, zm1, c, updateRealDist);
439  }
440 
441 
442  if(dpx>=0 && x<sizeXm1){
443  inspectCellPropagate(xp1, y, z, c, updateRealDist);
444  if(dpz >=0 && z<sizeZm1) inspectCellPropagate(xp1, y, zp1, c, updateRealDist);
445  if(dpz <=0 && z>0) inspectCellPropagate(xp1, y, zm1, c, updateRealDist);
446 
447  if(dpy>=0 && y<sizeYm1){
448  inspectCellPropagate(xp1, yp1, z, c, updateRealDist);
449  if(dpz >=0 && z<sizeZm1) inspectCellPropagate(xp1, yp1, zp1, c, updateRealDist);
450  if(dpz <=0 && z>0) inspectCellPropagate(xp1, yp1, zm1, c, updateRealDist);
451  }
452 
453  if(dpy<=0 && y>0){
454  inspectCellPropagate(xp1, ym1, z, c, updateRealDist);
455  if(dpz >=0 && z<sizeZm1) inspectCellPropagate(xp1, ym1, zp1, c, updateRealDist);
456  if(dpz <=0 && z>0) inspectCellPropagate(xp1, ym1, zm1, c, updateRealDist);
457  }
458  }
459 
460  if(dpx<=0 && x>0){
461  inspectCellPropagate(xm1, y, z, c, updateRealDist);
462  if(dpz >=0 && z<sizeZm1) inspectCellPropagate(xm1, y, zp1, c, updateRealDist);
463  if(dpz <=0 && z>0) inspectCellPropagate(xm1, y, zm1, c, updateRealDist);
464 
465  if(dpy>=0 && y<sizeYm1){
466  inspectCellPropagate(xm1, yp1, z, c, updateRealDist);
467  if(dpz >=0 && z<sizeZm1) inspectCellPropagate(xm1, yp1, zp1, c, updateRealDist);
468  if(dpz <=0 && z>0) inspectCellPropagate(xm1, yp1, zm1, c, updateRealDist);
469  }
470 
471  if(dpy<=0 && y>0){
472  inspectCellPropagate(xm1, ym1, z, c, updateRealDist);
473  if(dpz >=0 && z<sizeZm1) inspectCellPropagate(xm1, ym1, zp1, c, updateRealDist);
474  if(dpz <=0 && z>0) inspectCellPropagate(xm1, ym1, zm1, c, updateRealDist);
475  }
476  }
477  }
478 }
479 
480 void DynamicEDT3D::inspectCellPropagate(int &nx, int &ny, int &nz, dataCell &c, bool updateRealDist){
481  dataCell nc = data[nx][ny][nz];
482  if(!nc.needsRaise) {
483  int distx = nx-c.obstX;
484  int disty = ny-c.obstY;
485  int distz = nz-c.obstZ;
486  int newSqDistance = distx*distx + disty*disty + distz*distz;
487  if(newSqDistance > maxDist_squared)
488  newSqDistance = maxDist_squared;
489  bool overwrite = (newSqDistance < nc.sqdist);
490  if(!overwrite && newSqDistance==nc.sqdist) {
491  //the neighbor cell is marked to be raised, has no valid source obstacle
492  if (nc.obstX == invalidObstData){
493  overwrite = true;
494  }
495  else {
496  //the neighbor has no valid source obstacle but the raise wave has not yet reached it
497  dataCell tmp = data[nc.obstX][nc.obstY][nc.obstZ];
498 
499  if((tmp.obstX==nc.obstX && tmp.obstY==nc.obstY && tmp.obstZ==nc.obstZ)==false)
500  overwrite = true;
501  }
502  }
503  if (overwrite) {
504  if(newSqDistance < maxDist_squared){
505  open.push(newSqDistance, INTPOINT3D(nx,ny,nz));
506  nc.queueing = fwQueued;
507  }
508  if (updateRealDist) {
509  nc.dist = sqrt((double) newSqDistance);
510  }
511  nc.sqdist = newSqDistance;
512  nc.obstX = c.obstX;
513  nc.obstY = c.obstY;
514  nc.obstZ = c.obstZ;
515  }
516  data[nx][ny][nz] = nc;
517  }
518 }
519 
520 
521 float DynamicEDT3D::getDistance( int x, int y, int z ) const {
522  if( (x>=0) && (x<sizeX) && (y>=0) && (y<sizeY) && (z>=0) && (z<sizeZ)){
523  return data[x][y][z].dist;
524  }
525  else return distanceValue_Error;
526 }
527 
528 INTPOINT3D DynamicEDT3D::getClosestObstacle( int x, int y, int z ) const {
529  if( (x>=0) && (x<sizeX) && (y>=0) && (y<sizeY) && (z>=0) && (z<sizeZ)){
530  dataCell c = data[x][y][z];
531  return INTPOINT3D(c.obstX, c.obstY, c.obstZ);
532  }
533  else return INTPOINT3D(invalidObstData, invalidObstData, invalidObstData);
534 }
535 
536 int DynamicEDT3D::getSQCellDistance( int x, int y, int z ) const {
537  if( (x>=0) && (x<sizeX) && (y>=0) && (y<sizeY) && (z>=0) && (z<sizeZ)){
538  return data[x][y][z].sqdist;
539  }
540  else return distanceInCellsValue_Error;
541 }
542 
543 
544 void DynamicEDT3D::commitAndColorize(bool updateRealDist) {
545  // ADD NEW OBSTACLES
546  for (unsigned int i=0; i<addList.size(); i++) {
547  INTPOINT3D p = addList[i];
548  int x = p.x;
549  int y = p.y;
550  int z = p.z;
551  dataCell c = data[x][y][z];
552 
553  if(c.queueing != fwQueued){
554  if (updateRealDist) c.dist = 0;
555  c.sqdist = 0;
556  c.obstX = x;
557  c.obstY = y;
558  c.obstZ = z;
559  c.queueing = fwQueued;
560  data[x][y][z] = c;
561  open.push(0, INTPOINT3D(x,y,z));
562  }
563  }
564 
565  // REMOVE OLD OBSTACLES
566  for (unsigned int i=0; i<removeList.size(); i++) {
567  INTPOINT3D p = removeList[i];
568  int x = p.x;
569  int y = p.y;
570  int z = p.z;
571  dataCell c = data[x][y][z];
572 
573  if (isOccupied(x,y,z,c)==true) continue; // obstacle was removed and reinserted
574  open.push(0, INTPOINT3D(x,y,z));
575  if (updateRealDist) c.dist = maxDist;
576  c.sqdist = maxDist_squared;
577  c.needsRaise = true;
578  data[x][y][z] = c;
579  }
580  removeList.clear();
581  addList.clear();
582 }
583 
584 bool DynamicEDT3D::isOccupied(int x, int y, int z) const {
585  dataCell c = data[x][y][z];
586  return (c.obstX==x && c.obstY==y && c.obstZ==z);
587 }
588 
589 bool DynamicEDT3D::isOccupied(int &x, int &y, int &z, dataCell &c) {
590  return (c.obstX==x && c.obstY==y && c.obstZ==z);
591 }
DynamicEDT3D::initializeMap
void initializeMap(int _sizeX, int _sizeY, int sizeZ, bool ***_gridMap)
Initialization with a given binary map (false==free, true==occupied)
Definition: dynamicEDT3D.cpp:209
DynamicEDT3D::clearCell
void clearCell(int x, int y, int z)
remove an obstacle at the specified cell coordinate
Definition: dynamicEDT3D.cpp:260
DynamicEDT3D::setObstacle
void setObstacle(int x, int y, int z)
Definition: dynamicEDT3D.cpp:265
DynamicEDT3D::distanceInCellsValue_Error
static int distanceInCellsValue_Error
distance value returned when requesting distance in cell units for a cell outside the map ...
Definition: dynamicEDT3D.h:91
DynamicEDT3D::removeList
std::vector< INTPOINT3D > removeList
Definition: dynamicEDT3D.h:124
DynamicEDT3D::raiseCell
void raiseCell(INTPOINT3D &p, dataCell &c, bool updateRealDist)
Definition: dynamicEDT3D.cpp:338
DynamicEDT3D::getDistance
float getDistance(int x, int y, int z) const
returns the obstacle distance at the specified location
Definition: dynamicEDT3D.cpp:521
DynamicEDT3D::occupyCell
void occupyCell(int x, int y, int z)
add an obstacle at the specified cell coordinate
Definition: dynamicEDT3D.cpp:255
DynamicEDT3D::lastObstacles
std::vector< INTPOINT3D > lastObstacles
Definition: dynamicEDT3D.h:126
DynamicEDT3D::data
dataCell *** data
Definition: dynamicEDT3D.h:137
BucketPrioQueue::push
void push(int prio, T t)
push an element
BucketPrioQueue::empty
bool empty()
Checks whether the Queue is empty.
DynamicEDT3D::propagateCell
void propagateCell(INTPOINT3D &p, dataCell &c, bool updateRealDist)
Definition: dynamicEDT3D.cpp:385
DynamicEDT3D::open
BucketPrioQueue< INTPOINT3D > open
Definition: dynamicEDT3D.h:122
DynamicEDT3D::exchangeObstacles
void exchangeObstacles(std::vector< INTPOINT3D > newObstacles)
remove old dynamic obstacles and add the new ones
Definition: dynamicEDT3D.cpp:288
DynamicEDT3D::initializeEmpty
void initializeEmpty(int _sizeX, int _sizeY, int sizeZ, bool initGridMap=true)
Initialization with an empty map.
Definition: dynamicEDT3D.cpp:139
DynamicEDT3D::commitAndColorize
void commitAndColorize(bool updateRealDist=true)
Definition: dynamicEDT3D.cpp:544
INTPOINT3D
#define INTPOINT3D
Definition: point.h:42
DynamicEDT3D::addList
std::vector< INTPOINT3D > addList
Definition: dynamicEDT3D.h:125
DynamicEDT3D::maxDist
double maxDist
Definition: dynamicEDT3D.h:145
DynamicEDT3D::distanceValue_Error
static float distanceValue_Error
distance value returned when requesting distance for a cell outside the map
Definition: dynamicEDT3D.h:89
DynamicEDT3D::inspectCellRaise
void inspectCellRaise(int &nx, int &ny, int &nz, bool updateRealDist)
Definition: dynamicEDT3D.cpp:362
DynamicEDT3D::gridMap
bool *** gridMap
Definition: dynamicEDT3D.h:138
DynamicEDT3D::getSQCellDistance
int getSQCellDistance(int x, int y, int z) const
returns the squared obstacle distance in cell units at the specified location
Definition: dynamicEDT3D.cpp:536
BucketPrioQueue::pop
T pop()
return and pop the element with the lowest squared distance */
DynamicEDT3D::isOccupied
bool isOccupied(int x, int y, int z) const
checks whether the specficied location is occupied
Definition: dynamicEDT3D.cpp:584
DynamicEDT3D::update
virtual void update(bool updateRealDist=true)
update distance map to reflect the changes
Definition: dynamicEDT3D.cpp:313
DynamicEDT3D::removeObstacle
void removeObstacle(int x, int y, int z)
Definition: dynamicEDT3D.cpp:276
DynamicEDT3D::getClosestObstacle
INTPOINT3D getClosestObstacle(int x, int y, int z) const
gets the closest occupied cell for that location
Definition: dynamicEDT3D.cpp:528
FOR_EACH_NEIGHBOR_WITH_CHECK
#define FOR_EACH_NEIGHBOR_WITH_CHECK(function, p,...)
Definition: dynamicEDT3D.cpp:43
DynamicEDT3D::DynamicEDT3D
DynamicEDT3D(int _maxdist_squared)
Definition: dynamicEDT3D.cpp:109
DynamicEDT3D::inspectCellPropagate
void inspectCellPropagate(int &nx, int &ny, int &nz, dataCell &c, bool updateRealDist)
Definition: dynamicEDT3D.cpp:480

dynamicEDT3D
Author(s): Christoph Sprunk
autogenerated on Wed Jun 5 2019 19:26:37