XXLPositionDecomposition.h
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2015, 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: Ryan Luna */
36 
37 #ifndef OMPL_GEOMETRIC_PLANNERS_XXL_XXLPOSITIONDECOMPOSITION_
38 #define OMPL_GEOMETRIC_PLANNERS_XXL_XXLPOSITIONDECOMPOSITION_
39 
40 #include "ompl/geometric/planners/xxl/XXLDecomposition.h"
41 #include "ompl/base/spaces/RealVectorBounds.h"
42 #include "ompl/util/RandomNumbers.h"
43 
44 namespace ompl
45 {
46  namespace geometric
47  {
48  // A decomposition of an n-dimensional Euclidean space
49  class XXLPositionDecomposition : public XXLDecomposition
50  {
51  public:
52  XXLPositionDecomposition(const base::RealVectorBounds &bounds, const std::vector<int> &slices,
53  bool diagonalEdges = false);
54  virtual ~XXLPositionDecomposition();
55 
56  virtual int getNumRegions() const;
57 
59  virtual int getDimension() const;
60 
62  virtual int numLayers() const = 0;
63 
65  virtual int locateRegion(const base::State *s) const;
66 
68  virtual int locateRegion(const std::vector<double> &coord) const;
69 
71  virtual void getNeighbors(int rid, std::vector<int> &neighbors) const;
72 
75  virtual void getNeighborhood(int rid, std::vector<int> &neighborhood) const;
76 
78  virtual double distanceHeuristic(int r1, int r2) const;
79 
81  virtual bool sampleFromRegion(int r, base::State *s, const base::State *seed = nullptr) const = 0;
82 
84  virtual bool sampleFromRegion(int r, base::State *s, const base::State *seed, int layer) const = 0;
85 
87  virtual void project(const base::State *s, std::vector<double> &coord, int layer = 0) const = 0;
88 
90  virtual void project(const base::State *s, std::vector<int> &layers) const = 0;
91 
92  // \brief Return the (discrete) grid cell coordinates of the given region id
93  void ridToGridCell(int rid, std::vector<int> &cell) const;
94 
96  int gridCellToRid(const std::vector<int> &cell) const;
97 
99  int coordToRegion(const std::vector<double> &coord) const;
100  int coordToRegion(const double *coord) const;
101 
103  bool hasDiagonalEdges() const;
104 
105  protected:
106  // Compute the neighbors of rid (no diagonal edges)
107  void getNonDiagonalNeighbors(int rid, std::vector<int> &neighbors) const;
108 
109  // Compute the neighbors of rid (including diagonal edges)
110  void getDiagonalNeighbors(int rid, std::vector<int> &neighbors) const;
111 
112  // The bounds on the Euclidean part of the decomposition
114 
115  // The number of divisions in each Euclidean dimension
116  const std::vector<int> slices_;
117  // The extent of the grid cells in each dimension
118  std::vector<double> cellSizes_;
119 
120  // If true, the decomposition has diagonal edges
121  bool diagonalEdges_;
122 
123  int numRegions_;
124  };
125  } // namespace geometric
126 } // namespace ompl
127 
128 #endif
virtual int numLayers() const =0
Return the number of layers possible in this decomposition. Must be at least 1.
virtual int getNumRegions() const
Returns the number of regions in this XXLDecomposition.
virtual void getNeighborhood(int rid, std::vector< int > &neighborhood) const
Stores the given region's neighbors into the vector. This returns the 8-connected grid neighbors of t...
virtual double distanceHeuristic(int r1, int r2) const
An admissible and consistent distance heuristic between two regions. Manhattan distance on grid.
bool hasDiagonalEdges() const
Return true if the decomposition has diagonal edges.
virtual bool sampleFromRegion(int r, base::State *s, const base::State *seed=nullptr) const =0
Sample a state s from region r in layer 0.
virtual int locateRegion(const base::State *s) const
Return the id of the region that this state falls in.
int coordToRegion(const std::vector< double > &coord) const
Return the region id of the given coordinate in the decomposition.
virtual void project(const base::State *s, std::vector< double > &coord, int layer=0) const =0
Project the given State into the XXLDecomposition.
int gridCellToRid(const std::vector< int > &cell) const
Return the region id corresponding to the (discrete) grid cell coordinates.
virtual void getNeighbors(int rid, std::vector< int > &neighbors) const
Stores the given region's neighbors into a given vector.
virtual int getDimension() const
Return the dimension of this HiLoDecomposition.
Main namespace. Contains everything in this library.
The lower and upper bounds for an Rn space.