Beispiel #1
0
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;
}
Beispiel #2
0
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]);
}
Beispiel #3
0
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;
}