%%?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?=?generateRandomNode（world）;
end_node???=?generateRandomNode（world）;

%?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
??if?（NEcorner（1）?<=?SWcorner（1））?|?（NEcorner（2）?<=?SWcorner（2））
??????disp（‘Not?valid?corner?specifications!‘）
??????world=[];
??????
??%?create?world?data?structure
??else
????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
??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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%