00001
00002
00003
00004
00005
00006
00007
00008
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 #ifndef OMPL_CONTROL_PLANNERS_SYCLOP_GRIDDECOMPOSITION_
00038 #define OMPL_CONTROL_PLANNERS_SYCLOP_GRIDDECOMPOSITION_
00039
00040 #include <cstdlib>
00041 #include <boost/shared_ptr.hpp>
00042 #include <boost/unordered_map.hpp>
00043 #include "ompl/base/spaces/RealVectorBounds.h"
00044 #include "ompl/base/State.h"
00045 #include "ompl/control/planners/syclop/Decomposition.h"
00046
00047 namespace ompl
00048 {
00049 namespace control
00050 {
00052 class GridDecomposition : public Decomposition
00053 {
00054 public:
00056 GridDecomposition(const int len, const std::size_t dim, const base::RealVectorBounds& b);
00057
00058 virtual ~GridDecomposition()
00059 {
00060 }
00061
00062 virtual double getRegionVolume(const int rid) const
00063 {
00064 return cellVolume_;
00065 }
00066
00067 virtual void getNeighbors(const int rid, std::vector<int>& neighbors) const;
00068
00069 virtual int locateRegion(const base::State* s) const;
00070
00071 virtual const base::RealVectorBounds& getRegionBounds(const int rid);
00072
00073 protected:
00075 void regionToCoord(int rid, std::vector<int>& coord) const;
00076
00078 int coordToRegion (const std::vector<int> &coord) const;
00079
00081 void computeGridNeighbors (int rid, std::vector <int> &neighbors) const;
00082
00084 void computeGridNeighborsSub (const std::vector <int>&coord, std::vector <int> &neighbors,
00085 unsigned int dim, std::vector <int> &candidate) const;
00086
00087 const int length_;
00088 double cellVolume_;
00089 boost::unordered_map<int, boost::shared_ptr<base::RealVectorBounds> > regToBounds_;
00090
00091 private:
00093 int calcNumRegions(const int len, const std::size_t dim) const;
00094 };
00095 }
00096 }
00097 #endif