%%?pathRRT
%%??-?create?a?path?from?a?start?node?to?an?end?node
%%????using?the?RRT?algorithm.
%%??-?RRT?=?Rapidly-exploring?Random?Tree
%%??
%%?趙燕江修改版

function?pathRRT

%?create?random?world
Size?=?100;
NumObstacles?=?100;
world?=?createWorld（NumObstacles[Size;?Size][0;0]）;

%?standard?length?of?path?segments
segmentLength?=?5;

%?randomly?select?start?and?end?nodes
start_node?=[0?0?0?0?0?];
end_node???=?[100?100?0?0?0];

%?establish?tree?starting?with?the?start?node
tree?=?start_node;

%?check?to?see?if?start_node?connects?directly?to?end_node
if?（?（norm（start_node（1:2）-end_node（1:2））????&&（collision（start_nodeend_nodeworld）==0）?）
??path?=?[start_node;?end_node];
else
??numPaths?=?0;
??while?numPaths<1
??????[treeflag]?=?extendTree（treeend_nodesegmentLengthworld）;
??????numPaths?=?numPaths?+?flag;
??end
end

%?find?path?with?minimum?cost?to?end_node

path?=?findMinimumPath（treeend_node）;
plotWorld（worldpathtree）;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%?createWorld
%%??-?create?random?world?with?obstacles
%%??the?first?element?is?the?north?coordinate
%%??the?second?element?is?the?south?coordinate
function?world?=?createWorld（NumObstacles?NEcorner?SWcorner）

??%?check?to?make?sure?that?the?region?is?nonempty
?
??????
??%?create?world?data?structure

????world.NumObstacles?=?NumObstacles;
????world.NEcorner?=?NEcorner;
????world.SWcorner?=?SWcorner;
??????????????????????????
????%?create?NumObstacles?
????maxRadius?=?min（NEcorner（1）-?SWcorner（1）?NEcorner（2）-SWcorner（2））;
????maxRadius?=?5*maxRadius/NumObstacles/2;
????for?i=1:NumObstacles
????????%?randomly?pick?radius
????????world.radius（i）?=?maxRadius*rand;
????????%?randomly?pick?center?of?obstacles
????????cn?=?SWcorner（1）?+?world.radius（i）...
????????????+?（NEcorner（1）-SWcorner（1）-2*world.radius（i））*rand;
????????ce?=?SWcorner（2）?+?world.radius（i）...
????????????+?（NEcorner（2）-SWcorner（2）-2*world.radius（i））*rand;
????????world.cn（i）?=?cn;
????????world.ce（i）?=?ce;
????end

??
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%?generateRandomNode
%%???create?a?random?node?（initialize）
function?node=generateRandomNode（world）

%?randomly?pick?configuration
pn???????=?（world.NEcorner（1）-world.SWcorner（1））*rand;
pe???????=?（world.NEcorner（2）-world.SWcorner（2））*rand;
chi??????=?0;
cost?????=?0;
node?????=?[pn?pe?chi?cost?0];

%?check?collision?with?obstacle
while?collision（node?node?world）
???pn???????=?（world.NEcorner（1）-world.SWcorner（1））*rand;
???pe???????=?（world.NEcorner（2）-world.SWcorner（2））*rand;
???chi??????=?0;
???cost?????=?0;
???node?????=?[pn?pe?chi?cost?0];
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%?collision
%%???check?to?see?if?a?node?is?in?collsion?with?obstacles
function?collision_flag?=?collision（node?parent?world）;

collision_fla