%??Rahul?Kala?IIIT?Allahabad?Creative?Commons?Attribution-ShareAlike?4.0?International?License.?
%?The?use?of?this?code?its?parts?and?all?the?materials?in?the?text;?creation?of?derivatives?and?their?publication;?and?sharing?the?code?publically?is?permitted?without?permission.?

%?Please?cite?the?work?in?all?materials?as:?R.?Kala?（2014）?Code?for?Robot?Path?Planning?using?Rapidly-exploring?Random?Trees?Indian?Institute?of?Information?Technology?Allahabad?Available?at:?http://rkala.in/codes.html

map=im2bw（imread（‘map1.bmp‘））;?%?input?map?read?from?a?bmp?file.?for?new?maps?write?the?file?name?here
source=[10?10];?%?source?position?in?Y?X?format
goal=[490?490];?%?goal?position?in?Y?X?format
stepsize=20;?%?size?of?each?step?of?the?RRT
disTh=20;?%?nodes?closer?than?this?threshold?are?taken?as?almost?the?same
maxFailedAttempts?=?10000;
display=true;?%?display?of?RRT

%%%%%?parameters?end?here?%%%%%

tic;
if?~feasiblePoint（sourcemap）?error（‘source?lies?on?an?obstacle?or?outside?map‘）;?end
if?~feasiblePoint（goalmap）?error（‘goal?lies?on?an?obstacle?or?outside?map‘）;?end
if?display?imshow（map）;rectangle（‘position‘[1?1?size（map）-1]‘edgecolor‘‘k‘）;?end
RRTree=double（[source?-1]）;?%?RRT?rooted?at?the?source?representation?node?and?parent?index
failedAttempts=0;
counter=0;
pathFound=false;
while?failedAttempts<=maxFailedAttempts??%?loop?to?grow?RRTs
????if?rand?????????sample=rand（12）?.*?size（map）;?%?random?sample
????else
????????sample=goal;?%?sample?taken?as?goal?to?bias?tree?generation?to?goal
????end
????[A?I]=min（?distanceCost（RRTree（:1:2）sample）?[]1）;?%?find?closest?as?per?the?function?in?the?metric?node?to?the?sample
????closestNode?=?RRTree（I（1）1:2）;
????theta=atan2（sample（1）-closestNode（1）sample（2）-closestNode（2））;??%?direction?to?extend?sample?to?produce?new?node
????newPoint?=?double（int32（closestNode（1:2）?+?stepsize?*?[sin（theta）??cos（theta）]））;
????if?~checkPath（closestNode（1:2）?newPoint?map）?%?if?extension?of?closest?node?in?tree?to?the?new?point?is?feasible
????????failedAttempts=failedAttempts+1;
????????continue;
????end
????if?distanceCost（newPointgoal）????[A?I2]=min（?distanceCost（RRTree（:1:2）newPoint）?[]1）;?%?check?if?new?node?is?not?already?pre-existing?in?the?tree
????if?distanceCost（newPointRRTree（I2（1）1:2））????RRTree=[RRTree;newPoint?I（1）];?%?add?node
????failedAttempts=0;
????if?display?
????????line（[closestNode（2）;newPoint（2）][closestNode（1）;newPoint（1）]）;
????????counter=counter+1;M（counter）=getframe;
????end
end
if?display?&&?pathFound?
????line（[closestNode（2）;goal（2）][closestNode（1）;goal（1）]）;
????counter=counter+1;M（counter）=getframe;
end
if?display?
????disp（‘click/press?any?key‘）;
????waitforbuttonpress;?
end
if?~pathFound?error（‘no?path?found.?maximum?attempts?reached‘）;?end
path=[goal];
prev=I（1）;
while?prev>0
????path=[

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件????????3331??2014-06-06?15:35??RRT\astart.m
?????文件????????1012??2014-06-06?15:35??RRT\checkPath.m
?????文件?????????591??2014-06-06?15:35??RRT\distanceCost.m
?????文件?????????756??2014-06-06?15:35??RRT\feasiblePoint.m
?????文件??????251078??2014-06-06?15:35??RRT\map1.bmp
?????文件??????251078??2014-06-06?15:35??RRT\map2.bmp
?????文件??????251078??2014-06-06?15:35??RRT\map3.bmp
?????文件??????251078??2014-06-06?15:35??RRT\map4.bmp
?????文件??????251078??2014-06-06?15:35??RRT\map5.bmp
?????文件??????210658??2014-06-06?15:35??RRT\RRT.pdf