Dijkstra算法例子
程序代码
Dijkstra算法的程序如下：
function [d,p] = dijkstra(adj, s, t)
%使用dijkstra求最短路径
%adj 输入 矩阵 邻接矩阵
%s 输入 整数 起点
%t 输入 整数或 [] 终点
%d 输出 向量 路径长度，若t==[]，则返回从起点到所有节点的路径长度
%p 输出 向量或 元胞 路径，若t==[]，则返回从起点到所有节点的路径(cell)
nodes_num =size(adj, 1);
dist =inf(nodes_num, 1);
previous =inf(nodes_num, 1);
Q =[1:nodes_num]';
%求邻居
neighbors =cell(nodes_num, 1);
for i =1:nodes_num; neighbors{i} = find(adj(i, :) > 0); end
dist(s) = 0;
while~isempty(Q)
% 取出距离最小点
[~, min_ind] = min(dist(Q));
min_node = Q(min_ind);
Q = setdiff(Q, min_node);
% 若是终点，则结束程序
if min_node == t
d = dist(min_node);
p = generate_path(previous, t);
return;
end
% 更新邻居的距离
for i = 1:length(neighbors{min_node})
neighbor = neighbors{min_node}(i);
alt = dist(min_node) + adj(min_node,neighbor);
if alt < dist(neighbor)
dist(neighbor) = alt;
previous(neighbor) = min_node;
end
end
end
d = dist;
p =cell(nodes_num, 1);
for i =1:nodes_num; p{i} = generate_path(previous, i); end
end
%由前趋推出路径
function path= generate_path(previous, t)
path = [t];
whileprevious(t) <= length(previous)
path = [previous(t) path];
t = previous(t);
end
end
找图中顶点间最短距离
在这样一张图中，找到从A到D的最短距离和路径。构造邻接矩阵如下：
adj = [
0 12 0 0 0 16 14;
12 0 10 0 0 7 0;
0 10 0 3 5 6 0;
0 0 3 0 4 0 0;
0 0 5 4 0 2 8;
16 7 6 0 2 0 9;
14 0 0 0 8 9 0];
指定起点和终点，使用上面的程序计算即可：
[dist,path] = dijkstra(adj, 1, 4);
结果如下：
最短距离： 22.00
路径 ： 'A' 'F' 'E' 'D'
找栅格地图中两点间最短距离
如下图所示栅格地图，指定起点和终点，智能体（或机器人）只能在“上、下、左、右”四个方向移动，找出最短路径：
结果如下：
可以直接提供邻接矩阵给上面的程序，但是需要修改程序中求邻居的部分（四个方向相邻栅格中不是障碍物的栅格），同时还需要在程序中对某栅格是否是障碍物进行判断，因为是障碍物的话程序不需要对该栅格进行规划。
也可以为程序提供栅格数量（除障碍物）和每个栅格的邻居，删除程序中求邻居的部分，修改程序中邻居间的距离（比如为1）即可。