chap3/chap3_matlab/RRT.m
%***************************************
%Author: Chaoqun Wang
%Date: 2019-10-15
%***************************************
%% ?????
clear; close all;
x_I=1; y_I=1;           % ?????
x_G=700; y_G=700;       % ?????
Thr=50;                 %???????
Delta= 30;              % ??????
%% ?????
T.v(1).x = x_I;         % T????????v??????????????T???
T.v(1).y = y_I;
T.v(1).xPrev = x_I;     % ??????????????
T.v(1).yPrev = y_I;
T.v(1).dist=0;          %????????????????????
T.v(1).indPrev = 0;     %
%% ?????????
figure(1);
ImpRgb=imread('newmap.png');
Imp=rgb2gray(ImpRgb);
imshow(Imp)
xL=size(Imp,1);%??x???
yL=size(Imp,2);%??y???
hold on
plot(x_I, y_I, 'ro', 'MarkerSize',10, 'MarkerFaceColor','r');
plot(x_G, y_G, 'go', 'MarkerSize',10, 'MarkerFaceColor','g');% ????????
count=1;
for iter = 1:3000
    x_rand=rand(1,2).*[xL, yL];
    %Step 1: ???????????x_rand
    %?????x_rand(1),x_rand(2)????????????
    
    [~,num] = size(T.v);
    dis_array = zeros(num,1);
    
    for i = 1:num
        dis_array(i) = norm(x_rand-[T.v(i).x, T.v(i).y]);
    end
    
    [min_value, min_index] = min(dis_array);
    x_near=[T.v(min_index).x, T.v(min_index).y];
    %Step 2: ??????????????x_near
    %???x_near????T?
    
    dis= norm(x_near-x_rand);
    x_new = x_near + Delta/dis * (x_rand-x_near);
    %Step 3: ????x_new??
    %???????????Delta
    
    %???????collision-free
    if (~collisionChecking(x_near,x_new,Imp))
        continue;
    end
    count=count+1;
    
    %Step 4:
    T.v(count).x = x_new(1);
    T.v(count).y = x_new(2);
    T.v(count).xPrev = x_near(1);     % ??????????????
    T.v(count).yPrev = x_near(2);
    T.v(count).dist=norm(x_new-x_near);          %????????????????????
    T.v(count).indPrev = min_index;
    
    %Step 5:???????????
    %????????????Thr????????????????Thr??????for??
    if (norm(x_new-[x_G,y_G])<Thr)
        xline = linspace(x_near(1),x_new(1));
        yline = linspace(x_near(2),x_new(2));
        plot(xline,yline);
        break;
    end
    
    %Step 6:?x_near?x_new????????
    %?? 1???plot?????????????????????????plot?????hold on??
    %?? 2??????????for???????x_near?x_new????????
    hold on
    xline = linspace(x_near(1),x_new(1));
    yline = linspace(x_near(2),x_new(2));
    plot(xline,yline);
%     plot(x_new(1), x_new(2), 'ro', 'MarkerSize',10, 'MarkerFaceColor','b');
    
    pause(0.01); %??0.1s???RRT????????
end
%% ???????????
if iter < 2000
    path.pos(1).x = x_G; path.pos(1).y = y_G;
    path.pos(2).x = T.v(end).x; path.pos(2).y = T.v(end).y;
    pathIndex = T.v(end).indPrev; % ??????
    j=0;
    while 1
        path.pos(j+3).x = T.v(pathIndex).x;
        path.pos(j+3).y = T.v(pathIndex).y;
        pathIndex = T.v(pathIndex).indPrev;
        if pathIndex == 1
            break
        end
        j=j+1;
    end  % ????????
    path.pos(end+1).x = x_I; path.pos(end).y = y_I; % ??????
    for j = 2:length(path.pos)
        plot([path.pos(j).x; path.pos(j-1).x;], [path.pos(j).y; path.pos(j-1).y], 'b', 'Linewidth', 3);
    end
else
    disp('Error, no path found!');
end