Go to the documentation of this file.00001
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #ifndef _DYNAMICEDT3D_H_
00039 #define _DYNAMICEDT3D_H_
00040
00041 #include <limits.h>
00042 #include <queue>
00043
00044 #include "bucketedqueue.h"
00045
00047 class DynamicEDT3D {
00048
00049 public:
00050
00051 DynamicEDT3D(int _maxdist_squared);
00052 ~DynamicEDT3D();
00053
00055 void initializeEmpty(int _sizeX, int _sizeY, int sizeZ, bool initGridMap=true);
00057 void initializeMap(int _sizeX, int _sizeY, int sizeZ, bool*** _gridMap);
00058
00060 void occupyCell(int x, int y, int z);
00062 void clearCell(int x, int y, int z);
00064 void exchangeObstacles(std::vector<INTPOINT3D> newObstacles);
00065
00067 virtual void update(bool updateRealDist=true);
00068
00070 float getDistance( int x, int y, int z ) const;
00072 INTPOINT3D getClosestObstacle( int x, int y, int z ) const;
00073
00075 int getSQCellDistance( int x, int y, int z ) const;
00077 bool isOccupied(int x, int y, int z) const;
00078
00080 unsigned int getSizeX() const {return sizeX;}
00082 unsigned int getSizeY() const {return sizeY;}
00084 unsigned int getSizeZ() const {return sizeZ;}
00085
00086 typedef enum {invalidObstData = INT_MAX} ObstDataState;
00087
00089 static float distanceValue_Error;
00091 static int distanceInCellsValue_Error;
00092
00093 protected:
00094 struct dataCell {
00095 float dist;
00096 int obstX;
00097 int obstY;
00098 int obstZ;
00099 int sqdist;
00100 char queueing;
00101 bool needsRaise;
00102 };
00103
00104 typedef enum {free=0, occupied=1} State;
00105 typedef enum {fwNotQueued=1, fwQueued=2, fwProcessed=3, bwQueued=4, bwProcessed=1} QueueingState;
00106
00107
00108 inline void raiseCell(INTPOINT3D &p, dataCell &c, bool updateRealDist);
00109 inline void propagateCell(INTPOINT3D &p, dataCell &c, bool updateRealDist);
00110 inline void inspectCellRaise(int &nx, int &ny, int &nz, bool updateRealDist);
00111 inline void inspectCellPropagate(int &nx, int &ny, int &nz, dataCell &c, bool updateRealDist);
00112
00113 void setObstacle(int x, int y, int z);
00114 void removeObstacle(int x, int y, int z);
00115
00116 private:
00117 void commitAndColorize(bool updateRealDist=true);
00118
00119 inline bool isOccupied(int &x, int &y, int &z, dataCell &c);
00120
00121
00122 BucketPrioQueue<INTPOINT3D> open;
00123
00124 std::vector<INTPOINT3D> removeList;
00125 std::vector<INTPOINT3D> addList;
00126 std::vector<INTPOINT3D> lastObstacles;
00127
00128
00129 protected:
00130 int sizeX;
00131 int sizeY;
00132 int sizeZ;
00133 int sizeXm1;
00134 int sizeYm1;
00135 int sizeZm1;
00136
00137 dataCell*** data;
00138 bool*** gridMap;
00139
00140
00141 int padding;
00142 double doubleThreshold;
00143
00144 double sqrt2;
00145 double maxDist;
00146 int maxDist_squared;
00147 };
00148
00149
00150 #endif
00151