37 #include "ompl/base/PrecomputedStateSampler.h"
38 #include "ompl/base/StateSpace.h"
39 #include "ompl/util/Exception.h"
45 throw Exception(
"Empty set of states to sample from was specified");
51 StateSampler(space), states_(states), minStateIndex_(minStateIndex), maxStateIndex_(maxStateIndex)
54 throw Exception(
"Empty set of states to sample from was specified");
55 if (minStateIndex > maxStateIndex)
56 throw Exception(
"Minimum state index cannot be larger than maximum state index");
57 if (maxStateIndex >=
states_.size())
58 throw Exception(
"Index range out of bounds");
63 space_->copyState(state, states_[rng_.uniformInt(minStateIndex_, maxStateIndex_)]);
68 int index = rng_.uniformInt(minStateIndex_, maxStateIndex_);
69 double dist = space_->distance(near, states_[index]);
71 space_->interpolate(near, states_[index], distance / dist, state);
73 space_->copyState(state, states_[index]);
78 sampleUniformNear(state, mean, rng_.gaussian(0.0, stdDev));