%%???RRT*?3D?state?space
%%??-?create?a?path?from?a?start?node?to?an?end?node
%%????using?the?RRT?algorithm.
%%??-?RRT*?=?Rapidly-exploring?Random?Tree?star
%%?This?short?programm?is?writen?based?other?basic?RRT?code?and?used?for?test?my?hardware?work?.
%%?Apologize?no?time?to?add?the?enough?comments?
%%??Size（Adrian）?Xiao???DEC?2016??University?of?Queensland?Australia??uqsxiao@uq.edu.au

function?RRT_star_3D;

clear?all;
%?create?random?worlddetermine?the?parameters?of?simulation
Size?=?16384;???????%?the?2D?world?is?a?100?by?100?map

Obs_Cordi?=?[111;111;111];
NumObstacles?=?3;?%?randomly?obstacles?distributed?on?the?map
maxObstacleRadius?=?0.05;?%?define?the?max?radius?of?obstacle
maxturningangle?=?1.05;??%75deg
Heading?=?0.758;%?45?degree;?ccw?rotation?vector?from?positive?x-axis
world?=?createWorld（NumObstacles[Size;Size;Size][0;0;0]maxObstacleRadiusObs_Cordi）;
segmentLength?=?200;?%?standard?length?of?path?segments
hyper_R??=?450;

start_node?=[129129129000];????%start?node‘s?idx?is?1.?node?array?format?：[x?y?chi（indicate?if?this?node?get?to?the?end?node）?cost?parent‘s?idx]
end_node???=?[180018001800000];

%?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:3）-end_node（1:3））????&&（collision（start_nodeend_nodeworld）==0）?）?
??path?=?[start_node;?end_node];
else
??numPaths?=?0;?
??while?numPaths<2000
??????[treeflag]?=?extendTree（treeend_nodesegmentLengthworldmaxturninganglehyper_R）;
??????numPaths?=?numPaths?+?1;
??end
end

plotWorld（worldtree）;
clc
tree
size（tree）
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%?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?SWcornermaxRadius?obstacle_cordi）;

??%?check?to?make?sure?that?the?region?is?nonempty
??if?（NEcorner（1）?<=?SWcorner（1））?|?（NEcorner（2）?<=?SWcorner（2））?|?（NEcorner（3）?<=?SWcorner（3））%%?NEcorner（1:2）=（100?100）?SWcorner（1:2）=（00）
??????disp（‘Not?valid?corner?specifications!‘）
??????world=[];
??????
??%?create?world?data?structure
??else
????temp=size（obstacle_cordi）;
????NumObstacles=temp（1）;
????world.NumObstacles?=?NumObstacles;
????world.NEcorner?=?NEcorner;
????world.SWcorner?=?SWcorner;
?????
????for?i=1:NumObstacles
????????world.radius（i）?=?maxRadius;
????????world.cn（i）?=?obstacle_cordi（i1）;??????%?give?the?X?coordinate?to?current?obstacle
????????world.ce（i）?=?obstacle_cordi（i2）;???????%?give?the?Y?coordinate?to?current?obstacle
????????world.cz（i）?=?obstacle_cordi（i3）;?
????end
??end
%%?generateRandomNode
%%???create?a?random?node?（initialize）
function?node=generateRandomNode（world）;

%?randomly?pick?configuration?
pn???????=?（