ompl/extensions/triangle/TriangularDecomposition.h
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2012, 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_EXTENSIONS_TRIANGLE_TRIANGULARDECOMPOSITION_
00038 #define OMPL_EXTENSIONS_TRIANGLE_TRIANGULARDECOMPOSITION_
00039 
00040 #include "ompl/base/State.h"
00041 #include "ompl/base/StateSampler.h"
00042 #include "ompl/base/spaces/RealVectorBounds.h"
00043 #include "ompl/control/planners/syclop/Decomposition.h"
00044 #include "ompl/control/planners/syclop/GridDecomposition.h"
00045 #include "ompl/util/RandomNumbers.h"
00046 #include <ostream>
00047 #include <vector>
00048 #include <set>
00049 
00050 namespace ompl
00051 {
00052     namespace control
00053     {
00055         class TriangularDecomposition : public Decomposition
00056         {
00057             // TODO: Switch all geometry code to use boost::geometry.
00058             // This requires that we use boost version 1.47 or greater.
00059         public:
00060             struct Vertex
00061             {
00062                 Vertex(void) {}
00063                 Vertex(double vx, double vy);
00064                 bool operator==(const Vertex& v) const;
00065                 friend std::size_t hash_value(const Vertex& v);
00066                 double x, y;
00067             };
00068 
00069             //A polygon is a list of vertices in counter-clockwise order.
00070             struct Polygon
00071             {
00072                 Polygon(int nv) : pts(nv) {}
00073                 virtual ~Polygon() {}
00074                 std::vector<Vertex> pts;
00075             };
00076 
00077             struct Triangle : public Polygon
00078             {
00079                 Triangle() : Polygon(3) {}
00080                 virtual ~Triangle() {}
00081                 std::vector<int> neighbors;
00082                 double volume;
00083             };
00084 
00090             TriangularDecomposition(
00091                 const base::RealVectorBounds &bounds,
00092                 const std::vector<Polygon> &holes = std::vector<Polygon>(),
00093                 const std::vector<Polygon> &intRegs = std::vector<Polygon>()
00094             );
00095 
00096             virtual ~TriangularDecomposition(void);
00097 
00098             virtual int getNumRegions(void) const { return triangles_.size(); }
00099 
00100             virtual double getRegionVolume(int triID);
00101 
00102             virtual void getNeighbors(int triID, std::vector<int>& neighbors) const;
00103 
00104             virtual int locateRegion(const base::State* s) const;
00105 
00106             virtual void sampleFromRegion(int triID, RNG& rng, std::vector<double>& coord) const;
00107 
00108             void setup(void);
00109 
00110             void addHole(const Polygon& hole);
00111 
00112             void addRegionOfInterest(const Polygon& region);
00113 
00114             int getNumHoles(void) const;
00115 
00116             int getNumRegionsOfInterest(void) const;
00117 
00118             const std::vector<Polygon>& getHoles(void) const;
00119 
00120             const std::vector<Polygon>& getAreasOfInterest(void) const;
00121 
00124             int getRegionOfInterestAt(int triID) const;
00125 
00126             //Debug method: prints this decomposition as a list of polygons
00127             void print(std::ostream& out) const;
00128 
00129         protected:
00131             virtual int createTriangles();
00132 
00133             std::vector<Triangle> triangles_;
00134             std::vector<Polygon> holes_;
00135             std::vector<Polygon> intRegs_;
00139             std::vector<int> intRegInfo_;
00140             double triAreaPct_;
00141 
00142         private:
00143             class LocatorGrid : public GridDecomposition
00144             {
00145             public:
00146                 LocatorGrid(int len, const Decomposition *d) :
00147                     GridDecomposition(len, d->getDimension(), d->getBounds()),
00148                     triDecomp(d)
00149                 {
00150                 }
00151 
00152                 virtual ~LocatorGrid()
00153                 {
00154                 }
00155 
00156                 virtual void project(const base::State *s, std::vector<double>& coord) const
00157                 {
00158                     triDecomp->project(s, coord);
00159                 }
00160 
00161                 virtual void sampleFullState(const base::StateSamplerPtr& /*sampler*/, const std::vector<double>& /*coord*/, base::State* /*s*/) const
00162                 {
00163                 }
00164 
00165                 const std::vector<int>& locateTriangles(const base::State *s) const
00166                 {
00167                     return regToTriangles_[locateRegion(s)];
00168                 }
00169 
00170                 void buildTriangleMap(const std::vector<Triangle>& triangles);
00171 
00172             protected:
00173                 const Decomposition *triDecomp;
00174                 /* map from locator grid cell ID to set of triangles with which
00175                  * that cell intersects */
00176                 std::vector<std::vector<int> > regToTriangles_;
00177             };
00178 
00180             void buildLocatorGrid();
00181 
00183             static bool triContains(const Triangle& tri, const std::vector<double>& coord);
00184 
00186             static Vertex getPointInPoly(const Polygon& poly);
00187 
00188             LocatorGrid locator;
00189         };
00190     }
00191 }
00192 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines