RRT bidirectional algortihm matlab code

I'm working on RRT bi directional algorithm,in this algorithm we have one initial and one final goal and from each of this point there will be tree to each other side until the distance of both tree be less than the step size of each node to another node connect them together in this case i write the code for first point i mean initial goal and it work very well but how can i write for final goal and i ask matlab when the distance between two tree is less than step size connect them to each other.

clc; close;

openfig('Simple4.fig');hold on

Xs=1; Ys=1; Xg=9; Yg=9;

n=1300; step=0.01

CON=zeros(n);

size=1; Reach=0; N=zeros(2,n); N(1,1)=Xs; N(2,1)=Ys;

tic;

while size<n

if sqrt((N(1,size)-Xg)^2+(N(2,size)-Yg)^2)<step

Reach=1; ReachG=size; break

end

%%%%%%%%%%%%%%% Xrand (Collision Free) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

CD=0;

 while CD==0

Xc=10*rand(1); Yc=10*rand(1); Xn=Xc; Yn=Yc; simple4ColDec

end

Xrand=Xc; Yrand=Yc;

%%%%%%%%%%%%%%% Xnearest %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

 min=100;

for i=1:size

if sqrt((N(1,i)-Xrand)^2+(N(2,i)-Yrand)^2)<min

 min=sqrt((N(1,i)-Xrand)^2+(N(2,i)-Yrand)^2);

 Xnearest= N(1,i); Ynearest=N(2,i); nearest=i;

 end

 end

 %%%%%%%%%%%%%%%Xnew (Expanding the tree) %%%%%%%%%%%%%%%%%%%%%%%%%%%%

Xc=Xnearest; Yc=Ynearest; CD=1;

D=sqrt((Xnearest-Xrand)^2+(Ynearest-Yrand)^2);

if step<=sqrt((Xnearest-Xrand)^2+(Ynearest-Yrand)^2)

Xn=((v-step)/v)*Xnearest+(step/v)*Xrand;

 Yn=((v-step)/v)*Ynearest+(step/v)*Yrand;

 else

       Xn=Xrand; Yn=Yrand;

end

simple4steer

if CD==1

Xnew=Xrand; Ynew=Yrand;

else

Xnew=((v-1)/100)*Xrand+(1-(v-1)/100)*Xnearest;

Ynew=((v-1)/100)*Yrand+(1-(v-1)/100)*Ynearest;

 end

size=size+1

N(1,size)=Xnew; N(2,size)=Ynew;

CON(nearest,size)=sqrt((N(1,nearest)-N(1,size))^2+ ...

(N(2,nearest)-N(2,size))^2);

 CON(size,nearest)=CON(nearest,size);

end

t1=toc;

Reach

if Reach==1

N(1,size+1)=Xg; N(2,size+1)=Yg;

CON(ReachG,size+1)=sqrt((N(1,ReachG)-Xg)^2+(N(2,ReachG)-Yg)^2);

CON(size+1,ReachG)=CON(ReachG,size+1);

end

for i=1:size

plot(N(1,i),N(2,i),'go','markersize',2,'MarkerFaceColor','g');

for j=1:size

if CON(i,j)>0

plot([N(1,j) N(1,i)], [N(2,j) N(2,i)],'color',...

[0,0.5,0.8],'linewidth',1);

end

end

end

if Reach==1

G=sparse(CON);

[dist, path]=graphshortestpath(G, 1, size+1); l=length(path);

for i=1:l-1

plot([N(1,path(i)) N(1,path(i+1))], [N(2,path(i)) ...

N(2,path(i+1))],'b','linewidth',3);http://www.mathworks.com/matlabcentral/answers/questions/new#matlabmarkup-refresh

end

[Guillaume]

Rather than writing blank lines in between each line of code (which must have taken you ages) use the {} Code button to mark your code as code (or just put a double space at the beginning of each line).

I would have done it for you were it not for the blank lines that need removing.

[amina shafanejad]

i put blank line to read it easier if not its hard to read as question this things u mention i know about it i just put here u can read it clearly so do u have any idea for this or no?

[NASADUDE]

that was so helpfull Guillaume WOW!