【一】图算法之Dijkstra算法及C++实现

0. 前言

由于之前一个比赛用到了路径规划，于是就学习了一些图论中路径规划的算法，所以将自己的学习后的代码记录下来以作备忘。但也是只学了图搜索这类的方法，基于采样的RRT等其他方法个人认为不适合我的应用场景，所以也没有了解。

1. Dijkstra算法

单源最短路径问题：给定一个图$G=(V，E)$，求源结点$s\in V$到图中每个结点$v\in V$的最短路径。
Dijkstra算法就适用于解决带权重的有向图（或无向图）上的单源最短路径问题，同时算法要求图中所有边的权重非负。一般在求解最短路径的时候都是已知一个起点和一个终点，所以使用Dijkstra算法求解过后也就得到了所需起点到终点的最短路径（这样也会把一些不需要求解的结点也进行求解，时间就会慢一些，和A*比起来）。

1.1 算法的伪代码如下（来自《算法导论》）：

其中$G$为带权有向图，$s$为源节点（起点），$S$为已求出结点最短路径的结点集合，即源节点$s$到该集合中每个结点之间的最短路径已被找到，$Q$为等待被确定最短路径的结点集合。

1.2 算法思路：

针对一个带权有向图$G$，将所有结点分为两组$S$和$Q$，$S$是已经确定最短路径的结点集合，在初始时为空（初始时就可以将源节点$s$放入，毕竟源节点到自己的代价是0），$Q$为其余未确定最短路径的结点集合，每次从$Q$中找出一个起点到该结点代价最小的结点$u$，将$u$从$Q$中移出，并放入$S$中，对$u$的每一个相邻结点$v$进行松弛操作。松弛即对每一个相邻结点$v$，判断源节点$s$到结点$u$的代价与$u$到$v$的代价之和是否比原来$s$到$v$的代价更小，若代价比原来小则要将$s$到$v$的代价更新为$s$到$u$与$u$到$v$的代价之和，否则维持原样。如此一直循环直至集合$Q$为空，即所有节点都已经查找过一遍并确定了最短路径，至于一些起点到达不了的结点在算法循环后其代价仍为初始设定的值，不发生变化。

1.3 示例（来自《算法导论》）：

图1

如图1，求解$s$到其他各结点的最短代价，结点内数值为起点$s$到该结点的代价，蓝色结点为集合$S$的结点，即该结点已确定最短路径。初始将$s$加入集合$S$，结点$t$和$y$与起点相连，所以对应的代价为10和5，而结点$x$和$z$不与$s$直接连接，其代价为$\infty$。
此时各结点最短路径为（括号内为代价）：

结点	最短路径
$s$	$s$→$s$（0）
$y$	$s$→$y$（5）
$z$	$s$→$z$（$\infty$）
$t$	$s$→$t$（10）
$x$	$s$→$x$（$\infty$）

从剩余结点中选择值最小的结点即结点$y$，并更新与$y$相邻的结点代价，与$y$相连的结点为$t$、$x$、$z$。$s$到$y$与$y$到$t$的代价之和为8，比原来$s$到$t$的代价10要小，所以更新为8；$s$到$y$与$y$到$x$的代价之和为14，比原来$s$到$x$的代价$\infty$小，所以更新为14；$s$到$y$与$y$到$z$的代价之和为7，比原来的$s$到$z$的代价$\infty$小，更新为7，结果如图2。

结点	最短路径
$s$	$s$→$s$（0）
$y$	$s$→$y$（5）
$z$	$s$→$y$→$z$（7）
$t$	$s$→$y$→$t$（8）
$x$	$s$→$y$→$x$（14）

图2

从剩余结点中选择值最小的结点即结点$z$，并更新与$z$相邻的结点代价，与$z$相连结点分别为$s$、$x$。$s$是起点已经确定最短路径，所以就只有$x$了，$s$到$z$与$z$到$x$的代价之和为13，比原来$s$到$x$的代价14要小，所以更新为13，结果如图3。

结点	最短路径
$s$	$s$→$s$（0）
$y$	$s$→$y$（5）
$z$	$s$→$y$→$z$（7）
$t$	$s$→$y$→$t$（8）
$x$	$s$→$y$→$z$→$x$（13）

图3

从剩余结点中选择值最小的结点即结点$t$，并更新与$t$相邻的结点代价，与$t$相邻的结点为$y$、$x$。而$y$已经确定最短路径，所以就只有$x$了，$s$到$t$与$t$到$x$的代价之和为9，比原来$s$到$x$的代价13要小，所以更新为9，结果如图4。

结点	最短路径
$s$	$s$→$s$（0）
$y$	$s$→$y$（5）
$z$	$s$→$y$→$z$（7）
$t$	$s$→$y$→$t$（8）
$x$	$s$→$y$→$t$→$x$（9）

图4

此时还为确定最短路径的只有结点$x$了，然后更新与$x$相邻的结点代价，与$x$相邻的结点为$z$，但该结点已经确定最短路径，所以所有结点都已经确定最短路径，算法结束。

结点	最短路径
$s$	$s$→$s$（0）
$y$	$s$→$y$（5）
$z$	$s$→$y$→$z$（7）
$t$	$s$→$y$→$t$（8）
$x$	$s$→$y$→$t$→$x$（9）

图5

2. C++代码实现

2.1 图的构建

整个图的信息是保存在xml文件中的，整个文件分为两类：结点元素和边元素。结点元素用来保存图中各结点的一些信息，具体结构如下：

    <node>
        <nodeType>0</nodeType>
        <post>0,0</post>
        <!--RGB-->
        <nodeColor>0.0,1.0,0.0</nodeColor>
    </node>
    <node>
        <nodeType>1</nodeType>
        <post>1,2</post>
        <!--RGB-->
        <nodeColor>1.0,1.0,0.0</nodeColor>
    </node>

每一对<node> </node>中都是一个结点元素的相关信息，其中<nodeType> </nodeType>内表示结点的编号，从0开始。<post> </post>内表示该结点的位置即坐标，<nodeColor> </nodeColor>内为该结点的颜色，顺序为RGB，大小在0-1之间，颜色和结点坐标是便于显示。
边的结点元素用来保存各边的信息，具体结构如下：

    <!--0点连接的所有边-->
    <edge1>
        <edgeType>road</edgeType>
        <start2end>0,1</start2end>
        <weight>1</weight>
        <!--RGB-->
        <edgeColor>0.0,0.0,0.0</edgeColor>
        <probability>0.07</probability>
    </edge1>
    <edge1>
        <edgeType>river</edgeType>
        <start2end>0,2</start2end>
        <weight>2.5</weight>
        <!--RGB-->
        <edgeColor>0.0,0.0,1.0</edgeColor>
        <probability>0.01</probability>
    </edge1>
    <edge1>
        <edgeType>river</edgeType>
        <start2end>0,5</start2end>
        <weight>2.5</weight>
        <!--RGB-->
        <edgeColor>0.0,0.0,1.0</edgeColor>
        <probability>0.04</probability>
    </edge1>
    <!--1点连接的所有边-->
    <edge1>
        <edgeType>road</edgeType>
        <start2end>1,2</start2end>
        <weight>1</weight>
        <!--RGB-->
        <edgeColor>0.0,0.0,0.0</edgeColor>
        <probability>0.03</probability>
    </edge1>
    <edge1>
        <edgeType>culvert</edgeType>
        <start2end>1,3</start2end>
        <weight>2</weight>
        <!--RGB-->
        <edgeColor>0.82,0.71,0.55</edgeColor>
        <probability>0.06</probability>
    </edge1>
    <edge1>
        <edgeType>bridge</edgeType>
        <start2end>1,4</start2end>
        <weight>1.5</weight>
        <!--RGB-->
        <edgeColor>0.3,0.5,0.3</edgeColor>
        <probability>0.09</probability>
    </edge1>

其中每个<edge1> </edge1>内都是一条边，<edgeType> </edgeType>表示的是边的属性，包括road、river、bridge、culvert四类；<start2end> </start2end>表示边的起点和终点，第一个为起点，第二个为终点；<weight> </weight>表示边的权重；<edgeColor> </edgeColor>表示边的颜色RGB，同结点颜色；<probability> </probability>为边损坏概率，每条边有一定的概率无法通行，这个属性在Dijkstra算法和A*算法中用不上，所以无需介意。
程序所用的xml文件为包含50个节点的图，效果如图：

2.2 图的读取

由于信息是保存在xml文件中的，所以需要对xml文件进行读取，我使用的是TinyXML这个库，比较方便，下载解压后之间把6个文件（tinystr.h、tinystr.cpp、tinyxml.h、tinyxml.cpp、tinyxmlerror.cpp、tinyxmlparser.cpp）放到工程中就可以使用了，下载地址。
针对结点和边分别定义不同的类进行保存，然后利用TinyXML的库函数读取文件中的信息，将点和边分别保存，最后再将两者合并，建成一个完整的图。
点的定义：Node.h

#pragma once
#include <iostream>
#include <vector>
#include <string>
#include <algorithm>

using namespace std;

class Node
{
public:
	//结点名字
	int nodeType;
	//结点坐标
	int post[2] = { 0 };
};

边的定义：Edge.h

#pragma once
#include <iostream>
#include <vector>
#include <string>
#include <algorithm>

using namespace std;

class Edge
{
public:
	//边的类型：road、river、bridge、culvert
	string edgeType;
	//边的起点和终点
	string start2end[2] = {};
	//边的权重
	double weight;
	//边的损坏概率（暂时用不上）
	double probability;
};

将node和edge合并，方便在一个结构上查找信息，合并后的：Graph.h

#pragma once
#include <iostream>
#include <vector>
#include <string>
#include "tinystr.h"
#include "tinyxml.h"
#include <algorithm>
#include "Node.h"
#include "Edge.h"
#include <ctime>
#include <map>
#include <numeric>

using namespace std;

constexpr double Inf = 0x3f3f3f3f;

class Vertex
{
public:
	//point
	int name;
	//position of point
	int post[2] = { 0 };
	//points linked with vertex
	vector<int> childrenNode;
	//edges' type linked with vertex: road、bridge
	vector<string> edgeTypes;
	//edges' length linked with vertex
	vector<double> weights;
	vector<double> probabilities;
	//vertex state: true means the vertex is enabled; false means the vertex is disabled
	bool vertexState = true;
};

bool readXml(string strXmlPath, vector<Node>& node, vector<Edge>& edge, int& nodeNum, int& edgeNum);
void generateGraph(vector<Node>& node, vector<Edge>& edge, map<int, Vertex>& graph, int& nodeNum, int& edgeNum);

定义好之后就要先读取xml文件里的信息了，通过readXml函数实现。将xml文件中的点、边信息读取到对应的点集和边集中，然后再将对应的点集和边集建成一张图（函数generateGraph），具体代码如下：Graph.cpp

#include "Graph.h"

bool readXml(string strXmlPath, vector<Node>& node, vector<Edge>& edge, int& nodeNum, int& edgeNum)
{
	Node *pNode = new Node;
	Edge *pEdge = new Edge;

	string temp_post, temp_post_x, temp_post_y;
	string temp_start2end, temp_start, temp_end;

	//Load Xml file
	TiXmlDocument* Document = new TiXmlDocument();
	if (!Document->LoadFile(strXmlPath.c_str()))
	{
		cout << "Can not load Xml file" << endl;
		cin.get();
		return false;
	}

	//RootElement
	TiXmlElement* RootElement = Document->RootElement();
	//RootElement's FirstChildElement
	TiXmlElement* NextElement = RootElement->FirstChildElement();
	//Terminate loop until data fetch is end
	while (NextElement != NULL)
	{
		if (NextElement->ValueTStr() == "node")
		{
			nodeNum += 1;
			TiXmlElement* NodeElement = NextElement->FirstChildElement();

			//Assign nodeType
			while (NodeElement->ValueTStr() != "nodeType")
			{
				NodeElement = NodeElement->NextSiblingElement();
			}
			//pNode->nodeType = NodeElement->GetText();
			pNode->nodeType = stoi(NodeElement->GetText());

			//Assign node's position
			while (NodeElement->ValueTStr() != "post")
			{
				NodeElement = NodeElement->NextSiblingElement();
			}
			//Covert NodeElement's text to int array
			temp_post = NodeElement->GetText();
			int index = temp_post.find(',');
			char* currentIndex = (char*)temp_post.data();
			for (int i = 0; i < index; i++)
			{
				temp_post_x += *currentIndex;
				currentIndex++;
			}
			currentIndex = (char*)temp_post.data() + (index + 1);
			for (int i = index + 1; i < temp_post.size(); i++)
			{
				temp_post_y += *currentIndex;
				currentIndex++;
			}
			pNode->post[0] = stoi(temp_post_x);
			pNode->post[1] = stoi(temp_post_y);
			temp_post_x = "";
			temp_post_y = "";

			node.push_back(*pNode);
		}
		if (NextElement->ValueTStr() == "edge1")
		{
			edgeNum += 1;
			TiXmlElement* EdgeElement = NextElement->FirstChildElement();

			//Assign edgeType
			while (EdgeElement->ValueTStr() != "edgeType")
			{
				EdgeElement = EdgeElement->NextSiblingElement();
			}
			pEdge->edgeType = EdgeElement->GetText();


			//Assign edge's start and terminus
			while (EdgeElement->ValueTStr() != "start2end")
			{
				EdgeElement = EdgeElement->NextSiblingElement();
			}
			//Covert NodeElement's text to int array
			temp_start2end = EdgeElement->GetText();
			int index = temp_start2end.find(',');
			char* currentIndex = (char*)temp_start2end.data();
			for (int i = 0; i < index; i++)
			{
				temp_start += *currentIndex;
				currentIndex++;
			}
			currentIndex = (char*)temp_start2end.data() + (index + 1);
			for (int i = index + 1; i < temp_start2end.size(); i++)
			{
				temp_end += *currentIndex;
				currentIndex++;
			}
			pEdge->start2end[0] = temp_start;
			pEdge->start2end[1] = temp_end;
			temp_start = "";
			temp_end = "";

			//Assign edge's length
			while (EdgeElement->ValueTStr() != "weight")
			{
				EdgeElement = EdgeElement->NextSiblingElement();
			}
			pEdge->weight = atof(EdgeElement->GetText());

			while (EdgeElement->ValueTStr() != "probability")
			{
				EdgeElement = EdgeElement->NextSiblingElement();
			}
			pEdge->probability = atof(EdgeElement->GetText());

			edge.push_back(*pEdge);
		}
		//Read next node in same layer
		NextElement = NextElement->NextSiblingElement();
	}

	//free memory
	delete pNode;
	delete pEdge;
	delete Document;
	cout << "Read file completely" << endl;
	return true;
}

void generateGraph(vector<Node>& node, vector<Edge>& edge, map<int, Vertex>& graph, int& nodeNum, int& edgeNum)
{
	Vertex* currentVertex = new Vertex;
	/*currentGraph->vertex_num = nodeNum;
	currentGraph->edge_num = edgeNum;*/
	for (int i = 0; i < nodeNum; i++)
	{
		currentVertex->name = node[i].nodeType;//节点名
		currentVertex->post[0] = node[i].post[0];//节点坐标
		currentVertex->post[1] = node[i].post[1];
		for (int j = 0; j < edgeNum; j++)
		{
			//对当前节点所连的边进行处理
			if (currentVertex->name == stoi(edge[j].start2end[0]))
			{
				currentVertex->childrenNode.push_back(stoi(edge[j].start2end[1]));
				currentVertex->edgeTypes.push_back(edge[j].edgeType);
				currentVertex->weights.push_back(edge[j].weight);
				currentVertex->probabilities.push_back(edge[j].probability);
			}
			else
			{
				continue;
			}
		}
		graph[i] = *currentVertex;
		//clear vector
		currentVertex->edgeTypes.erase(currentVertex->edgeTypes.begin(), currentVertex->edgeTypes.end());
		currentVertex->childrenNode.erase(currentVertex->childrenNode.begin(), currentVertex->childrenNode.end());
		currentVertex->weights.erase(currentVertex->weights.begin(), currentVertex->weights.end());
		currentVertex->probabilities.erase(currentVertex->probabilities.begin(), currentVertex->probabilities.end());
		/*vector<string>().swap(currentGraph->edgeType);
		vector<string>().swap(currentGraph->edge_dst);*/
	}
	delete currentVertex;
}

处理完毕后，整个图的信息就保存在graph中。

2.3算法实现

在实现Dijkstra时，考虑了两种计算规则，一种是按最短距离来算，一种是按最少时间来算，距离直接用的是两个点坐标的欧氏距离。时间为距离除以权重。代码如下：Dijkstra.h

#pragma once
#include "Graph.h"
#include <numeric>
#include <cmath>

class Dis : public Vertex
{
public:
	bool visit;
	double value;
	string path;
};

//计算X->Y的距离，若边X->Y存在则设为距离，否则设为无穷
double calDistance(Vertex &X, Vertex &Y);

//计算X->Y的时间，若边存在则返回距离/权重，否则返回无穷
double calTime(Vertex &X, Vertex &Y);

//begin为起点；flag表示选择计算距离还是时间，默认0为距离，1为时间
void Dijkstra(map<int, Vertex> &graph, map<int, Dis> &dis, int begin, int end, int flag = 0);

//打印路径
void printPath(map<int, Dis> &dis, int begin, int end, int flag = 0);

Dijkstra.cpp

#include "Dijkstra.h"

double calDistance(Vertex &X, Vertex &Y)
{
	double temp;
	auto it = find(X.childrenNode.begin(), X.childrenNode.end(), Y.name);
	if (it != X.childrenNode.end())//说明存在X->Y边
	{
		temp = pow(abs(X.post[0] - Y.post[0]), 2) + pow(abs(X.post[1] - Y.post[1]), 2);
		return pow(temp, 0.5);
	}
	else
	{
		return Inf;
	}
}

double calTime(Vertex &X, Vertex &Y)
{
	double temp;
	auto it = find(X.childrenNode.begin(), X.childrenNode.end(), Y.name);
	if (it != X.childrenNode.end())//说明边存在
	{
		double dist = calDistance(X, Y);
		temp = X.weights.at(distance(X.childrenNode.begin(), it));
		return dist / temp;
	}
	else
	{
		return Inf;
	}
}

void Dijkstra(map<int, Vertex> &graph, map<int, Dis> &dis, int begin, int end, int flag)
{
	Dis *pdis = new Dis;
	Vertex temp_graph;
	vector<Dis*> currentNeighborhoods;//当前节点的相邻节点

	//初始化dis数组
	for (int i = 0; i < graph.size(); i++)
	{
		pdis->name = graph[i].name;
		pdis->post[0] = graph[i].post[0];
		pdis->post[1] = graph[i].post[1];
		pdis->childrenNode = graph[i].childrenNode;
		pdis->edgeTypes = graph[i].edgeTypes;
		pdis->weights = graph[i].weights;
		pdis->probabilities = graph[i].probabilities;
		pdis->vertexState = graph[i].vertexState;
		if (begin == graph[i].name)
		{
			//设置起点到起点的路径为0
			pdis->value = 0;
			//设置起点访问状态为true
			pdis->visit = true;
		}
		else
		{
			//设置初始值
			if (flag == 0)//计算距离
			{
				pdis->value = calDistance(graph[begin], graph[i]);
			}
			else//计算时间
			{
				pdis->value = calTime(graph[begin], graph[i]);
			}
			//设置访问状态
			pdis->visit = false;
		}
		//设置当前路径
		pdis->path = to_string(begin) + "->" + to_string(graph[i].name);
		//添加到dis中
		dis[i] = *pdis;
	}

	//计算剩余的顶点的最短路径（刨掉起点）
	int count = 1;
	while (count != dis.size())
	{
		//temp保存当前dis数组中值最小的下标
		int temp = 0;
		//min记录当前的最小值
		double min = Inf;
		for (int i = 0; i < dis.size(); i++)
		{
			if (!dis.at(i).visit && dis.at(i).value < min)
			{
				min = dis.at(i).value;
				temp = i;
			}
		}
		if (min == Inf)
		{
			break;
		}
		//将temp对应的点放入已经找到最短路径的集合中
		dis.at(temp).visit = true;
		count++;

		//获取当前最小节点的相邻节点
		currentNeighborhoods.clear();
		vector<int> outArray(dis[temp].childrenNode);
		for (int i = 0; i < outArray.size(); i++)
		{
			auto it = dis.find(outArray[i]);
			if (it != dis.end())
			{
				currentNeighborhoods.push_back(&(it->second));
			}
		}

		//松弛
		for (int i = 0; i < currentNeighborhoods.size(); i++)
		{
			if (flag == 0)//求距离
			{
				if (!currentNeighborhoods[i]->visit && (dis[temp].value + calDistance(graph[temp], graph[currentNeighborhoods[i]->name]) < currentNeighborhoods[i]->value))
				{
					//如果新得到的边可以影响其他未访问的顶点，那就更新它的最短路径
					currentNeighborhoods[i]->value = dis[temp].value + calDistance(graph[temp], graph[currentNeighborhoods[i]->name]);
					currentNeighborhoods[i]->path = dis[temp].path + "->" + to_string(currentNeighborhoods[i]->name);
				}
			}
			else//求时间
			{
				if (!currentNeighborhoods[i]->visit && (dis[temp].value + calTime(graph[temp], graph[currentNeighborhoods[i]->name]) < currentNeighborhoods[i]->value))
				{
					//如果新得到的边可以影响其他未访问的顶点，那就更新它的最短路径
					currentNeighborhoods[i]->value = dis[temp].value + calTime(graph[temp], graph[currentNeighborhoods[i]->name]);
					currentNeighborhoods[i]->path = dis[temp].path + "->" + to_string(currentNeighborhoods[i]->name);
				}
			}
		}
	}

	//打印路径
	printPath(dis, begin, end, flag);
}

void printPath(map<int, Dis> &dis, int begin, int end, int flag)
{
	if (flag == 0)//显示距离
	{
		cout << "The shortest path from " << begin << " to " << end << " is: " << endl;
	}
	else//显示时间
	{
		cout << "The shortest path from " << begin << " to " << end << " is: " << endl;
	}
	
	//只显示终点路径
	auto it = dis.find(end);
	if (it->second.value != Inf)
	{
		if (flag == 0)
		{
			cout << it->second.path << "\t" << "distance: " << it->second.value << endl;
		}
		else
		{
			cout << it->second.path << "\t" << "time: " << it->second.value << endl;
		}
	}
	else
	{
		cout << it->second.path << "\t" << "no shortest path" << endl;
	}

	//显示起点到所有点的距离
	/*for (auto &i : dis)
	{
		if (i.second.value != Inf)
		{
			if (flag == 0)
			{
				cout << i.second.path << "\t" << "distance: " << i.second.value << endl;
			}
			else
			{
				cout << i.second.path << "\t" << "time: " << i.second.value << endl;
			}
		}
		else
		{
			cout << i.second.path << "\t" << "no shortest path" << endl;
		}
	}*/
}

主函数：main.cpp

#include "Graph.h"
#include "Dijkstra.h"

using namespace std;

int main()
{
	vector<Node> nodes;
	vector<Edge> edges;
	map<int, Vertex> graph1, graph2;
	//vector<Table> table1, table2;

	bool result;
	int nodeNum = 0;
	int edgeNum = 0;

	result = readXml("../map_node50.xml", nodes, edges, nodeNum, edgeNum);
	if (result)
	{
		generateGraph(nodes, edges, graph1, nodeNum, edgeNum);
		graph2 = graph1;
	}
	int flag1 = 0;
	map<int, Dis> dis1, dis2;
	Dijkstra(graph1, dis1, 0, 49, flag1);//shortest distance
	//int flag2 = 1;
	//Dijkstra(graph2, dis2, 0, 49, flag2);//shortest time
	
}

测试的map_node50.xml文件下载地址。
如此就可以运行测试得到结果了，结果如图：