37 #include "ompl/control/SimpleDirectedControlSampler.h"
38 #include "ompl/control/SpaceInformation.h"
45 ompl::control::SimpleDirectedControlSampler::~SimpleDirectedControlSampler(
void)
51 return getBestControl(control, source, target, NULL);
56 return getBestControl(control, source, target, previous);
63 cs_->sampleNext(control, previous, source);
65 cs_->sample(control, source);
67 const double minDuration = si_->getMinControlDuration();
68 const double maxDuration = si_->getMaxControlDuration();
70 unsigned int steps = cs_->sampleStepCount(minDuration, maxDuration);
72 if (numControlSamples_ > 1)
74 Control *tempControl = si_->allocControl();
78 si_->propagateWhileValid(source, control, steps, tempState);
79 double bestDistance = si_->distance(tempState, target);
82 for (
unsigned int i = 1; i < numControlSamples_; ++i)
84 unsigned int sampleSteps = cs_->sampleStepCount(minDuration, maxDuration);
86 cs_->sampleNext(tempControl, previous, source);
88 cs_->sample(tempControl, source);
90 si_->propagateWhileValid(source, tempControl, sampleSteps, tempState);
91 double tempDistance = si_->distance(tempState, target);
92 if (tempDistance < bestDistance)
94 si_->copyControl(control, tempControl);
95 bestDistance = tempDistance;
100 si_->freeControl(tempControl);
101 si_->freeState(tempState);