00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef OMPL_GEOMETRIC_PLANNERS_PRM_CONNECTION_STRATEGY_
00038 #define OMPL_GEOMETRIC_PLANNERS_PRM_CONNECTION_STRATEGY_
00039
00040 #include "ompl/datastructures/NearestNeighbors.h"
00041 #include <boost/function.hpp>
00042 #include <boost/shared_ptr.hpp>
00043 #include <boost/math/constants/constants.hpp>
00044 #include <vector>
00045
00046 namespace ompl
00047 {
00048
00049 namespace geometric
00050 {
00051
00055 template <class Milestone>
00056 class KStrategy
00057 {
00058 public:
00059
00062 KStrategy(const unsigned int k,
00063 const boost::shared_ptr< NearestNeighbors<Milestone> > &nn) :
00064 k_(k), nn_(nn)
00065 {
00066 neighbors_.reserve(k_);
00067 }
00068
00069 virtual ~KStrategy(void)
00070 {
00071 }
00072
00074 void setNearestNeighbors(const boost::shared_ptr< NearestNeighbors<Milestone> > &nn)
00075 {
00076 nn_ = nn;
00077 }
00078
00082 std::vector<Milestone>& operator()(const Milestone& m)
00083 {
00084 nn_->nearestK(m, k_, neighbors_);
00085 return neighbors_;
00086 }
00087
00088 protected:
00089
00091 unsigned int k_;
00092
00094 boost::shared_ptr< NearestNeighbors<Milestone> > nn_;
00095
00097 std::vector<Milestone> neighbors_;
00098 };
00099
00125 template <class Milestone>
00126 class KStarStrategy : public KStrategy<Milestone>
00127 {
00128 public:
00129
00139 KStarStrategy(const boost::function0<unsigned int>& n,
00140 const boost::shared_ptr< NearestNeighbors<Milestone> > &nn,
00141 const unsigned int d = 1) :
00142 KStrategy<Milestone>(n(), nn), n_(n), kPRMConstant_(1.0 + 1.0/(double)d)
00143 {
00144 }
00145
00146 std::vector<Milestone>& operator()(const Milestone& m)
00147 {
00148 KStrategy<Milestone>::k_ = static_cast<unsigned int>(
00149 ceil(kPRMConstant_ * boost::math::constants::euler<double>() * log((double)n_())));
00150 return static_cast<KStrategy<Milestone>&>(*this)(m);
00151 }
00152
00153 protected:
00154
00156 const boost::function0<unsigned int> n_;
00157 const double kPRMConstant_;
00158
00159 };
00160
00161 }
00162
00163 }
00164
00165 #endif