C++ CONFIG::vector方法代码示例

int algorithms::Planner<IROBOT>::find_next_owner(int cell, const CONFIG& bnd_cfg ) const
{
    for (int index : get_cell(cell).get_boundaries()) {
        int neighbor = get_boundary(index).otherside(cell);
        if(get_cell(neighbor).on_boundary(bnd_cfg.vector(), 1e-15))
            return neighbor;
    }
    return NOT_FOUND;
}

void algorithms::Planner<IROBOT>::compute_potential(const IROBOT& robot, const CONFIG& goal)
{
    std::clock_t    t0 = std::clock();
    int goal_cell  = NOT_FOUND;
    for (int i = 0; i < cells.size(); ++i) {
        if( goal_cell == NOT_FOUND && cells[i].contains(goal.vector())) {
            goal_cell = i;
            break;
        }
    }
    if (goal_cell == NOT_FOUND)
        std::cout << "Can't find goal cell\n";
    // Build potential field
    cells[goal_cell].set_potential(0.0);
    std::priority_queue<Potential_node> queue;
    
    for (int index : cells[goal_cell].get_boundaries()) {
        int neighbor = all_boundaries[index].otherside(goal_cell);
        cells[neighbor].set_potential(0.0);
        queue.emplace(neighbor, 0.0);
    }
    // Dijkstra's algorithm
    while( !queue.empty() )
    {
        Potential_node node = queue.top();
        queue.pop();
        int current = node.cell;
        if (node.priority > cells[current].get_potential())
            continue;
        Cell<IROBOT>& current_cell = cells[current];
        for (int index : current_cell.get_boundaries()) {
            Boundary<IROBOT>& boundary = all_boundaries[index];
            int neighbor = boundary.otherside(current);
            Cell<IROBOT>& neighbor_cell = cells[neighbor];
            double tentative_potential = current_cell.get_potential() + boundary.get_weight();
            if (tentative_potential < neighbor_cell.get_potential()) {
                neighbor_cell.set_potential(tentative_potential);
                queue.emplace(neighbor, tentative_potential);
            }
        }
    }
    std::clock_t    t1 = std::clock();
    std::cout << "Time cost for building potential field: \n\t" << (t1 - t0) / (double)(CLOCKS_PER_SEC / 1000) << " ms\n";
}

std::vector<typename IROBOT::CONFIG> algorithms::Planner<IROBOT>::AStar(const IROBOT& robot, const CONFIG& start, const CONFIG& goal)
{
    
    double smallest_radius = get_smallest_radius();
    // Find the cells containing the start and the goal
    int start_cell = NOT_FOUND, goal_cell = NOT_FOUND;
    for (int i = 0; i < cells.size(); ++i) {
        if( cells[i].contains(goal.vector()))
            goal_cell = i;
        if (cells[i].contains(start.vector())) {
            start_cell = i;
            std::cout << "Start cell potential : " << cells[i].get_potential() << '\n';
        }
    }
    for (Boundary<IROBOT>& boundary : all_boundaries)
        boundary.reset();
    
    if( start_cell == NOT_FOUND)
        throw "start config is not in any cell!";
    else if (goal_cell == NOT_FOUND )
        throw "goal config is not in any cell!";
    
    // Initialize for the nodes
    nodes.clear();
    nodes.reserve(1000000);
    int start_node = get_new_info(start);
    get_info(start_node).came_from = -1;
    int goal_node = get_new_info(goal);
    get_info(goal_node).cell = goal_cell;
    int metric_query = 0;
    std::cout << "Initialize the queue of AStar\n";
    // Initialize for the queue
    std::priority_queue<AStar_node> openset;
    {
        double g_score_start = 0.0;
        int current_cell_index = start_cell;
        double interval = (std::log(cells[current_cell_index].radius() / smallest_radius) + 1) * smallest_radius;
        for (int boundary_index : cells[current_cell_index].get_boundaries()) {
            int begin, end;
            get_boundary(boundary_index).get_boundary_configs(*this, interval, begin, end);
            for (int vertex = begin; vertex < end; ++vertex) {
                double tentative_g_score = g_score_start + IROBOT::metric(robot, start, nodes[vertex].cfg);
                nodes[vertex].came_from = start_node;
                nodes[vertex].g_score = tentative_g_score;
                nodes[vertex].f_score = tentative_g_score + cells[current_cell_index].get_potential();
                nodes[vertex].heuristic = cells[current_cell_index].get_potential();
                nodes[vertex].cell = start_cell;
                nodes[vertex].set_open();
                openset.emplace(vertex, nodes[vertex].f_score);
                ++metric_query;
            }
            
        }
    }
    std::cout << "Start AStar\n";
    // Starting A*
    while (!openset.empty()) {
        int current_node = openset.top().info_index;
        openset.pop();
        if (nodes[current_node].is_closed())
            continue;
        get_info(current_node).set_closed();
        if (current_node == goal_node) {
            std::cout << "Find Goal by expolring " << nodes.size() << " nodes with " << metric_query << " queries\n";
            return this->trace_back(nodes[current_node], start, goal);
        }
        
        int prev_cell;
        int current_cell;
        // Special case for start's neighbor
        if (nodes[current_node].came_from == start_node) {
            prev_cell = nodes[current_node].cell;
            current_cell = prev_cell;
        } else {
            prev_cell = nodes[nodes[current_node].came_from].cell;
            current_cell = find_next_owner(prev_cell, nodes[nodes[current_node].came_from].cfg); // search the neighbors of prev cell to see which cell contians current_cfg, that is the current_cell. ( there is only one such cell )
            
        }
        get_info(current_node).cell = current_cell;
        
        // time to check if the path has common cells.
        int neighbor_cell_index = find_next_owner(current_cell, nodes[current_node].cfg); // get the cell that also contains current config
        if( neighbor_cell_index == NOT_FOUND ) {
            continue;
        }
        cells[current_cell].set_visited();
        cells[neighbor_cell_index].set_visited();
        // Special case for goal's neighbor
        if (cells[neighbor_cell_index].contains(goal.vector())) {
            if( !nodes[goal_node].is_open() ) {
                nodes[goal_node].set_open();
                nodes[goal_node].g_score = get_info(current_node).g_score + IROBOT::metric(robot, nodes[current_node].cfg, goal);
                nodes[goal_node].came_from = current_node;
                openset.emplace(goal_node, nodes[goal_node].g_score);
                ++metric_query;
            } else {
                double distance_to_goal = IROBOT::metric(robot, nodes[current_node].cfg, goal);
                ++metric_query;
                if (get_info(current_node).g_score + distance_to_goal < nodes[goal_node].g_score) {
                    // 1. remove the neighbor_cfg from openset
//.........这里部分代码省略.........