在GitHub我发现以下几点:
函数(距离、路径)= dijkstra算法(节点、段start_id finish_id)
% DIJKSTRA算法计算和路径点之间最短的距离在地图上
%使用迪杰斯特拉最短路径算法
%
%(距离、路径)= DIJKSTRA算法(节点、段SID, FID)
%计算开始和结束节点之间最短的距离和路径SID和支撑材
%
%(距离、路径)= DIJKSTRA算法(节点、段SID)
%计算最短的距离和路径从开始节点SID
%其他节点的映射
%
%注意:
% DIJKSTRA算法建立了这样一个例子是如果没有提供输入创建的,
%但是忽略了例子,只是过程的输入,如果他们。
%
%的输入:
%节点应该是Nx3或Nx4矩阵格式[ID X Y]或[ID X Y Z]
%,ID是一个整数,X, Y, Z是笛卡尔坐标位置)
%的部分应该是一个Mx3矩阵[ID N1 N2]的格式
%,ID是一个整数,N1、N2从节点列表中对应节点ID
%,这样有一个(无向)边缘节点N1和N2 /段之间的节点
% SID应该整数对应的节点ID列表的开始节点
% FID(可选)应该是一个整数的节点ID列表中相应的完成
%
%输出:
%距离是最短欧氏距离
%如果FID被指定,DIST将1 x1代表最短的两倍
%之间的欧几里得距离SID和FID沿着地图段。DIST会
%值正如果没有段连接SID和支撑材。
%如果FID没有指定,DIST 1 xn向量代表最短
% SID和所有其他节点之间的欧几里得距离在地图上。DIST会
%的正价值的任何节点不能达成的地图。
%路径节点的列表包含最短的路线
%如果支撑材是指定的,路径将一个节点id 1 xp向量SID支撑材。
%将返回NAN如果没有段连接SID支撑材。
%如果FID没有指定,路径将1 xn向量代表的细胞
%从SID最短路线在地图上所有其他节点。路径将会有一个值
%的南不能达成任何节点沿着地图的片段。
%
%的例子:
% dijkstra算法;%计算两个节点之间的最短路径和距离
% %的地图上随机生成节点和部分
%
%的例子:
%节点= [(1:10);兰德(100 * 2,10)];
%段= [(1:17);地板(1:0.5:9);装天花板(2:0.5:10)];
%图;情节(节点(:,2),节点(:,3),' k。');
%等;
% s = 1:17
%如果(s < = 10)文本(节点(年代,2),节点(年代,3),(“num2str (s)]);结束
%的阴谋(节点(段(s, 2:3) ', 2),节点(段(s, 2:3)”, 3),“k”);
%结束
% (d, p) = dijkstra算法(节点、段1,10)
% n = 2:长度(p)
%的阴谋(节点(p (n - 1: n), 2),节点(p (n - 1: n), 3),“r -。”,“线宽”,2);
%结束
%推迟;
%
%作者:约瑟夫·柯克
%的邮件:jdkirk630 gmail。com
%版本:1.3
%发布日期:5/18/07
如果(输入参数个数< 3)%设置
%(生成随机的例子节点和部分如果不作为输入)
%的节点创建一个随机集/顶点,并连接它们中的一些
%边缘/段。然后图形生成的地图。
num_nodes = 40;L = 100;max_seg_length = 30;id = (1: num_nodes) ';
节点= [id L *兰德(num_nodes, 2)];%创建随机节点
h =图;情节(节点(:,2),节点(:,3),“k”。)%绘制节点
文本(节点(num_nodes, 2),节点(num_nodes, 3),…
(' 'num2str (ids (num_nodes))),“颜色”,“b”,“FontWeight”,“b”)
持有在
num_segs = 0;段= 0 (num_nodes * (num_nodes-1) / 2, 3);
为我= 1:num_nodes-1%之间创建边缘的一些节点
文本(节点(我,2),节点(我,3),(' 'num2str (ids(我))),“颜色”,“b”,“FontWeight”,“b”)
为j = i + 1: num_nodes
d =√总和(节点(我2:3)——节点(j, 2:3)) ^ 2));
如果和(d < max_seg_length,兰德< 0.6)
情节([节点(我,2)节点(j, 2)]、[节点(我,3)节点(j, 3)),“k -”)
%增加这部分的链接列表
num_segs = num_segs + 1;
段(num_segs:) = (num_segs节点(我,1)节点(j, 1)];
结束
结束
结束
段(num_segs + 1: num_nodes * (num_nodes-1) / 2,:) = [];
轴([0 L 0 L])
%计算使用迪杰斯特拉最短路径的算法
%得到随机开始/结束节点,计算最短的距离和路径。
start_id =装天花板(num_nodes *兰德);disp ([“开始id =”num2str (start_id)]);
finish_id =装天花板(num_nodes *兰德);disp ([“完成id =”num2str (finish_id)]);
(距离、路径)= dijkstra算法(节点、段start_id finish_id);
disp ([“距离= 'num2str(距离)]);disp (['路径= ['num2str(路径)“]”]);
%如果最短路径存在,在地图上把它画出来。
图(h)
为k = 2:长度(路径)
m =找到(节点(:1)= =路径(k - 1));
n =找到(节点(:1)= =路径(k));
情节([节点(m, 2)节点(n, 2)]、[节点(m, 3)节点(n, 3)),“ro - - - - - -”,“线宽”2);
结束
标题([“最短的距离”num2str (start_id)”到“…
num2str (finish_id)“=”num2str(距离)))
持有从
其他的% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
%的主要功能——迪杰斯特拉算法
%初始化
node_id =节点(:1);
[num_map_pts,关口]=大小(节点);
表=稀疏(num_map_pts, 2);
shortest_distance =正(num_map_pts, 1);
解决= 0 (num_map_pts, 1);
路径= num2cell (NaN (num_map_pts, 1));
坳= 2;
pidx =找到(start_id = = node_id);
shortest_distance (pidx) = 0;
表(pidx坳)= 0;
解决(pidx) = 1;
路径(pidx) = {start_id};
如果(输入参数个数< 4)%计算所有节点的最短路径
while_cmd =“和(~定居)> 0”;
其他的%早期终止算法
while_cmd =“定居(zz) = = 0”;
zz =找到(finish_id = = node_id);
结束
而eval (while_cmd)
%更新表
表(:,col-1) =表(:,上校);
表(pidx坳)= 0;
%段中找到相邻的节点列表
neighbor_ids =[段(node_id (pidx) = =段(:,2),3);
段(node_id (pidx) = =段(:,3),2)];
%计算相邻节点的距离和路径的跟踪
为k = 1:长度(neighbor_ids)
cidx =找到(neighbor_ids (k) = = node_id);
如果~定居(cidx)
d =√总和(节点(pidx 2:关口)-节点(cidx 2:关口))^ 2));
如果(表(cidx col-1) = = 0) | |…
(表(cidx col-1) >(表(pidx col-1) + d))
表(cidx坳)=表(pidx col-1) + d;
tmp_path =路径(pidx);
路径(cidx) = {[tmp_path {1} neighbor_ids (k)]};
其他的
表(cidx坳)=表(cidx col-1);
结束
结束
结束
%表中找到的最小非零值并将其保存
nidx =发现(表(:,上校));
ndx =发现(表(nidx坳)= = min(表(nidx坳)));
如果isempty (ndx)
打破
其他的
pidx = nidx (ndx (1));
shortest_distance (pidx) =表(pidx坳);
解决(pidx) = 1;
结束
结束
如果(输入参数个数< 4)%返回的距离和路径数组的所有节点
dist = shortest_distance ';
路径=路径”;
其他的%返回结束节点的距离和路径
dist = shortest_distance (zz);
路径=路径(zz);
路径={1}路径;
结束
结束
不这个由https://github.com/andrehacker/fu-muster/blob/master/u12-randforests/@tree findpath.m
函数路径= findpath (obj, n1、n2)
% % FINDPATH两个节点之间的最短路径
% = T。FINDPATH (N1、N2)指数,这些指数遍历的顺序返回
%通过T树节点的索引节点的N1索引N2。
%
%返回的路径是最短的边数。它
%总是始于指数N1和结尾指数N2。这样的路径总是
%存在以来所有节点连接在树上。
%
%的例子:
% %找到节点ABplp和节点之间的路径“Ca”
%血统= tree.example;
% n1 =找到(lineage.strcmp (' ABplp '));
% n2 =找到(lineage.strcmp (Ca));
% =血统。findpath (n1、n2)
% pt =树(血统,“明确的”);
%指数= 1;
% i =路径
% pt = pt.set(我,指数);
%指数=指数+ 1;
%结束
% disp (pt.tostring)
如果n1 = = n2
路径= n1;
elseif任何(n2 = = obj.depthfirstiterator (n1))
% n2 n1的孩子
路径= (n1下降(n1));
其他的
% n2是其他地方的树
路径= (n1提升(n1));
结束
函数p =提升(n)
如果n = = n2
p = [];
返回;
结束
父母= obj.getparent (n);
如果任何(n2 = = obj.depthfirstiterator(父)
%是在父后裔,所以来自父母
p =[父下降(父)];
其他的
%不,所以我们仍然有上升
p =(父(父)提升);
结束
结束
函数p =下降(n)
如果n = = n2
p = [];
返回
结束
孩子= obj.depthfirstiterator (n);
为c =孩子(2:结束)%摆脱当前节点的序列
如果任何(n2 = = obj.depthfirstiterator (c))
p =下降(c) [c];
打破
结束
结束
结束
结束
函数(路径、目标gfound) = findPath(地图,V, E,启动、中转地点ECwaypoints,半径)
% FINDPATH:返回开始节点之间的最短路径
V %最近的航点给定一组的节点和边E。
%
%(路径、目标GFOUND) = FINDPATH(地图,V, E,启动、中转地点ECWAYPOINTS,半径)的回报
%开始节点之间的最短路径和最亲密的路标
% V给出一组节点和边E。
%
%的输入
%地图N-by-4矩阵包含坐标的墙壁
环境:百分比(x1, y1, x2, y2)
% V组节点的路线图
% E的边缘的路线图
%开始1×2数组包含x / y坐标的开始位置
%路标点n除以2包含中转地点的坐标矩阵
%的环境(x, y):
% ECwaypoints n除以2的坐标矩阵包含额外的信用
%中转地点环境:(x, y)
%半径半径的机器人
%
%输出
%路径包含一系列的点代表的n除以2数组
%最短路径连接初始和最亲密的目标点
%的目标1×2数组包含x / y坐标最近的目标节点
% gfound整数表示的数量目标
%
%康奈尔大学
%美4180/5180:自主移动机器人
%总决赛
% Pu,肯尼斯(kp295)
% % = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
%初始化变量
% = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
%的开始节点之间所有可能的边缘和顶点V,检查
%边缘是有效的,如果将它添加到E
为i = 1:尺寸(V, 1)
v =(我:);
如果(edgeIsFree(开始,v,地图,半径))
E = [E;首先,v];
%去除:
%的阴谋([(1)开始,v (1)]、[v开始(2),(2)),' b ');
结束
结束
% V加起点
V = [V;开始);
%建立目标节点的列表
目标=(锚点;ECwaypoints);
%使用dijkstra算法找到最小路径
[路径、目标gfound] = dijkstra算法(V E,开始,目标);
结束
,还发现另一个https://github.com/kennethpu/iRoombot/blob/172347c1be1d87b6e4138d1dfc848abff899af38/dijkstra.m
函数[路径、目标gfound] = dijkstra算法(V E开始,目标)
% DIJKSTRA算法:将图像作为输入由一组节点V和表示
%边E,起始位置和目标位置的列表,然后返回
%开始节点之间的最短路径和最接近的目标节点。使用
%的迪杰斯特拉算法
%
%(路径、目标GFOUND) = DIJKSTRA算法(V E开始,目标)的回报
%启动和目标节点之间的最短路径节点给定一组V
%和边E
%
%的输入
% V组节点图
% E的边缘图
%开始1×2包含x / y开始节点的坐标数组
% n除以2目标数组包含x / y坐标的目标节点
%
%输出
%路径包含一系列的点代表的n除以2数组
%最短路径连接初始和最亲密的目标点
%的目标1×2数组包含x / y坐标最近的目标节点
% gfound整数表示的数量目标
%
%康奈尔大学
%美4180/5180:自主移动机器人
%总决赛
% Pu,肯尼斯(kp295)
% % = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
%初始化变量
% = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
V_cost = 1(大小(V, 1), 1) * 9999;%的数组节点成本(最初嗨)
V_prev = 0(大小(V, 1), 1);最初%的列表节点的父指数(0)
%的整数来跟踪目标发现的数量
gfound = 0;
%设置成本(距离开始)开始为0
V_cost = setCost(启动、V V_cost 0);
%所有节点最初是在问
Q = V;%被优化的节点列表
% % = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
% DJIKSTRAS运行
% = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
%循环而仍有优化的节点
而大小(Q, 1)
% u在Q以最小的成本获得节点
qidx = minNode (Q, V, V_cost);
u = Q (qidx:);
% u从Q删除节点
问(qidx:) = [];
%打破循环的如果你的成本是马克斯。没有更多的节点是可及的
%从当前节点
如果(getCost (u, V, V_cost) = = 9999)
打破;
结束
%获得邻居节点索引的列表
n_ids = getNeighbors (u, V, E);
% u的遍历所有的邻居
为i = 1:尺寸(n_ids, 1)
%得到邻居节点n
n = V (n_ids(我):);
% n的选择成本的成本计算u + u之间的距离
% n
u_cost = getCost (u, V, V_cost);
n_dist = pdist ([u; n],“欧几里得”);
alt = u_cost + n_dist;
%如果计算选择成本比n,存储成本便宜
% n取代存储成本和集u n的父节点
如果alt < getCost (n、V V_cost)
V_cost = setCost (n、V、V_cost alt);
V_prev = setPrev (n, V, V_prev u);
结束
结束
如果ismember (u,目标,“行”)
gfound = gfound + 1;
如果gfound = =大小(目标1)
打破;
结束
结束
结束
% % = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
%找到最近的目标
% = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
%初始化数组来保存目标节点和相应的成本
goal_costs = 0(大小(目标1),大小(目标2)+ 1);
为i = 1:尺寸(目标1)
%在goal_costs获得为每个目标成本和存储
goal_costs(我:)=(目标(我,:),getCost(目标(我:),V, V_cost)];
结束
%按照成本goal_costs排序
goal_costs = sortrows (goal_costs 3);
%返回最便宜的目标
目标= goal_costs (1:2);
% % = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
%重建最小路径接近目标
% = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
%从目标点
u =目标;
%的阴谋(u(1),(2),“去”,“线宽”,2);
%添加目标点的路径
路径= u;
%倒退从目标点添加父节点到父节点索引
%不再有效
而getPrev (u, V, V_prev)
prev_idx = getPrev (u, V, V_prev);
u = V (prev_idx:);
%的阴谋(u(1),(2),“去”,“线宽”,2);
%的阴谋([u(1)路径(1,1)],[u(2),路径(1、2),“g”,“线宽”,2);
路径= (u;路径);
结束
结束
% % = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
% minNode
% = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
% Helper函数得到的索引节点和最小的成本
%的节点列表
%
%的输入
% Q搜索的节点列表
% V图的节点列表
V % V_cost列表相关费用
%
%输出
% nidx指数Q节点的最小成本
函数nidx = minNode (Q, V, V_cost)
%初始化变量
nidx = 1;
min_cost = 9999;
%在问遍历所有节点
为i = 1:尺寸(Q, 1)
节点=问(我:);
成本= getCost(节点,V, V_cost);
%如果节点成本小于最低,保存最低成本和节点索引
如果(成本< min_cost)
nidx =我;
min_cost =成本;
结束
结束
结束
% % = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
% getCost
% = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
% Helper函数得到一个节点的成本
%
%的输入
% n给定节点
% V图的节点列表
V % V_cost列表相关费用
%
%输出
%与节点相关成本费用
函数成本= getCost (n、V V_cost)
%中找到相应指数n V
[~,idx] = ismember (n, V,“行”);
% V_cost查找相应的成本
成本= V_cost (idx);
结束
% % = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
% setCost
% = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
% Helper函数来设置一个节点的成本
%
%的输入
% n给定节点
% V图的节点列表
V % V_cost列表相关费用
%与节点相关成本费用
%
%输出
V % ret更新列表,相关费用
函数ret = setCost (n、V、V_cost、成本)
%中找到相应指数n V
[~,idx] = ismember (n, V,“行”);
%设置相应的成本在V_cost我们的目标成本
V_cost (idx) =成本;
%返回列表更新我们的改变是不会丢失
ret = V_cost;
结束
% % = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
% getPrev
% = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
% Helper函数获得一个给定节点的父节点的索引
%
%的输入
% n给定节点
% V图的节点列表
% V_prev父母与V的列表
%
%输出
% prev_idx指数给定节点的父节点
函数prev_idx = getPrev (n、V V_prev)
%中找到相应指数n V
[~,idx] = ismember (n, V,“行”);
% V_prev查找对应的父指数
prev_idx = V_prev (idx);
结束
% % = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
% setPrev
% = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
% Helper函数来设置一个给定节点的父节点
%
%的输入
% n给定节点
% V图的节点列表
% V_prev父母与V的列表
%:给定节点的父节点
%
%输出
% ret更新列表的父母与V
函数ret = setPrev (n、V、V_prev上一页)
%中找到相应指数n V
[~,idx] = ismember (n, V,“行”);
%找到指数对应prev V
[~,prev_idx] = ismember(上一页,V,“行”);
%设置相应prev_idx V_prev prev_idx到我们的目标
V_prev (idx) = prev_idx;
%返回列表更新我们的改变是不会丢失
ret = V_prev;
结束
% % = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
% getNeighbors
% = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
% Helper函数得到所有相邻节点的一个给定的指标
%的节点
%
%的输入
% n给定节点
% V图的节点列表
% E图中的边列表
%
%输出
ids % n×1 V数组索引节点的相邻节点n
函数id = getNeighbors (n、V E)
%初始化空列表
id = [];
%遍历所有边缘
为e = 1:尺寸(e, 1)
%获得端点的边缘
v1 = E (E, 1:2);
v2 = E (E, 3:4);
%如果目标节点对应于第一端点,保存
%的端点
如果(所有(v1 = = n))
[~,idx] = ismember (v2, V,“行”);
id = [id; idx];
%如果目标节点对应于第二端点,先保存
%的端点
elseif(所有(v2 = = n))
[~,idx] = ismember (v1, V,“行”);
id = [id; idx];
结束
结束
结束
