function?c;
clc
clear?all
close?all
%map1?隨機地表。
%?a=10;
%?b=0.2;
%?c=0.1;
%?d=0.6;
%?e=1;
%?f=0.1;
%?g=0.1;
%?for?x=1:80
%?????for?y=1:80
%?Z1=sin（y+a）+b*sin（x）+cos（d*（x^2+y^2）^（1/2））+e*cos（y）+f*sin（f*（x^2+y^2）^（1/2））+g*cos（y）;
%?%?Z1=SquareDiamond（628）;
%??figure（1）;
%??surf（Z1）;?%畫出三維曲面?
%??shading?flat;?%各小曲面之間不要網格?
%?%map2?山峰圖
tic;
h=[203525382025];
x0=[104045602020];
y0=[102550304510];
xi=[5.5854.55.53.5];
yi=[5765.564.5];
Z2=CeatHill（6hx0y0xiyi80）;?
figure（2）;
surf（Z2）;?%畫出三維曲面?
shading?flat;?%各小曲面之間不要網格?
%map3?合成圖
%??Z3=max（Z1Z2）;
%??figure（3）;
%??surf（Z3）;?%畫出三維曲面?
%??shading?flat;?%各小曲面之間不要網格?
segmentLength?=5;
start_node?=??[5705000];
end_node???=[803010100];
hold?on
plot3（start_node（:1）start_node（:2）start_node（:3）‘r*‘）;
plot3（end_node（:1）end_node（:2）end_node（:3）‘r*‘）;
tree?=?start_node;
if?（?（norm（start_node（1:3）-end_node（1:3））????&（collision（start_nodeend_node）==0）?）
??path?=?[start_node;?end_node];
??else
??numPaths?=?0;
??while?numPaths<1
??????[treeflag]?=?extendTree（treeend_nodesegmentLengthZ2）;
??????numPaths?=?numPaths?+?flag;
??end
end
?path?=?findMinimumPath（tree）;
?plot3（path（:1）path（:2）path（:3）‘r‘）;??
?toc;
?function?[data]=CeatHill（Nhx0y0xiyinum）?
x=1:1:num;y=1:1:num;
for?m=1:num
????for?n=1:num
????????Sum=0;
????????for?k=1:N
???????????s=h（k）*exp（-（（x（m）-x0（k））/xi（k））^2-（（y（n）-y0（k））/yi（k））^2）;
???????????Sum=Sum+s;
????????end
????????data（mn）=Sum;
????end
end

?function?collision_flag?=?collision（node?parentZ2high）;
collision_flag?=?0;
?h=[203525382025];
x0=[104045602020];
y0=[102550304510];
xi=[5.5854.55.53.5];
yi=[5765.564.5];
Z1=Z2;
%?for?i=1:80
%?????for?j=1:80
%?????????if（Z1（ji）>high）
%?????????????Z1（ji）=10000;
%?????????end
%?????end
%?end
if?（（node（1）>80）...
????|?（node（1）<0）...
????|?（node（2）>80）...
????|?（node（2）<0））
??collision_flag?=?1;
else
?????for?sigma?=?0:0.1:1
?????????p?=?sigma*node（1:3）?+?（1-sigma）*parent（1:3）;
??????????Sum1=0;
????????for?k=1:6
???????????s=h（k）*exp（-（（p（2）-x0（k））/xi（k））^2-（（p（1）-y0（k））/yi（k））^2）;
???????????Sum1=Sum1+s;
????????end
????????if（p（3）????????????collision_flag?=?1;
????????end
%?[linerow]=find（Z1==10000）;
%?ct=length（line）;
%?for?tt=1:ct
%?????B=floor（p（1））;
%?????C=floor（p（2））;
%?if?（B==line（tt1）&C==row（tt1））??????
%?????????collision_flag?=?1;
%?end
%?end

?????end
end
?function?[new_treeflaghigh]?=?extendTree（treeend_nodesegmentLengthZ2）;
??flag1?=?0;
??qet=1;
??while?flag1==0
????%?select?a?random?point
????if（qet==0）
????????randomPoint?=?[...
????????80*rand...
????????80*rand...
????????80*rand];
????else
????????randomPoint?=?[...
????????end_node（:1）...
????????end_node（:2）...
????????end_node（:3）];
????end
????%?find?leaf?on?node?that?is?closest?to?randomPoint
????%0.45*sqrt（tree（:1:

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件????????4925??2017-12-26?15:53??3維避障RRT算法\rrtsanwei.m
?????目錄???????????0??2018-05-19?18:25??3維避障RRT算法\