ompl/control/planners/syclop/GridDecomposition.h
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2011, Rice University 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Rice University nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Matt Maly */ 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 #include "ompl/util/RandomNumbers.h" 00047 00048 namespace ompl 00049 { 00050 namespace control 00051 { 00053 class GridDecomposition : public Decomposition 00054 { 00055 public: 00059 GridDecomposition(int len, int dim, const base::RealVectorBounds &b); 00060 00061 virtual ~GridDecomposition() 00062 { 00063 } 00064 00065 virtual int getNumRegions(void) const { return numGridCells_; } 00066 00067 virtual double getRegionVolume(int /*rid*/) 00068 { 00069 return cellVolume_; 00070 } 00071 00072 virtual void getNeighbors(int rid, std::vector<int>& neighbors) const; 00073 00074 virtual int locateRegion(const base::State *s) const; 00075 00076 virtual void sampleFromRegion(int rid, RNG &rng, std::vector<double>& coord) const; 00077 00078 protected: 00080 virtual const base::RealVectorBounds& getRegionBounds(int rid) const; 00081 00083 void regionToGridCoord(int rid, std::vector<int>& coord) const; 00084 00086 int gridCoordToRegion (const std::vector<int> &coord) const; 00087 00089 int coordToRegion(const std::vector<double>& coord) const; 00090 00092 void coordToGridCoord(const std::vector<double>& coord, std::vector<int>& gridCoord) const; 00093 00095 void computeGridNeighbors (int rid, std::vector <int> &neighbors) const; 00096 00098 void computeGridNeighborsSub (const std::vector<int>& coord, std::vector<int>& neighbors, 00099 int dim, std::vector<int>& candidate) const; 00100 00101 int length_; 00102 double cellVolume_; 00103 mutable boost::unordered_map<int, boost::shared_ptr<base::RealVectorBounds> > regToBounds_; 00104 00105 private: 00106 const int numGridCells_; 00107 }; 00108 } 00109 } 00110 #endif