function?[]?=?Dijkstraguihua（）

clear?all;
close?all;
disp（‘Dijkstra?Path?Planning?start!!‘）;

p.start=[4.527.5];??
p.goal=[26.52.5];?
p.XYMAX=30;????

obstacle=GetBoundary（p）;%初始化障礙物

path=Dijkstra（obstaclep）;%30x2

figure（1）

if?length（obstacle）>=1
????plot（obstacle（:1）obstacle（:2）‘Or‘）;
????hold?on;
end
plot（p.start（1）p.start（2）‘*b‘）;hold?on;
plot（p.goal（1）p.goal（2）‘*r‘）;hold?on;
if?length（path）>=1
????%plot（path（:1）path（:2）‘b‘‘linewidth‘2）;
????hold?on;
end
j=size（path1）;%30
mindistance=0;
for?i=j:-1:2
????s=distance（path（i1）path（i2）path（i-11）path（i-12））;
????mindistance=s+mindistance;
end
axis（[0?p.XYMAX?0?p.XYMAX]）
set（gca‘xtick‘0:1:p.XYMAX‘ytick‘0:1:p.XYMAX‘GridLinestyle‘‘-‘...
????‘xgrid‘‘on‘‘ygrid‘‘on‘）;
grid?on;
disp（‘Shortest?Length?is‘）
disp（?mindistance）

end



function?path=Dijkstra（obstaclep）

path=[];

open=[p.start（1）?p.start（2）?0?p.start（1）?p.start（2）];
close=[];

next=MotionModel（）;

findFlag=false;

while?~findFlag
????initNopen=length（open（:1））;

????if?initNopen==0?
????????disp（‘No?path?to?goal!!‘）;?
????????return;?
????end
????for?io=1:initNopen
????????if?isSamePosi（open（io1:2）p.goal）
????????????disp（‘Find?Goal!!‘）;
????????????close=[open;close];
????????????findFlag=true;
????????????break;
????????end
????????
????????for?in=1:length（next（:1））
????????????m=[open（io1）+next（in1）?open（io2）+next（in2）?open（io3）+next（in3）];

????????????if?isObstacle（mobstacle）?
????????????????continue;
????????????end

????????????[flag?targetInd]=FindList（mopenclose）;
????????????
????????????if?flag==1
????????????????if?m（3）????????????????????open（targetInd3）=m（3）;
????????????????????open（targetInd4）=open（io1）;
????????????????????open（targetInd5）=open（io2）;
????????????????end
????????????elseif?flag==2?
????????????????if?m（3）????????????????????close（targetInd4）=open（io1）;
????????????????????close（targetInd5）=open（io2）;
????????????????????open=[open;?close（targetInd:）];
????????????????????close（targetInd:）=[];
????????????????end
????????????else?
????????????????open=[open;[m?open（io1）?open（io2）]];
????????????end
????????end
????end
????if?findFlag==false
????????close=[close;?open（1:initNopen:）];
????????open（1:initNopen:）=[];
????end

????%animation（openclosepobstacle）;

end

path=GetPath（closep.startio）;

end


function?result=isSamePosi（ab）

result=false;
if?a（1）==b（1）?&&?a（2）==b（2）
????result=true;
end
end

function?boundary=GetBoundary（~）

boundary=[];
for?i1=0:30
???boundary=[boundary;[0.5?i1+0.5]];
???boundary=[boundary;[28.5?i1+0.5]];
???boundary=[boundary;[29.5?i1+0.5]];
end
for?i2=0:30
????%boundary=[boundary;[i2+0.5?0.5]];
????boundary=[boundary;[i2+0.5?29.5]];
end
for?i3=0:10
????boundary=[boundary;[i3+8.5?24.5]];
????boundary=[boundary;[i3+8.5?25.5]];
end
for?i4=0:1
??

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件???????63140??2014-09-24?22:49??Dijkstra\09-24-2014?10-48-51?_jpg
?????文件????????1515??2017-05-08?11:41??Dijkstra\DijkstraPlan.m
?????文件???????10664??2017-05-08?14:20??Dijkstra\DijkstraSample.m
?????文件????????5949??2017-06-01?12:06??Dijkstra\Dijkstraguihua.m
?????文件????????4604??2017-05-09?16:42??Dijkstra\Dijkstrasampleyichu.m
?????文件?????????156??2017-05-09?16:48??Dijkstra\distance.m
?????目錄???????????0??2018-01-19?09:30??Dijkstra\