All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
ConnectionStrategy.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, 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: James D. Marble */
36 
37 #ifndef OMPL_GEOMETRIC_PLANNERS_PRM_CONNECTION_STRATEGY_
38 #define OMPL_GEOMETRIC_PLANNERS_PRM_CONNECTION_STRATEGY_
39 
40 #include "ompl/datastructures/NearestNeighbors.h"
41 #include <boost/function.hpp>
42 #include <boost/shared_ptr.hpp>
43 #include <boost/math/constants/constants.hpp>
44 #include <vector>
45 
46 namespace ompl
47 {
48 
49  namespace geometric
50  {
51 
55  template <class Milestone>
56  class KStrategy
57  {
58  public:
59 
62  KStrategy(const unsigned int k,
63  const boost::shared_ptr< NearestNeighbors<Milestone> > &nn) :
64  k_(k), nn_(nn)
65  {
66  neighbors_.reserve(k_);
67  }
68 
69  virtual ~KStrategy(void)
70  {
71  }
72 
74  void setNearestNeighbors(const boost::shared_ptr< NearestNeighbors<Milestone> > &nn)
75  {
76  nn_ = nn;
77  }
78 
82  std::vector<Milestone>& operator()(const Milestone& m)
83  {
84  nn_->nearestK(m, k_, neighbors_);
85  return neighbors_;
86  }
87 
88  protected:
89 
91  unsigned int k_;
92 
94  boost::shared_ptr< NearestNeighbors<Milestone> > nn_;
95 
97  std::vector<Milestone> neighbors_;
98  };
99 
125  template <class Milestone>
126  class KStarStrategy : public KStrategy<Milestone>
127  {
128  public:
129  typedef boost::function<unsigned int()> NumNeighborsFn;
139  KStarStrategy(const NumNeighborsFn& n,
140  const boost::shared_ptr< NearestNeighbors<Milestone> > &nn,
141  const unsigned int d = 1) :
142  KStrategy<Milestone>(n(), nn), n_(n),
143  kPRMConstant_(boost::math::constants::e<double>() + (boost::math::constants::e<double>()/(double)d))
144  {
145  }
146 
147  std::vector<Milestone>& operator()(const Milestone& m)
148  {
149  KStrategy<Milestone>::k_ = static_cast<unsigned int>(ceil(kPRMConstant_ * log((double)n_())));
150  return static_cast<KStrategy<Milestone>&>(*this)(m);
151  }
152 
153  protected:
154 
156  const NumNeighborsFn n_;
157  const double kPRMConstant_;
158 
159  };
160 
161  }
162 
163 }
164 
165 #endif