寻路算法基础

一、概述

广度优先搜索、Dijkstra和A*是图上的三种典型路径规划算法。它们都可用于图搜索，不同之处在于队列和启发式函数两个参数。

算法的一般性原理如下：

• 将边界初始化为包含起始节点的队列。
• 当边界队列不为空时，从队列中“访问”并删除一个“当前”节点，同时将访问节点的每个邻居节点添加到队列，其成本是到达当前节点的成本加上从当前节点访问邻居的成本再加上邻居节点和目标节点的启发式函数值。其中，启发式函数是对两个节点的路径成本的估计。
• 存储访问路径（通常存储在cameFrom图中），以便后续重建路径。如果邻居节点已经在列表中，同时新路径的成本较低，那么更改其成本。
• 找到目标路径（提前退出）或列表为空时，停止算法。

点此链接进入交互演示页面：https://interactive-pathfinding.netlify.com/

二、BFS

使用先进先出队列实现BFS。这种队列会忽略路径中链接的开销，并根据跳数进行扩展，因此可以确保找到最短路径的跳数，而跳数相关的成本。启发式函数的选择是任意的，因为在这个过程中其并不起作用。

使用数组可实现先进先出，即将元素附加到末尾并从头删除

BFS演示动图。注意边界节点（黄色）是如何在网格中扩展为正方形的。在这里，正方形是相同“跳距”的节点集。

#include <iostream>
#include <vector>
#include <queue>
#include <algorithm> 
using namespace std;
 
struct Node {
    int x;
    int y;
};
class Solution {
public:
    vector<Node> shortestPathBinaryMatrix(vector<vector<int>>& grid) {
        int ans = 0;
        queue<Node> myQ; // BFS一般通过队列方式解决
        int M = grid.size();
        int N = grid[0].size();
        vector<Node> ret;
        // 先判断边界条件，很明显，这两种情况下都是不能到达终点的。
        if (grid[0][0] == 1 || grid[M - 1][N - 1] == 1) {
            return ret;
        }
       
        // 记录已经走过的节点的上一个节点
        vector<vector<Node>> mem(M, vector<Node>(N, {-1,-1}));
        
        myQ.push({0, 0});
        mem[0][0] = {-2,-2};
 
        // 以下是标准BFS的写法
        while (!myQ.empty()) {
            int size = myQ.size();
 
            for (int i = 0; i < size; i++) {
                Node currentNode = myQ.front();
                int x = currentNode.x;
                int y = currentNode.y;
 
                // 判断是否满足退出的条件
                if (x == (N - 1) && y == (M - 1)) {
                    cout<<"start"<<endl;
                    int p=x,q=y;
                    ret.push_back({p,q});
                    while(mem[q][p].x!=-2 && mem[q][p].y!=-2){
                        ret.push_back(mem[q][p]);
                        p=mem[q][p].x;q=mem[q][p].y;
                        cout<<p<<q<<endl;
                    }
                    reverse(ret.begin(),ret.end());
                    return ret;
                }
 
                // 下一个节点所有可能情况
                vector<Node> nextNodes = {{x + 1, y}, {x - 1, y}, {x + 1, y - 1}, {x + 1, y + 1},
                                          {x, y + 1}, {x, y - 1}, {x - 1, y - 1}, {x - 1, y + 1}};
 
                for (auto& n : nextNodes) {
                    // 过滤条件1： 边界检查
                    if (n.x < 0 || n.x >= N || n.y < 0 || n.y >= M) {
                        continue;
                    }
                    // 过滤条件2：备忘录检查
                    if (mem[n.y][n.x].x != -1 && mem[n.y][n.x].y!=-1) {
                        continue;
                    }
                    // 过滤条件3：题目中的要求
                    if (grid[n.y][n.x] == 1) {
                        continue;
                    }
 
                    // 通过过滤筛选，加入队列！
                    mem[n.y][n.x] = currentNode;
                    myQ.push(n);
                }               
 
                myQ.pop();
            }
            ans++;
        }
 
        return ret;        
    }
};
 
int main(int argc,  char* argv[]) {
  vector<vector<int>> input={{0,0},{0,0}};
  Solution *test=new  Solution();
  vector<Node> ret=test->shortestPathBinaryMatrix(input);
  cout<<"road"<<ret.size()<<endl;
  for(auto n: ret){
      cout<<n.x<<n.y<<endl;
  }
    system("pause");
  return 0;
}

三、DFS

var map = [
    [1, 1, 1, 1, 1, 0],
    [0, 0, 0, 1, 1, 1],
    [1, 1, 1, 0, 0, 1],
    [1, 0, 1, 1, 1, 1],
    [1, 1, 0, 0, 0, 0],
    [1, 1, 1, 1, 1, 1]
];
function findRoad1(map) {
    let beginPos = [0,0];
    let endPos = [5,5];
    let directions = [[0,1],[0,-1],[1,0],[-1,0]];
    let visited = new Array(6).fill(false).map(value => new Array(6).fill(false));
    let isInArea = function (x,y) {
        return x >= 0 && x < 6 && y >= 0 && y < 6;
    }
    let path = [];

    let backTacking = function (cur) {
        visited[cur[0]][cur[1]] = true;
        path.push(cur);
        if(cur[0] == endPos[0] && cur[1] == endPos[1]){
            return true;
        }
        for (let dir of directions) {
            let posX = cur[0]+dir[0];
            let posY = cur[1]+dir[1];
            if(isInArea(posX,posY) && map[posX][posY] == 1 && !visited[posX][posY]){
                if(backTacking([posX,posY])){
                    return true;
                }
            }
        }
        path.pop();
        return false;
    }

    if(backTacking(beginPos)){
        return path;
    }

    return [];
    
}

四、Dijkstra

在图上使用优先级队列和始终返回0的启发式函数，便得到Dijkstra算法。
Dijkstra 是解决单源最短路径问题的算法，是贪婪算法的经典例子，是广度优先搜索算法，是一种发散式的搜索，计算源点(起点)到所有节点的最短路径，解决的是有权图中最短路径问题(注意：权值不能为负)。时间复杂度和空间复杂度都比较高。
相比于BFS，Dijkstra最大的不同在于考虑了成本。通过该算法，可以根据节点到节点的成本找到最短路径。

优先级队列使用数组实现，在每次插入新节点后对该数组进行排序。尽管实现优先级队列还有其他更高效的方式，但在我们的场景中，数组是足够快的，而且实现起来也简单。

Dijkstra展示动画，注意此时的边界是一个圆。

Dijkstra算法是一个计算有权图中两个点之间最短路径的算法，基于广度优先搜索，使用一个优先队列保存待处理的节点。因为：在纯粹的Dijkstra算法中，通过一次性将所有的节点都初始化并添加到队列之后，开始循环迭代地移出最小代价节点，更新其后继节点的代价等步骤。每次都是在所有候选节点中找出代价节点，然后移出，是不会添加新的节点到候选队列的。

然而在这种只有0和1的二维网格地图中，也可以使用一种优化的的广度优先搜索算法：首先把起点的坐标放入队列，并设置起点的路径长度为0。然后重复以下操作，直到队列为空：从队列中提取出一个点，然后依次检查与它相邻的所有点，如果相邻的点没有被访问过，或者通过当前点到达相邻点的路径长度小于已知的路径长度，就更新相邻点的路径长度，并把相邻点放入队列。在这个过程中，只要达到目标点，就可以得到从起点到目标点的最短路径长度。

但出于这个例子的简单性，在网格地图中只有0和1，等同于无权图，因此使用Dijkstra算法并不能带来显著的优势，反而会降低代码的简洁性。如果网格地图中有多种不同的权值，那么使用Dijkstra算法就能更好地找到最短路径。

五、A*寻路算法

为实现A算法，需要传递一个实际启发式函数，例如两个节点之间的欧式距离。通过“节点成本”+“节点到目标节点的估算成本”对节点进行加权，通过优先搜索更大可能的节点加快搜索速度。

**借助启发式方法，A可以比Dijkstra或BFS更快地找到正确路径。**

任务：用整型二维数组模拟地图（1代表墙壁，0代表通路），给定起点坐标和终点坐标，用A*寻路算法查找最短路径

#include <iostream>
#include <vector>
 
#define ROWS 12    //地图行数
#define COLS 12    //地图列数
 
#define SC   10    //直线代价
#define OC   14    //斜线代价
 
using namespace std;
 
//地图节点
struct Pos
{
    int Row = 0;            //所处行数
    int Col = 0;            //所处列数
 
    int G = 0;          //当前点到起点的代价
    int H = 0;          //当前点到终点的代价
    int F = 0;          //总代价   
 
    void SetF()
    {
        F = G + H;
    }
};
 
//A*搜索树结构
struct TreeNode
{
    Pos MyPos;                          //位置信息
    TreeNode* pParent = nullptr;        //父结点指针
    TreeNode* pChild0 = nullptr;        //孩子0结点指针
    TreeNode* pChild1 = nullptr;        //孩子1结点指针
    TreeNode* pChild2 = nullptr;        //孩子2结点指针
    TreeNode* pChild3 = nullptr;        //孩子3结点指针
    TreeNode* pChild4 = nullptr;        //孩子4结点指针
    TreeNode* pChild5 = nullptr;        //孩子5结点指针
    TreeNode* pChild6 = nullptr;        //孩子6结点指针
    TreeNode* pChild7 = nullptr;        //孩子7结点指针
};
 
//探索方向
enum Director
{
    D_up, D_down, D_left, D_right, D_lup, D_ldown, D_rup, D_rdown
};
 
//辅助地图结点
struct Mark {
    int  val;            //1表示墙壁，0表示通路
    bool bIsFind;       //是否已被探索，1表示已被探索，0表示未被探索
};
 
//创建新的树结点
TreeNode* CreateTreeNode(Pos PosA);
 
//判断是否可以探索
bool bCanWalk(Mark PathMap[ROWS][COLS], int Row, int Col);
 
//计算两点间的曼哈顿距离
int Distance_Manhatttan(Pos PosA, Pos PosB);
 
 
int main()
{
    
    vector<TreeNode*>::iterator it;
    vector<TreeNode*>::iterator itMin;
  
    //初始化地图，12*12，1代表墙壁，0代表通路
    int Map[ROWS][COLS] =
    {
 
        {0,0,0,0,0,1,0,0,0,0,0,0},
        {0,0,0,1,0,1,0,0,0,0,0,0},
        {0,0,0,1,0,1,0,0,0,0,0,0},
        {1,0,0,1,0,1,0,0,0,0,0,0},
        {1,1,1,1,0,1,0,0,0,0,0,0},
        {0,0,0,0,0,1,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,1,0,0,0,0,0,0},
        {0,0,0,0,0,1,0,0,0,0,0,0},
        {0,0,0,0,0,1,0,0,0,0,0,0},
        {0,0,0,0,0,1,0,0,0,0,0,0},
        {0,0,0,0,0,1,0,0,0,0,0,0},
    };
 
    //初始化辅助地图
    Mark PathMap[ROWS][COLS] = { 0 };
    for (int i = 0; i < ROWS; i++)
    {
        for (int j = 0; j < COLS; j++)
        {
            PathMap[i][j].val = Map[i][j];
        }
    }
 
    //初始化起点坐标和终点坐标
    Pos SrcPos = { 1,1 };
    Pos DstPos = { 2,11 };
 
    //起点作为树的根结点
    TreeNode* pRoot = CreateTreeNode(SrcPos);
    pRoot->MyPos.G = Distance_Manhatttan(DstPos, SrcPos);
    pRoot->MyPos.SetF();
 
    //临时容器，用于存储当前待探索的树结点
    vector<TreeNode*> buff;
    //布尔变量判断是否已寻路完成
    bool bFindPath = false;
    //根结点作为探索的起点
    TreeNode* pTempNode = pRoot;
 
    while (1)
    {
        //对当前点的八个方向进行探索
        for (int i = 0; i < 8; i++)
        {
            Pos TempPos = pTempNode->MyPos;
 
            //如果当前结点坐标为终点坐标，寻路完成，退出循环
            if (TempPos.Col == DstPos.Col && TempPos.Row == DstPos.Row)
            {
                bFindPath = true;
                break;
            }
 
            //根据寻路方向判断G值的增量，直线代价为SC，斜线代价为OC
            switch (i)
            {
            case D_up:       TempPos.Row--;  TempPos.G += SC;                     break;
            case D_down:     TempPos.Row++;  TempPos.G += SC;                     break;
            case D_left:     TempPos.Col--;  TempPos.G += SC;                     break;
            case D_right:    TempPos.Col++;  TempPos.G += SC;                     break;
            case D_lup:      TempPos.Row--;  TempPos.Col--;  TempPos.G += OC;     break;
            case D_ldown:    TempPos.Row++;  TempPos.Col--;  TempPos.G += OC;     break;
            case D_rup:      TempPos.Row--;  TempPos.Col++;  TempPos.G += OC;     break;
            case D_rdown:    TempPos.Row++;  TempPos.Col++;  TempPos.G += OC;     break;
            default:
                break;
            }
 
            //判断TempPos是否符合探索条件
            if (bCanWalk(PathMap, TempPos.Row, TempPos.Col))
            {
                //辅助地图标记相关位置为已探索，并计算相应的H,F值
                PathMap[TempPos.Row][TempPos.Col].bIsFind = true;
                TempPos.H = Distance_Manhatttan(TempPos, DstPos);
                TempPos.SetF();
 
                //创建新结点入树
                TreeNode* pNew = CreateTreeNode(TempPos);
                switch (i)
                {
                case 0:     pTempNode->pChild0 = pNew;   break;
                case 1:     pTempNode->pChild1 = pNew;   break;
                case 2:     pTempNode->pChild2 = pNew;   break;
                case 3:     pTempNode->pChild3 = pNew;   break;
                case 4:     pTempNode->pChild4 = pNew;   break;
                case 5:     pTempNode->pChild5 = pNew;   break;
                case 6:     pTempNode->pChild6 = pNew;   break;
                case 7:     pTempNode->pChild7 = pNew;   break;
                default:
                    break;
                }
                pNew->pParent = pTempNode;
 
                //将新结点添加到待探索的节点容器中
                buff.push_back(pNew);
            } 
        }
 
        //寻路完成，退出循环
        if (bFindPath || buff.size() == 0) break;
 
        //选取F值最小的结点作为下一个pTempNode
        itMin = buff.begin();
        for (it = buff.begin(); it != buff.end(); it++)
        {
            itMin = (*it)->MyPos.F < (*itMin)->MyPos.F ? it : itMin;
        }
        pTempNode = *itMin;
        buff.erase(itMin);
    }
 
    //寻路完成，成功则打印路径，失败则返回错误信息
    if (bFindPath)
    {
        cout << "已找到最佳路径如下：" << endl;
        while (pTempNode)
        {
            printf_s("(%d,%d)\n", pTempNode->MyPos.Row, pTempNode->MyPos.Col);
            pTempNode = pTempNode->pParent;
        }
    }
    else
    {
        cout << "ERROR: 未找到路径！" << endl;
    }
 
    return 0;
}
 
//判断是否可以探索
bool bCanWalk(Mark PathMap[ROWS][COLS], int Row, int Col)
{
    if (Row >= ROWS || Row<0 || Col>COLS || Col < 0) return false;
   
    if (PathMap[Row][Col].bIsFind) return false;
    if (PathMap[Row][Col].val) return false;
 
    return true;
}
 
//计算两点间的曼哈顿距离
int Distance_Manhatttan(Pos PosA,Pos PosB)
{
    return (PosA.Row > PosB.Row ? (PosA.Row - PosB.Row) : (PosB.Row - PosA.Row))
        + (PosA.Col > PosB.Col ? (PosA.Col - PosB.Col) : (PosB.Col - PosA.Col));
}
 
 
//创建新的树结点
TreeNode* CreateTreeNode(Pos PosA)
{
    TreeNode* pNew = new TreeNode;
    memset(pNew, 0, sizeof(TreeNode));
    pNew->MyPos = PosA;
    return pNew;
}

非允许的启发式函数：
只有应用可允许启发式函数，A*才能找到最短路径，这也意味着算法永远不会高估实际路径长度。由于欧氏距离是两点之间的最短距离/路径，因此欧氏距离绝不会超出。

但如果将其乘以常数k>0会怎样呢？这样会高估距离，成为非允许的启发式函数。

//A*寻路寻路
var map = [
    [1, 1, 1, 1, 1, 0],
    [0, 0, 0, 1, 1, 1],
    [1, 1, 1, 0, 0, 1],
    [1, 0, 1, 1, 1, 1],
    [1, 1, 0, 0, 0, 0],
    [1, 1, 1, 1, 1, 1]
];

function Node(x,y,parent=null){
    this.x = x;
    this.y = y;
    this.parent = parent;
}
//用于存放已探索过的节点
var closeList = [];
//用于存放需要探索的节点，开始时将起点加入到开放列表中
var openList = [];
//计算H值，用来得到某个点到终点的距离
function  H(point) {
    return Math.abs(5-point.x)+Math.abs(5-point.y);
}

//计算G值，用来得到某个点与起点的距离
function G(point){
    return point.x+point.y;
}

//计算F值
function F(point) {
    return H(point) + G(point);
}

//寻找开放列表中F值最小的点
function getMinNode() {
    let temp = openList[0];
     for(var i=1;i<openList.length;i++){
        if(F(openList[i]) < F(temp)){
            temp=openList[i];
        }
    }
    return temp;
}

function isInArea(point) {
    return point.x >= 0 && point.x < 6 &&  point.y >= 0 &&  point.y < 6;
}

function isInList(point,list) {
    for(var i = 0; i < list.length; i++) {
        if(point.x === list[i].x && point.y === list[i].y) {
            return i;
        }
    }
    return -1;

}

function  findRoad2(map) {
    let path = [];
    let starNode = new Node(0,0);
    let endNode = new Node(5,5);
    openList.push(starNode);
    
    while (openList.length > 0) {
        var cur = getMinNode();
        if(cur.x == endNode.x && cur.y == endNode.y){
            while(cur.parent){
                path.push(cur);
                cur = cur.parent;
            }
            return path.reverse();
        }else{
             closeList.push(cur);
            let index = isInList(cur,openList);
            if(index >= 0){
                openList.splice(index,1)
            }
        }
       
        //找出他相邻的点
        let surround = {
            right: new Node(cur.x+1,cur.y,cur),
            left: new Node(cur.x-1,cur.y,cur),
            up: new Node(cur.x,cur.y-1,cur),
            down: new Node(cur.x,cur.y+1,cur),
        }
        //检查每一个相邻的点
       for(let point in surround){
            // 如果此节点可以通过，或者在关闭列表中则跳过
            if(isInArea(surround[point])){
                if(map[surround[point].x][surround[point].y] == 0 || isInList(surround[point],closeList) >= 0){
                    continue;
                }
                // 如果不在开启列表中，就把此节点加入到开启列表
                if(isInList(surround[point],openList) < 0){
                    openList.push(surround[point]);
                }
            }
          
        }
    }
    return null;
}