TriangularDecomposition.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2012, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Matt Maly */
36 
37 #ifndef OMPL_EXTENSIONS_TRIANGLE_TRIANGULARDECOMPOSITION_
38 #define OMPL_EXTENSIONS_TRIANGLE_TRIANGULARDECOMPOSITION_
39 
40 #include "ompl/base/State.h"
41 #include "ompl/base/StateSampler.h"
42 #include "ompl/base/spaces/RealVectorBounds.h"
43 #include "ompl/control/planners/syclop/Decomposition.h"
44 #include "ompl/control/planners/syclop/GridDecomposition.h"
45 #include "ompl/util/RandomNumbers.h"
46 #include <ostream>
47 #include <vector>
48 #include <set>
49 
50 namespace ompl
51 {
52  namespace control
53  {
56  {
57  // TODO: Switch all geometry code to use boost::geometry.
58  // This requires that we use boost version 1.47 or greater.
59  public:
60  struct Vertex
61  {
62  Vertex(void) {}
63  Vertex(double vx, double vy);
64  bool operator==(const Vertex& v) const;
65  friend std::size_t hash_value(const Vertex& v);
66  double x, y;
67  };
68 
69  //A polygon is a list of vertices in counter-clockwise order.
70  struct Polygon
71  {
72  Polygon(int nv) : pts(nv) {}
73  virtual ~Polygon() {}
74  std::vector<Vertex> pts;
75  };
76 
77  struct Triangle : public Polygon
78  {
79  Triangle() : Polygon(3) {}
80  virtual ~Triangle() {}
81  std::vector<int> neighbors;
82  double volume;
83  };
84 
91  const base::RealVectorBounds &bounds,
92  const std::vector<Polygon> &holes = std::vector<Polygon>(),
93  const std::vector<Polygon> &intRegs = std::vector<Polygon>()
94  );
95 
96  virtual ~TriangularDecomposition(void);
97 
98  virtual int getNumRegions(void) const { return triangles_.size(); }
99 
100  virtual double getRegionVolume(int triID);
101 
102  virtual void getNeighbors(int triID, std::vector<int>& neighbors) const;
103 
104  virtual int locateRegion(const base::State* s) const;
105 
106  virtual void sampleFromRegion(int triID, RNG& rng, std::vector<double>& coord) const;
107 
108  void setup(void);
109 
110  void addHole(const Polygon& hole);
111 
112  void addRegionOfInterest(const Polygon& region);
113 
114  int getNumHoles(void) const;
115 
116  int getNumRegionsOfInterest(void) const;
117 
118  const std::vector<Polygon>& getHoles(void) const;
119 
120  const std::vector<Polygon>& getAreasOfInterest(void) const;
121 
124  int getRegionOfInterestAt(int triID) const;
125 
126  //Debug method: prints this decomposition as a list of polygons
127  void print(std::ostream& out) const;
128 
129  protected:
131  virtual int createTriangles();
132 
133  std::vector<Triangle> triangles_;
134  std::vector<Polygon> holes_;
135  std::vector<Polygon> intRegs_;
139  std::vector<int> intRegInfo_;
140  double triAreaPct_;
141 
142  private:
143  class LocatorGrid : public GridDecomposition
144  {
145  public:
146  LocatorGrid(int len, const Decomposition *d) :
147  GridDecomposition(len, d->getDimension(), d->getBounds()),
148  triDecomp(d)
149  {
150  }
151 
152  virtual ~LocatorGrid()
153  {
154  }
155 
156  virtual void project(const base::State *s, std::vector<double>& coord) const
157  {
158  triDecomp->project(s, coord);
159  }
160 
161  virtual void sampleFullState(const base::StateSamplerPtr& /*sampler*/, const std::vector<double>& /*coord*/, base::State* /*s*/) const
162  {
163  }
164 
165  const std::vector<int>& locateTriangles(const base::State *s) const
166  {
167  return regToTriangles_[locateRegion(s)];
168  }
169 
170  void buildTriangleMap(const std::vector<Triangle>& triangles);
171 
172  protected:
173  const Decomposition *triDecomp;
174  /* map from locator grid cell ID to set of triangles with which
175  * that cell intersects */
176  std::vector<std::vector<int> > regToTriangles_;
177  };
178 
180  void buildLocatorGrid();
181 
183  static bool triContains(const Triangle& tri, const std::vector<double>& coord);
184 
186  static Vertex getPointInPoly(const Polygon& poly);
187 
188  LocatorGrid locator;
189  };
190  }
191 }
192 #endif
A TriangularDecomposition is a triangulation that ignores obstacles.
virtual void getNeighbors(int triID, std::vector< int > &neighbors) const
Stores a given region&#39;s neighbors into a given vector.
virtual int locateRegion(const base::State *s) const
Returns the index of the region containing a given State. Most often, this is obtained by first calli...
int getRegionOfInterestAt(int triID) const
Returns the region of interest that contains the given triangle ID. Returns -1 if the triangle ID is ...
A boost shared pointer wrapper for ompl::base::StateSampler.
virtual double getRegionVolume(int triID)
Returns the volume of a given region in this Decomposition.
virtual int createTriangles()
Helper method to triangulate the space and return the number of triangles.
A GridDecomposition is a Decomposition implemented using a grid.
A Decomposition is a partition of a bounded Euclidean space into a fixed number of regions which are ...
Definition: Decomposition.h:62
virtual void sampleFullState(const base::StateSamplerPtr &sampler, const std::vector< double > &coord, base::State *s) const =0
Samples a State using a projected coordinate and a StateSampler.
Main namespace. Contains everything in this library.
Definition: Cost.h:42
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:54
virtual const base::RealVectorBounds & getBounds() const
Returns the bounds of this Decomposition.
Definition: Decomposition.h:91
virtual void sampleFromRegion(int triID, RNG &rng, std::vector< double > &coord) const
Samples a projected coordinate from a given region.
virtual int getDimension() const
Returns the dimension of this Decomposition.
Definition: Decomposition.h:85
Definition of an abstract state.
Definition: State.h:50
std::vector< int > intRegInfo_
Maps from triangle ID to index of Polygon in intReg_ that contains the triangle ID. Maps to -1 if the triangle ID is not in a region of interest.
virtual void project(const base::State *s, std::vector< double > &coord) const =0
Project a given State to a set of coordinates in R^k, where k is the dimension of this Decomposition...
The lower and upper bounds for an Rn space.
virtual int getNumRegions(void) const
Returns the number of regions in this Decomposition.
TriangularDecomposition(const base::RealVectorBounds &bounds, const std::vector< Polygon > &holes=std::vector< Polygon >(), const std::vector< Polygon > &intRegs=std::vector< Polygon >())
Creates a TriangularDecomposition over the given bounds, which must be 2-dimensional. The underlying mesh will be a conforming Delaunay triangulation. The triangulation will ignore any obstacles, given as a list of polygons. The triangulation will respect the boundaries of any regions of interest, given as a list of polygons. No two obstacles may overlap, and no two regions of interest may overlap.