PRM

close all;
clear all;
%% map
global obs radius
obs=[22,-16;
    16,-18];
radius=[15.41 15.05];
src=[30;40];
dst=[-20;-40];

%% PRM
n=2000;
points=zeros(2,n);
points(:,1)=src;
points(:,n)=dst;
dis=zeros(n,n);
for i=2:n-1
    while true
        prand=rand(1,2)*100-50;
        if check(prand)
            break
        end
    end
    points(:,i)=prand;
end
for i=1:n
    for j=1:n
        if checkarea([points(:,i) points(:,j)])
            dis(i,j)=norm(points(:,j)-points(:,i));
            dis(j,i)=dis(i,j);
        else
            dis(i,j)=inf;
            dis(j,i)=dis(i,j);
        end
    end
end

%% D algorithm
pre=zeros(1,n);
mindis=ones(1,n)*inf;
mindis(1,1)=0;
visited=zeros(1,n);
pseq=n;
for i=1:n
    dmin=inf;
    dnum=0;
    for j=1:n
        if dmin>mindis(1,j) && visited(1,j)==0
            dmin=mindis(1,j);
            dnum=j;
        end
    end
    if dnum==0
        disp("D algorithm error1!")
        return 
    end
    visited(1,dnum)=1;
    if dnum==n
        break
    end
    for j=1:n
        if visited(1,j)==0
            if mindis(1,dnum)+dis(dnum,j)<mindis(1,j)
                mindis(1,j)=mindis(1,dnum)+dis(dnum,j);
                pre(1,j)=dnum;
            end
        end
    end
end
i=n;
while i~=1
   pseq=[pre(1,i) pseq];
   i=pre(1,i);
end
if pseq(1,1)~=1
    disp("D algorithm error2!")
    return
end
%% plot
for i=1:length(pseq)
    x(i)=points(1,pseq(1,i));
    y(i)=points(2,pseq(1,i));
end
plot(x,y,'g-',"Linewidth",2)
hold on
plot(src(1,1),src(2,1),'ro',"Markersize",10,'MarkerFaceColor','r')
plot(dst(1,1),dst(2,1),'bo',"Markersize",10,'MarkerFaceColor','b')
rectangle('Position',[obs(1,1)-radius(1,1),obs(2,1)-radius(1,1),radius(1,1)*2,radius(1,1)*2],'Curvature',[1,1],'FaceColor',[0 0 0])
rectangle('Position',[obs(1,2)-radius(1,2),obs(2,2)-radius(1,2),radius(1,2)*2,radius(1,2)*2],'Curvature',[1,1],'FaceColor',[0 0 0])
axis equal