37 #include "ompl/base/DiscreteMotionValidator.h"
38 #include "ompl/util/Exception.h"
41 void ompl::base::DiscreteMotionValidator::defaultSettings()
45 throw Exception(
"No state space for motion validator");
53 int nd = stateSpace_->validSegmentCount(s1, s2);
58 State *test = si_->allocState();
60 for (
int j = 1 ; j < nd ; ++j)
62 stateSpace_->interpolate(s1, s2, (
double)j / (
double)nd, test);
63 if (!si_->isValid(test))
65 lastValid.second = (double)(j - 1) / (double)nd;
67 stateSpace_->interpolate(s1, s2, lastValid.second, lastValid.first);
76 if (!si_->isValid(s2))
78 lastValid.second = (double)(nd - 1) / (double)nd;
80 stateSpace_->interpolate(s1, s2, lastValid.second, lastValid.first);
95 if (!si_->isValid(s2))
102 int nd = stateSpace_->validSegmentCount(s1, s2);
105 std::queue< std::pair<int, int> > pos;
108 pos.push(std::make_pair(1, nd - 1));
111 State *test = si_->allocState();
116 std::pair<int, int> x = pos.front();
118 int mid = (x.first + x.second) / 2;
119 stateSpace_->interpolate(s1, s2, (
double)mid / (
double)nd, test);
121 if (!si_->isValid(test))
130 pos.push(std::make_pair(x.first, mid - 1));
132 pos.push(std::make_pair(mid + 1, x.second));
135 si_->freeState(test);
virtual bool checkMotion(const State *s1, const State *s2) const
Check if the path between two states (from s1 to s2) is valid. This function assumes s1 is valid...
SpaceInformation * si_
The instance of space information this state validity checker operates on.
Definition of an abstract state.