dynamicEDT3D.h
Go to the documentation of this file.
00001 
00009 /*
00010  * Copyright (c) 2011-2012, C. Sprunk, B. Lau, W. Burgard, University of Freiburg
00011  * All rights reserved.
00012  *
00013  * Redistribution and use in source and binary forms, with or without
00014  * modification, are permitted provided that the following conditions are met:
00015  *
00016  *     * Redistributions of source code must retain the above copyright
00017  *       notice, this list of conditions and the following disclaimer.
00018  *     * Redistributions in binary form must reproduce the above copyright
00019  *       notice, this list of conditions and the following disclaimer in the
00020  *       documentation and/or other materials provided with the distribution.
00021  *     * Neither the name of the University of Freiburg nor the names of its
00022  *       contributors may be used to endorse or promote products derived from
00023  *       this software without specific prior written permission.
00024  *
00025  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00026  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00027  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00028  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00029  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00030  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00031  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00032  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00033  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00034  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035  * POSSIBILITY OF SUCH DAMAGE.
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   // methods
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   // queues
00122   BucketPrioQueue<INTPOINT3D> open;
00123 
00124   std::vector<INTPOINT3D> removeList;
00125   std::vector<INTPOINT3D> addList;
00126   std::vector<INTPOINT3D> lastObstacles;
00127 
00128   // maps
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   // parameters
00141   int padding;
00142   double doubleThreshold;
00143 
00144   double sqrt2;
00145   double maxDist;
00146   int maxDist_squared;
00147 };
00148 
00149 
00150 #endif
00151 


dynamicEDT3D
Author(s): Christoph Sprunk
autogenerated on Thu Feb 11 2016 23:51:18