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 #include "ompl/base/PrecomputedStateSampler.h"
00038 #include "ompl/base/StateSpace.h"
00039 #include "ompl/util/Exception.h"
00040
00041 ompl::base::PrecomputedStateSampler::PrecomputedStateSampler(const StateSpace *space, const std::vector<const State*> &states) :
00042 StateSampler(space), states_(states)
00043 {
00044 if (states_.empty())
00045 throw Exception("Empty set of states to sample from was specified");
00046 minStateIndex_ = 0;
00047 maxStateIndex_ = states_.size() - 1;
00048 }
00049
00050 ompl::base::PrecomputedStateSampler::PrecomputedStateSampler(const StateSpace *space, const std::vector<const State*> &states, std::size_t minStateIndex, std::size_t maxStateIndex) :
00051 StateSampler(space), states_(states), minStateIndex_(minStateIndex), maxStateIndex_(maxStateIndex)
00052 {
00053 if (states_.empty())
00054 throw Exception("Empty set of states to sample from was specified");
00055 if (minStateIndex > maxStateIndex)
00056 throw Exception("Minimum state index cannot be larger than maximum state index");
00057 if (maxStateIndex >= states_.size())
00058 throw Exception("Index range out of bounds");
00059 }
00060
00061 void ompl::base::PrecomputedStateSampler::sampleUniform(State *state)
00062 {
00063 space_->copyState(state, states_[rng_.uniformInt(minStateIndex_, maxStateIndex_)]);
00064 }
00065
00066 void ompl::base::PrecomputedStateSampler::sampleUniformNear(State *state, const State *near, const double distance)
00067 {
00068 int index = rng_.uniformInt(minStateIndex_, maxStateIndex_);
00069 double dist = space_->distance(near, states_[index]);
00070 if (dist > distance)
00071 space_->interpolate(near, states_[index], distance / dist, state);
00072 else
00073 space_->copyState(state, states_[index]);
00074 }
00075
00076 void ompl::base::PrecomputedStateSampler::sampleGaussian(State *state, const State *mean, const double stdDev)
00077 {
00078 sampleUniformNear(state, mean, rng_.gaussian(0.0, stdDev));
00079 }