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)即可。

原文发布于微信公众号 - mwangblog(mwangblog)

原文发表时间:2019-05-13

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

发表于

我来说两句

0 条评论
登录 后参与评论

扫码关注云+社区

领取腾讯云代金券