2.A* Search

A *伪代码

Search( grid, initial_point, goal_point ) :

1.初始化一个空的打开节点列表 2.使用以下内容初始化起始节点:

  • initial_point给出的x和y值
  • g = 0,其中g是每一步的成本
  • h由启发函数(当前坐标和目标的函数)给出
  1. 将新节点添加到打开的节点列表中
  2. while 打开节点的列表是非空的:
  • 按f值对打开的列表进行排序
  • 弹出最佳单元格(称为current单元格)。
  • 在路径中将单元格的坐标标记在网格中。
  • if current单元格是goal单元格:
    • 返回单元格
  • else将搜索范围扩展到current节点的邻居。这包括以下步骤:
    • 检查网格中的每个相邻单元,以确保该单元为空:它尚未关闭且没有障碍。
    • 如果单元格为空,请计算成本(g值)和启发式方法,然后将其添加到打开的节点列表中
    • 将单元格标记为关闭。
  1. 如果由于打开的节点列表为空而退出while循环,则表明您用完了新节点以进行探索,但未找到路径。
#include <algorithm>  // for sort
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>
using std::cout;
using std::ifstream;
using std::istringstream;
using std::sort;
using std::string;
using std::vector;
using std::abs;

// State classes
enum class State { kEmpty, kObstacle, kClosed, kPath, kStart, kFinish };

// Directional deltas
const int delta[4][2]{ {-1, 0}, {0, -1}, {1, 0}, {0, 1} };


vector<State> ParseLine(string line) {
    istringstream sline(line);
    int n;
    char c;
    vector<State> row;
    while (sline >> n >> c && c == ',') {
        if (n == 0) {
            row.push_back(State::kEmpty);
        }
        else {
            row.push_back(State::kObstacle);
        }
    }
    return row;
}


vector<vector<State>> ReadBoardFile(string path) {
    ifstream myfile(path);
    vector<vector<State>> board{};
    if (myfile) {
        string line;
        while (getline(myfile, line)) {
            vector<State> row = ParseLine(line);
            board.push_back(row);
        }
    }
    return board;
}


/**
 * Compare the F values of two cells.
 */
bool Compare(const vector<int> a, const vector<int> b) {
    int f1 = a[2] + a[3]; // f1 = g1 + h1
    int f2 = b[2] + b[3]; // f2 = g2 + h2
    return f1 > f2;
}


/**
 * Sort the two-dimensional vector of ints in descending order.
 */
void CellSort(vector<vector<int>>* v) {
    sort(v->begin(), v->end(), Compare);
}


// Calculate the manhattan distance
int Heuristic(int x1, int y1, int x2, int y2) {
    return abs(x2 - x1) + abs(y2 - y1);
}


/**
 * Check that a cell is valid: on the grid, not an obstacle, and clear.
 */
bool CheckValidCell(int x, int y, vector<vector<State>>& grid) {
    bool on_grid_x = (x >= 0 && x < grid.size());
    bool on_grid_y = (y >= 0 && y < grid[0].size());
    if (on_grid_x && on_grid_y)
        return grid[x][y] == State::kEmpty;
    return false;
}


/**
 * Add a node to the open list and mark it as open.
 */
void AddToOpen(int x, int y, int g, int h, vector<vector<int>>& openlist, vector<vector<State>>& grid) {
    // Add node to open vector, and mark grid cell as closed.
    openlist.push_back(vector<int>{x, y, g, h});
    grid[x][y] = State::kClosed;
}


/**
 * Expand current nodes's neighbors and add them to the open list.
 */
void ExpandNeighbors(const vector<int>& current, int goal[2], vector<vector<int>>& openlist, vector<vector<State>>& grid) {
    // Get current node's data.
    int x = current[0];
    int y = current[1];
    int g = current[2];

    // Loop through current node's potential neighbors.
    for (int i = 0; i < 4; i++) {
        int x2 = x + delta[i][0];
        int y2 = y + delta[i][1];

        // Check that the potential neighbor's x2 and y2 values are on the grid and not closed.
        if (CheckValidCell(x2, y2, grid)) {
            // Increment g value and add neighbor to open list.
            int g2 = g + 1;
            int h2 = Heuristic(x2, y2, goal[0], goal[1]);
            AddToOpen(x2, y2, g2, h2, openlist, grid);
        }
    }
}


/**
 * Implementation of A* search algorithm
 */
vector<vector<State>> Search(vector<vector<State>> grid, int init[2], int goal[2]) {
    // Create the vector of open nodes.
    vector<vector<int>> open{};

    // Initialize the starting node.
    int x = init[0];
    int y = init[1];
    int g = 0;
    int h = Heuristic(x, y, goal[0], goal[1]);
    AddToOpen(x, y, g, h, open, grid);

    while (open.size() > 0) {
        // Get the next node
        CellSort(&open);
        auto current = open.back();
        open.pop_back();
        x = current[0];
        y = current[1];
        grid[x][y] = State::kPath;

        // Check if we're done.
        if (x == goal[0] && y == goal[1]) {
            grid[init[0]][init[1]] = State::kStart;
            grid[goal[0]][goal[1]] = State::kFinish;
            return grid;
        }

        // If we're not done, expand search to current node's neighbors.
        ExpandNeighbors(current, goal, open, grid);
    }

    // We've run out of new nodes to explore and haven't found a path.
    cout << "No path found!" << "\n";
    return std::vector<vector<State>>{};
}


string CellString(State cell) {
    switch (cell) {
    case State::kObstacle: return "山     ";
    case State::kPath: return     "路径   ";
    case State::kStart: return    "起点   ";
    case State::kFinish: return   "终点   ";
    default: return               "0      ";
    }
}


void PrintBoard(const vector<vector<State>> board) {
    for (int i = 0; i < board.size(); i++) {
        for (int j = 0; j < board[i].size(); j++) {
            cout << CellString(board[i][j]);
        }
        cout << "\n";
    }
}

//#include "test.cpp"

int main() {
    int init[2]{ 0, 0 };
    int goal[2]{ 4, 5 };
    auto board = ReadBoardFile("1.board");
    auto solution = Search(board, init, goal);
    PrintBoard(solution);
    system("pause");
    // Tests
   /* TestHeuristic();
    TestAddToOpen();
    TestCompare();
    TestSearch();
    TestCheckValidCell();
    TestExpandNeighbors();*/
}

本文参与腾讯云自媒体分享计划,欢迎正在阅读的你也加入,一起分享。

我来说两句

0 条评论
登录 后参与评论

相关文章

  • 1.Introduction to the C++ Language

    在之前的代码中,每个变量的类型都已明确声明。通常,这不是必需的,编译器可以根据分配的值确定类型。要自动确定类型,请使用auto关键字。

    小飞侠xp
  • 1.C与C++

    使用c++中的标准库类型vector可以很轻松的完成任务。 不需要管理内存分配,对不同的类型都可以处理

    小飞侠xp
  • ROS通信架构(下)

    队长与爱人相互七十年不能共舞,蚁人与女儿分隔五年未能相见,钢铁侠邂逅父亲期盼新生,雷神遇见母亲不忍分别。时间会给爱情设置衰老的考验,给生命带来变化的乐趣,会让未...

    小飞侠xp
  • 《C++ primer》--第11章

    习题11.1 algorithm头文件定义了一个count的函数,其功能类似于find。这个函数使用一对迭代器和一个值做参数,返回这个值出现次数的统计结果。编写...

    猿人谷
  • C++名字命名空间实现判等

    汐楓
  • 让视频里的你完全消失,Adobe最新SOTA模型实现无痕修图,无需先验知识

    Adobe 提出的这种新型视频修图算法可以同时修复缺失图像和移动(光流)信息,基于 Deep Image Prior(DIP)提出。DIP 利用卷积网络架构来修...

    机器之心
  • vector作为参数的三种传参方式

    c++中常用的vector容器作为参数时,有三种传参方式,分别如下(为说明问题,用二维vector):

    xiaoxi666
  • springboot集成ueditor富文本编辑器【需要修改ueditor源码】-和上一篇不一样

    最近工作需要重新搭建公司网站,其中需要使用富文本编辑器,货比三家,最后选择了百度团队的UEditor。项目框架为springboot,所以涉及到springbo...

    凯哥Java
  • kubernetes实现用户自定义扩缩容

    本文章主要参考walkthrough,aggregation和auth。涉及custom metric API的注册认证以及API server aggrega...

    charlieroro
  • NGINX部署HTTPS

    drunkdream

扫码关注云+社区

领取腾讯云代金券

玩转腾讯云 有奖征文活动