static double avgIntsN(int s, int l, const int N)
{
RNG r;
double sum = 0.0;
for (int i = 0 ; i < N ; ++i)
sum += r.uniformInt(s, l);
return sum / (double)N;
}
void randomizedAllocator(const base::SpaceInformation *si)
{
RNG r;
const unsigned int n = 500;
std::vector<base::State*> states(n + 1, NULL);
for (unsigned int i = 0 ; i < n * 1000 ; ++i)
{
int j = r.uniformInt(0, n);
if (states[j] == NULL)
states[j] = si->allocState();
else
{
si->freeState(states[j]);
states[j] = NULL;
}
}
for (unsigned int i = 0 ; i < states.size() ; ++i)
if (states[i])
si->freeState(states[i]);
}
bool ompl::geometric::pSBL::checkSolution(RNG &rng, bool start, TreeData &tree, TreeData &otherTree, Motion *motion, std::vector<Motion*> &solution)
{
Grid<MotionInfo>::Coord coord;
projectionEvaluator_->computeCoordinates(motion->state, coord);
otherTree.lock.lock();
Grid<MotionInfo>::Cell* cell = otherTree.grid.getCell(coord);
if (cell && !cell->data.empty())
{
Motion *connectOther = cell->data[rng.uniformInt(0, cell->data.size() - 1)];
otherTree.lock.unlock();
if (pdef_->getGoal()->isStartGoalPairValid(start ? motion->root : connectOther->root, start ? connectOther->root : motion->root))
{
Motion *connect = new Motion(si_);
si_->copyState(connect->state, connectOther->state);
connect->parent = motion;
connect->root = motion->root;
motion->lock.lock();
motion->children.push_back(connect);
motion->lock.unlock();
addMotion(tree, connect);
if (isPathValid(tree, connect) && isPathValid(otherTree, connectOther))
{
if (start)
connectionPoint_ = std::make_pair(motion->state, connectOther->state);
else
connectionPoint_ = std::make_pair(connectOther->state, motion->state);
/* extract the motions and put them in solution vector */
std::vector<Motion*> mpath1;
while (motion != nullptr)
{
mpath1.push_back(motion);
motion = motion->parent;
}
std::vector<Motion*> mpath2;
while (connectOther != nullptr)
{
mpath2.push_back(connectOther);
connectOther = connectOther->parent;
}
if (!start)
mpath1.swap(mpath2);
for (int i = mpath1.size() - 1 ; i >= 0 ; --i)
solution.push_back(mpath1[i]);
solution.insert(solution.end(), mpath2.begin(), mpath2.end());
return true;
}
}
}
else
otherTree.lock.unlock();
return false;
}