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;
Xc=10*rand(1); Yc=10*rand(1); Xn=Xc; Yn=Yc; simple4ColDec
end
Xrand=Xc; Yrand=Yc;
%%%%%%%%%%%%%%% Xnearest %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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
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;
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
