-
Notifications
You must be signed in to change notification settings - Fork 0
/
ShowPath.m
44 lines (43 loc) · 1.02 KB
/
ShowPath.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
function ShowPath(G,path)
if nargin < 1 % demo mode
disp('No input was set!');
return;
end
map=G.mapshow;
drate=G.drate;
figure;
imshow(map);
imagesc(map);
hold on;
for i=1:size(path,2)
color=rand(1,3);
if (path(i).dist==-1)
continue;
end
Pathtemp=path(i).points;
l=size(Pathtemp,2);
x=[];
y=[];
for ll=1:l
x=[x G.points(2,Pathtemp(ll))*drate+(drate/2)];
y=[y G.points(1,Pathtemp(ll))*drate+(drate/2)];
end
hold on;
plot(x,y,'b-');
hold on;
coltemp=[];
plot(x,y,'color',color(1,:),'LineWidth',3);
plot(x,y,'color',color(1,:),'LineWidth',3);
hold on;
plot(x(1),y(1), '-mo',...
'LineWidth',2,...
'MarkerEdgeColor','k',...
'MarkerFaceColor',color(1,:),...%[.49 1 .63],...
'MarkerSize',11);
hold on;
plot(x(end),y(end),'--ys',...
'LineWidth',2,...
'MarkerEdgeColor','k',...
'MarkerFaceColor',color(1,:),...
'MarkerSize',11);
end