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