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  {
55  class TriangularDecomposition : public Decomposition
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() = default;
63  Vertex(double vx, double vy);
64  bool operator==(const Vertex &v) const;
65  double x, y;
66  };
67 
68  // A polygon is a list of vertices in counter-clockwise order.
69  struct Polygon
70  {
71  Polygon(int nv) : pts(nv)
72  {
73  }
74  virtual ~Polygon() = default;
75  std::vector<Vertex> pts;
76  };
77 
78  struct Triangle : public Polygon
79  {
80  Triangle() : Polygon(3)
81  {
82  }
83  ~Triangle() override = default;
84  std::vector<int> neighbors;
85  double volume;
86  };
87 
93  TriangularDecomposition(const base::RealVectorBounds &bounds,
94  std::vector<Polygon> holes = std::vector<Polygon>(),
95  std::vector<Polygon> intRegs = std::vector<Polygon>());
96 
97  ~TriangularDecomposition() override;
98 
99  int getNumRegions() const override
100  {
101  return triangles_.size();
102  }
103 
104  double getRegionVolume(int triID) override;
105 
106  void getNeighbors(int triID, std::vector<int> &neighbors) const override;
107 
108  int locateRegion(const base::State *s) const override;
109 
110  void sampleFromRegion(int triID, RNG &rng, std::vector<double> &coord) const override;
111 
112  void setup();
113 
114  void addHole(const Polygon &hole);
115 
116  void addRegionOfInterest(const Polygon &region);
117 
118  int getNumHoles() const;
119 
120  int getNumRegionsOfInterest() const;
121 
122  const std::vector<Polygon> &getHoles() const;
123 
124  const std::vector<Polygon> &getAreasOfInterest() const;
125 
128  int getRegionOfInterestAt(int triID) const;
129 
130  // Debug method: prints this decomposition as a list of polygons
131  void print(std::ostream &out) const;
132 
133  protected:
135  virtual int createTriangles();
136 
137  std::vector<Triangle> triangles_;
138  std::vector<Polygon> holes_;
139  std::vector<Polygon> intRegs_;
143  std::vector<int> intRegInfo_;
144  double triAreaPct_;
145 
146  private:
147  class LocatorGrid : public GridDecomposition
148  {
149  public:
150  LocatorGrid(int len, const Decomposition *d)
151  : GridDecomposition(len, d->getDimension(), d->getBounds()), triDecomp(d)
152  {
153  }
154 
155  ~LocatorGrid() override = default;
156 
157  void project(const base::State *s, std::vector<double> &coord) const override
158  {
159  triDecomp->project(s, coord);
160  }
161 
162  void sampleFullState(const base::StateSamplerPtr & /*sampler*/, const std::vector<double> & /*coord*/,
163  base::State * /*s*/) const override
164  {
165  }
166 
167  const std::vector<int> &locateTriangles(const base::State *s) const
168  {
169  return regToTriangles_[locateRegion(s)];
170  }
171 
172  void buildTriangleMap(const std::vector<Triangle> &triangles);
173 
174  protected:
175  const Decomposition *triDecomp;
176  /* map from locator grid cell ID to set of triangles with which
177  * that cell intersects */
178  std::vector<std::vector<int>> regToTriangles_;
179  };
180 
182  void buildLocatorGrid();
183 
185  static bool triContains(const Triangle &tri, const std::vector<double> &coord);
186 
188  static Vertex getPointInPoly(const Polygon &poly);
189 
190  LocatorGrid locator;
191  };
192  }
193 }
194 #endif
void sampleFromRegion(int triID, RNG &rng, std::vector< double > &coord) const override
Samples a projected coordinate from a given region.
double getRegionVolume(int triID) override
Returns the volume of a given region in this Decomposition.
virtual int getDimension() const
Returns the dimension of this Decomposition.
Definition of an abstract state.
Definition: State.h:113
A GridDecomposition is a Decomposition implemented using a grid.
int getRegionOfInterestAt(int triID) const
Returns the region of interest that contains the given triangle ID. Returns -1 if the triangle ID is ...
virtual int createTriangles()
Helper method to triangulate the space and return the number of triangles.
virtual const base::RealVectorBounds & getBounds() const
Returns the bounds of this Decomposition.
A Decomposition is a partition of a bounded Euclidean space into a fixed number of regions which are ...
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.
int locateRegion(const base::State *s) const override
Returns the index of the region containing a given State. Most often, this is obtained by first calli...
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.
int getNumRegions() const override
Returns the number of regions in this Decomposition.
void getNeighbors(int triID, std::vector< int > &neighbors) const override
Stores a given region's neighbors into a given vector.
std::vector< int > intRegInfo_
Maps from triangle ID to index of Polygon in intReg_ that contains the triangle ID....
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
TriangularDecomposition(const base::RealVectorBounds &bounds, std::vector< Polygon > holes=std::vector< Polygon >(), std::vector< Polygon > intRegs=std::vector< Polygon >())
Creates a TriangularDecomposition over the given bounds, which must be 2-dimensional....