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...
Definition: RandomNumbers.h:58
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 ...
Definition: Decomposition.h:63
virtual int getDimension() const
Returns the dimension of this Decomposition.
Definition: Decomposition.h:83
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.
Definition: Decomposition.h:89
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.