Dijstra算法原理及C++实现

参考博客：
（1）数据结构与算法 —— 最短路径Dijkstra算法（迪杰斯特拉）详细图解以及python实现
（2）基于图搜索的自动驾驶规划算法 - BFS，Dijstra，A*
（3）【算法学习笔记】 图（四）用优先级队列优化Dijkstra算法求最短路径（邻接矩阵存储）

1 图论基础

图有三种：无向图、有向图、带权重的图
无向图
有向图

带权重的图

Dijkstra算法：从起点开始逐步扩展，每一步为一个节点找到最短路径。

这是一个由多个节点，多个连接的边组成的有向图，每条边有一个权重，代表这条边的长度。现在，我们想从节点1到6，最短的路应该怎么走，最短的路径长度是多少？

主要步骤：

1. 每次从未标记的节点中选择距离出发点最近的节点，标记，收录到最优路径集合中。
2. 计算刚加入节点A的临近节点B的距离（不包含标记的点），若（节点A的距离+节点A到节点B的距离）<节点B的距离，就更新节点B的距离和前面点。

开始
	将起点放入open_list中
	While True
		if open_list为空
			搜索失败，结束
		取open_list中g(n)最小的节点
		将节点加入closed_list中
		if 节点为终点
			找到路径，结束
		遍历当前节点未在closed_list中的临接节点
		if 节点在open_list中
			更新节点g(n)值
		else
			计算节点g(n)值，加入open_list
结束

open_list:节点到起点有路径的。
closed_list:收录以后的那些节点，找到到起点最短路径的那些节点。

	出发点1	前进点
1	$\infty$
2	$\infty$
3	$\infty$
4	$\infty$
5	$\infty$
6	$\infty$
7	$\infty$

选择距离出发点最近的节点1，标记，收录到最优路径集合中。

	出发点1	前进点
1 √	0
2	$\infty$
3	$\infty$
4	$\infty$
5	$\infty$
6	$\infty$
7	$\infty$

接着更新1临近节点4和2的距离，分别是1和2，接着在未标记的节点中寻找距离出发点最小的节点4，收录到最优路径节点中。

	出发点1	前进点
1 √	0
2	2	1
3	$\infty$
4 √	1	1
5	$\infty$
6	$\infty$
7	$\infty$

更新4临近节点3，6，7,接着在未标记的节点中寻找距离出发点最小的节点2，收录到最优路径节点中。

	出发点1	前进点
1 √	0
2 √	2	1
3	3 (3<$\infty$)	4
4 √	1	1
5	$\infty$
6	9 (9<$\infty$)	4
7	5 (5<$\infty$)	4

更新2临近节点5，接着在未标记的节点中寻找距离出发点最小的节点3，收录到最优路径节点中。

	出发点1	前进点
1 √	0
2 √	2	1
3 √	3 (3<$\infty$)	4
4 √	1	1
5	13 (13<$\infty$)	2
6	9 (9<$\infty$)	4
7	5 (5<$\infty$)	4

更新3临近节点6，接着在未标记的节点中寻找距离出发点最小的节点7，收录到最优路径节点中。

	出发点1	前进点
1 √	0
2 √	2	1
3 √	3 (3<$\infty$)	4
4 √	1	1
5	13 (13<$\infty$)	2
6	8(8<9 (9<$\infty$))	3
7 √	5 (5<$\infty$)	4

更新7临近点6，接着在未标记的节点中寻找距离出发点最小的节点6，收录到最优路径节点中。

	出发点1	前进点
1 √	0
2 √	2	1
3 √	3 (3<$\infty$)	4
4 √	1	1
5	13 (13<$\infty$)	2
6 √	6(6<8(8<9 (9<$\infty$)))	7
7 √	5 (5<$\infty$)	4

节点6临近点均已收录，无需更新，接着在未标记的节点中寻找距离出发点最小的节点5，收录到最优路径节点中。

	出发点1	前进点
1 √	0
2 √	2	1
3 √	3 (3<$\infty$)	4
4 √	1	1
5 √	13 (13<$\infty$)	2
6 √	6(6<8(8<9 (9<$\infty$)))	7
7 √	5 (5<$\infty$)	4

回溯：
6的前面点是7，7的前面点是4，4的前面点是1，所以最短路径：1->4->7->6，长度是6。

2 Dijstra算法流程

Dijkstra算法基本原理：

Dijkstra算法是根据贪心算法实现的，首先找出当前点到所有能到达的点之间最短的距离，然后松弛一次继续循环。所谓松弛一次，就是在已经访问过的点中遍历一遍，看看有没有更近的，如果有更近的就更新距离。这样每次找最近的可达点+松弛遍历历史节点的操作，一直重复就能找到最短路径。

核心思想：
（1）相比BFS，Dijstra维护一个新变量g(n)，g(n)表示从起始节点到当前节点的累积成本
（2）从openset(Min-priority queue)中访问累积成本g最低的节点
伪代码：

Dijstra(G, start, goal):
	let open_list be priority_queue;
	open_list.push(start, 0);
	g[start] = 0;
	while (!open_list.empty())
	{
		current = open_list.pop();
		mark current as visited;
		if (current is the goal) return current;
		for (all unvisited neightbours next of current in G)
		{
			next_cost = g[current] + cost(current, next);
			if (next is not in open_list)
				open_list.push(next, next_cost);
			else {
				if (g[next] > next_cost)
					g[next] = next_cost;
			}
		}
	}

Dijstra算法满足最优性，但是缺少对终点的启发性，对于求两点之间的最短距离，效率是低效的。

3 手撕Dijstra算法

#include <iostream>
#include <cmath>
#include <vector>
#include <algorithm>
#include <string>

using namespace std;

struct Point {
    int x, y;
    Point(int x, int y) : x(x), y(y) {};
    double distance(Point p) {
        return sqrt((x - p.x) * (x - p.x) + (y - p.y) * (y - p.y));
    }
};

// 定义节点
struct Node {
    Point point; // 点的位置
    double g; // 到起点的距离
    Node* parent;
    Node(Point point, double g, Node* parent = NULL) : point(point), g(g), parent(parent) {};
};

// 比较函数
struct NodeCompare {
    bool operator()(Node* n1, Node* n2) {
        return n1->g < n2->g; // 升序排列，保证第一个最小
    }
};

// Dijstra路径规划主算法
vector<Point> Dijstra(vector<vector<int>>& gridmap, Point& start, Point& goal) {
    int row = gridmap.size();
    int col = gridmap[0].size();
    // 定义openlist和closelist
    vector<Node*> openlist; // 存储待搜索的节点
    vector<Node*> closelist;// 存储已经搜索的节点
    openlist.push_back(new Node(start, start.distance(start))); // 将起点放入openlist
    // 寻找路径
    vector<Point> path;
    while (!openlist.empty()) {
        // 从openlist中取出g最小的节点作为当前搜索节点
        sort(openlist.begin(), openlist.end(), NodeCompare{});
        Node* current = *openlist.begin();
        openlist.erase(openlist.begin());// current取出之后从openlist中删除
        closelist.push_back(current);// current放入closelist
        // 1 找到终点 回溯路径
        if (current->point.x == goal.x && current->point.y == goal.y) {
            while (current != NULL) {
                path.push_back(current->point);
                current = current->parent;
            }
            reverse(path.begin(), path.end());
            return path;
        } else {// 2 不是终点，对current的邻近节点进行扩展讨论
            int x = current->point.x;
            int y = current->point.y;
            vector<Point> neighbors = {
                    {x-1,y-1},{x-1,y},{x-1,y+1},
                    {x,y-1},                {x,y+1},
                    {x+1,y-1},{x+1,y},{x+1,y+1}
            };
            // 对邻近节点遍历
            for (auto n : neighbors) {
                // 节点n在地图范围内，同时不是障碍物
                if (n.x >= 0 && n.x < row && n.y >= 0 && n.y < col && gridmap[n.x][n.y] == 0) {
                    // 节点在closelist中直接跳过
                    bool incloselist = false;
                    for (auto c : closelist) {
                        if (c->point.x == n.x && c->point.y == n.y) {
                            incloselist = true;
                            break;
                        }
                    }
                    if (incloselist) continue;
                    // 节点是否在openlist中
                    bool inopenlist = false;
                    for (auto o : openlist) {
                        if (o->point.x == n.x && o->point.y == n.y) {
                            inopenlist = true;
                            // 节点在openlist中需要更新g和parent
                            double g = current->g + n.distance(current->point);
                            if (g < o->g) { // 距离更小，替换父节点
                                o->g = g;
                                o->parent = current;
                            }
                            break;
                        }
                    }
                    if (!inopenlist) { // n不在openlist中需要将n加入到openlist中
                        double g = current->g + n.distance(current->point);
                        openlist.push_back(new Node(n, g, current));
                    }
                }
            }
        }



    }
    // 如果在寻路阶段未能搜索到路径，直接返回空路径结果
    return path;
}


// 基于栅格地图的Dijstra路径规划算法 C++实现
int main() {
    vector<vector<int>> gridmap = { // 栅格地图
            {0,1,0,1,0},
            {1,0,1,0,1},
            {0,1,1,1,0},
            {0,0,1,0,0},
            {0,1,0,1,0}
    };
    Point start(0, 0);
    Point goal(4, 4);
    vector<Point> path = Dijstra(gridmap, start, goal);
    cout << path.size() << endl;
    for (auto p : path) {
        if (p.x == goal.x && p.y == goal.y) {
            cout << "(" << p.x << "," << p.y << ")" << endl;
        } else {
            cout << "(" << p.x << "," << p.y << ")" << "->";
        }
    }
    return 0;
}