Loading...
Searching...
No Matches
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
50namespace 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() = 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
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
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
The lower and upper bounds for an Rn space.
Definition of an abstract state.
Definition State.h:50
A Decomposition is a partition of a bounded Euclidean space into a fixed number of regions which are ...
virtual int getDimension() const
Returns the dimension of this Decomposition.
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.
virtual const base::RealVectorBounds & getBounds() const
Returns the bounds of this Decomposition.
A GridDecomposition is a Decomposition implemented using a grid.
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...
A TriangularDecomposition is a triangulation that ignores obstacles.
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 int createTriangles()
Helper method to triangulate the space and return the number of triangles.
std::vector< int > intRegInfo_
Maps from triangle ID to index of Polygon in intReg_ that contains the triangle ID....
int getRegionOfInterestAt(int triID) const
Returns the region of interest that contains the given triangle ID. Returns -1 if the triangle ID is ...
void sampleFromRegion(int triID, RNG &rng, std::vector< double > &coord) const override
Samples a projected coordinate from a given region.
void getNeighbors(int triID, std::vector< int > &neighbors) const override
Stores a given region's neighbors into a given vector.
double getRegionVolume(int triID) override
Returns the volume of a given region in this Decomposition.
int getNumRegions() const override
Returns the number of regions in this Decomposition.
Main namespace. Contains everything in this library.