All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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_CONTROL_PLANNERS_SYCLOP_TRIANGULARDECOMPOSITION_
38 #define OMPL_CONTROL_PLANNERS_SYCLOP_TRIANGULARDECOMPOSITION_
39 
40 #include "ompl/base/State.h"
41 #include "ompl/base/spaces/RealVectorBounds.h"
42 #include "ompl/control/planners/syclop/Decomposition.h"
43 #include "ompl/control/planners/syclop/GridDecomposition.h"
44 
45 namespace ompl
46 {
47  namespace control
48  {
51  {
52  public:
53  virtual ~TriangularDecomposition()
54  {
55  }
56 
57  virtual double getRegionVolume(unsigned int triID);
58 
59  virtual void getNeighbors(unsigned int triID, std::vector<unsigned int>& neighbors) const;
60 
61  virtual int locateRegion(const base::State* s) const;
62 
63  virtual void sampleFromRegion(unsigned int triID, RNG& rng, std::vector<double>& coord) const;
64 
65  //Debug method: prints this decomposition as a list of polygons
66  void print(std::ostream& out) const;
67 
68 
69  protected:
70  struct Vertex
71  {
72  double x, y;
73  };
74 
75  //A polygon is a list of vertices in counter-clockwise order.
76  struct Polygon
77  {
78  Polygon(unsigned int nv) : pts(nv) {}
79  virtual ~Polygon() {}
80  std::vector<Vertex> pts;
81  };
82 
83  struct Triangle : public Polygon
84  {
85  Triangle(void) : Polygon(3) {}
86  virtual ~Triangle() {}
87  std::vector<unsigned int> neighbors;
88  double volume;
89  };
90 
94  TriangularDecomposition(unsigned int dim, const base::RealVectorBounds& b,
95  const std::vector<Polygon>& holes = std::vector<Polygon>());
96 
98  virtual unsigned int createTriangles();
99 
100  std::vector<Triangle> triangles_;
101  std::vector<Polygon> holes_;
102 
103  private:
104  class LocatorGrid : public GridDecomposition
105  {
106  public:
107  LocatorGrid(unsigned int len, const Decomposition* d) :
108  GridDecomposition(len, d->getDimension(), d->getBounds()),
109  triDecomp(d)
110  {
111  }
112 
113  virtual ~LocatorGrid()
114  {
115  }
116 
117  virtual void project(const base::State* s, std::vector<double>& coord) const
118  {
119  triDecomp->project(s, coord);
120  }
121 
122  virtual void sampleFullState(const base::StateSamplerPtr& /*sampler*/, const std::vector<double>& /*coord*/, base::State* /*s*/) const
123  {
124  }
125 
126  const std::vector<unsigned int>& locateTriangles(const base::State* s) const
127  {
128  return regToTriangles_[locateRegion(s)];
129  }
130 
131  void buildTriangleMap(const std::vector<Triangle>& triangles);
132 
133  protected:
134  const Decomposition* triDecomp;
135  /* map from locator grid cell ID to set of triangles with which
136  * that cell intersects */
137  std::vector<std::vector<unsigned int> > regToTriangles_;
138  };
139 
141  void buildLocatorGrid();
142 
144  bool triContains(const Triangle& tri, const std::vector<double>& coord) const;
145 
147  Vertex pointWithinPoly(const Polygon& poly) const;
148 
149  LocatorGrid locator;
150  };
151  }
152 }
153 #endif
A TriangularDecomposition is a triangulation that ignores obstacles.
virtual void getNeighbors(unsigned int triID, std::vector< unsigned int > &neighbors) const
Stores a given region'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...
TriangularDecomposition(unsigned int dim, const base::RealVectorBounds &b, const std::vector< Polygon > &holes=std::vector< Polygon >())
Constructor. Creates a TriangularDecomposition over the given bounds, which must be 2-dimensional...
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 sampleFromRegion(unsigned int triID, RNG &rng, std::vector< double > &coord) const
Samples a projected coordinate from a given region.
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.
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:54
virtual unsigned int createTriangles()
Helper method to triangulate the space and return the number of triangles.
virtual double getRegionVolume(unsigned int triID)
Returns the volume of a given region in this Decomposition.
Definition of an abstract state.
Definition: State.h:50
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 unsigned int getDimension() const
Returns the dimension of this Decomposition.
Definition: Decomposition.h:88
Decomposition(unsigned int dim, const base::RealVectorBounds &b, unsigned int nreg=0)
Constructor. Creates a Decomposition with a given dimension and a given set of bounds. Accepts as an optional argument a given number of regions.
Definition: Decomposition.h:69
The lower and upper bounds for an Rn space.
virtual const base::RealVectorBounds & getBounds() const
Returns the bounds of this Decomposition.
Definition: Decomposition.h:94