How to remove lines that are on obstacles PRM example

Question

I have the following code to generate a PRM map that will be used for A* application. There exist 2 problems with the code

1. It keeps the blue lines representing the original PRM where lines can cross over obstacles. I don't want to keep the blue lines but I couldn't find the way to remove them.
2. The green lines are going over obstacles even though they shouldn't

The code is as follows

clc;
clear all;
close all;


seed = 123512;
rng(seed);

xaxis = 100;
yaxis = 100;

obstacles = false(xaxis,yaxis);
[X,Y] = meshgrid(1:xaxis,1:yaxis);

obstacles(50:75,50:75) = true;
obstacles(25:35,30:40) = true;
obstacles(25:35,60:80) = true;

figure;
imshow(~obstacles,"InitialMagnification",1000);
axis([0 xaxis 0 yaxis]);
axis xy;
axis on;

%PRM PARAMETERS
max_nodes_connect = 4;
max_connect_len = 40;
segments = 1;
max_nodes_grid = 30;
skipped = 0;

%PRM ALGO
nodes = 0; %Counter
map = zeros(size(obstacles)); %generate map
map(obstacles) = 1; %put the obstacles
Graph_connections = Inf(max_nodes_grid,max_nodes_connect + 1); %rows = # nodes cols = ID and neighbors 

while (nodes < max_nodes_grid)
    node_x = randi(xaxis);
    node_y = randi(yaxis);

    if(map(node_y,node_x)==1 || map(node_y,node_x)==2)
        continue;
    end

    nodes = nodes + 1; %a valid node generated
    map(node_y,node_x) = 2; %2 means there exists a node at that location

    hold on 
    scatter(node_x,node_y,"red","filled")
    
    %NODES TO CONNECT
    nodes_to_connect = [];
    distances = [];

    for i= 1:numel(Graph_connections(:,1))
        if(Graph_connections(i,1)==Inf)
            break
        end
        [row,col] = ind2sub(size(map),Graph_connections(i,1));

        %Check if within range
        if(norm([node_y,node_x]-[row,col])>max_connect_len)
            continue;
        end

        line_on_obstacle = check_obstacle(map,node_x,node_y,row,col);
        %Check if obstacle thru line HAS TO BE WRITTEN
        if(line_on_obstacle)
            disp("Check Obstacle: " + line_on_obstacle);
            skipped = skipped + 1;
            continue;
        end

        nodes_to_connect = [nodes_to_connect, Graph_connections(i,1)];
        distances = [distances; [Graph_connections(i,1),norm([node_y,node_x]-[row,col])]];
    end
        Graph_connections(nodes,1) = sub2ind(size(map),node_y,node_x);
        if(size(distances)>0)
            sorted_distances = sortrows(distances,2);
            for i = 1:min(max_nodes_connect,size(sorted_distances,1))
                Graph_connections(nodes,i+1) = sorted_distances(i,1);
                [row,col] = ind2sub(size(map),sorted_distances(i,1));
                if(line_on_obstacle==false)
                    disp("Line is not on obstacle")
                    hold on
                    plot([node_x,col],[node_y,row],"green","LineWidth",1.5);
                    continue;                    
                else
                    disp("Line is on obstacle: " + [node_x,col] + " " + [node_y,row]);
                    break;
                end
            end
            disp("==========================")
        end
end

function on_line = check_obstacle(map,node_x,node_y,row,col)
    on_line = 0;
    my_line = line([node_x,col],[node_y,row]);
    line_spacing = max(abs(my_line.XData(1) - my_line.XData(2))+1,abs(my_line.XData(1) - my_line.XData(2))+1);
    x_coordinates_line = round(linspace(my_line.XData(1),my_line.XData(2),line_spacing));
    y_coordinates_line = round(linspace(my_line.YData(1),my_line.YData(2),line_spacing));
    for i = 1:line_spacing
        if(map(x_coordinates_line(i),y_coordinates_line(i))==1)
            disp("ON OBSTACLE: " + x_coordinates_line(i) + " " + y_coordinates_line(i));
            on_line = true;
            break;
        end
    end
end

The check_obstacle function is used to check if the points on the line are in the boundaries of obstacles. What am I missing here?

Answer 1

close all;clear all;clc
format short

nx=100;ny=100;     % grid size
[X,Y]=meshgrid(1:nx,1:ny);

Nobst=3      % amount obstacles
Nnet=30      % amount net points
Dmax=100     % net segment max length

% OBSTACLES
% define obstacles. 
% Following are as defined in question
P1=[50 50; 75 50; 75 75; 50 75; 50 50];
P2=[25 30; 25 40; 35 40; 35 30; 25 30];
P3=[25 60; 25 80; 35 80;35 60;25 60];

% obstacle points all in one array
Pobst=[P1(:);P2(:);P3(:)];
Pobst=reshape(Pobst,[size(P1,1),size(P1,2),Nobst]);

% plot obstacles
hp=[]
figure(1)
ax=gca
hp=patch(squeeze(Pobst(:,1,:)),squeeze(Pobst(:,2,:)),[.5 .5 .5])
axis([1 nx 1 ny]);grid on
hp.EdgeAlpha=0;
ax.DataAspectRatio=[1 1 1]
hold(ax,'on')

% obstacle segments list : [x1 y1 x2 y2 d(X1,Y1)]

Lobst1=[]
for k=2:1:size(P1,1)
    Lobst1=[Lobst1; [P1(k-1,:) P1(k,:) ((P1(k-1,1)-P1(k,1))^2+(P1(k-1,2)-P1(k,2))^2)^.5]];
end
Lobst2=[]
for k=2:1:size(P2,1)
    Lobst2=[Lobst2; [P2(k-1,:) P2(k,:) ((P2(k-1,1)-P2(k,1))^2+(P2(k-1,2)-P2(k,2))^2)^.5]];
end
Lobst3=[]
for k=2:1:size(P3,1)
    Lobst3=[Lobst3; [P3(k-1,:) P3(k,:) ((P3(k-1,1)-P3(k,1))^2+(P3(k-1,2)-P3(k,2))^2)^.5]];
end
Lobst=[Lobst1;Lobst2;Lobst3]

%% NETWORK
% all grid points outside obstacles
[in1,on1]=inpolygon(X(:),Y(:),P1(:,1),P1(:,2));
[in2,on2]=inpolygon(X(:),Y(:),P2(:,1),P2(:,2));
[in3,on3]=inpolygon(X(:),Y(:),P3(:,1),P3(:,2));

Xout=X(~in1 & ~in2 & ~in3);Yout=Y(~in1 & ~in2 & ~in3);
% plot(ax,Xout,Yout,'og','LineStyle','none')  % check

% choose Nnet points outside obstacles
nP2=randi([1 numel(Xout)],Nnet,1);
Pnet=[Xout(nP2) Yout(nP2)];

plot(ax,Pnet(:,1),Pnet(:,2),'or','LineStyle','none')

% net segments list [x1 y1 x2 y2 d(X2,Y2) 0/1] 6th column [0 1] 1: draw 0: do not draw
nLnet=nchoosek([1:size(Pnet,1)],2);
Lnet=[Pnet(nLnet(:,1),1) Pnet(nLnet(:,1),2) ...   % segment 1st point
         Pnet(nLnet(:,2),1) Pnet(nLnet(:,2),2) ...    % segment 2nd point
         ((Pnet(nLnet(:,1),1)-Pnet(nLnet(:,1),2)).^2+(Pnet(nLnet(:,2),1)+Pnet(nLnet(:,2),2)).^2).^.5 ... % segment length
         ones(size(nLnet,1),1)];   

% check all net links are plotted
for k=1:1:size(Lnet,1)
    plot(ax,[Lnet(k,1) Lnet(k,3)],[Lnet(k,2) Lnet(k,4)],'b');
    hold(ax,'on')
end

% remove segments longer than Dmax
Lnet=sortrows(Lnet,5,'descend');
[~,n1]=max(Lnet(:,5)<=Dmax);
Lnet(1:n1-1,:)=[];
for k=1:1:size(Lnet,1)
    plot(ax,[Lnet(k,1) Lnet(k,3)],[Lnet(k,2) Lnet(k,4)],'r');
    hold(ax,'on')
end

%%

Redrawing and NOT removing net segments longer than Dmax

close all
hp=[]
figure(1)
ax=gca
hp=patch(squeeze(Pobst(:,1,:)),squeeze(Pobst(:,2,:)),[.5 .5 .5])
axis([1 nx 1 ny]);grid on
hp.EdgeAlpha=0;
ax.DataAspectRatio=[1 1 1]
hold(ax,'on')
plot(ax,Pnet(:,1),Pnet(:,2),'or','LineStyle','none')
Lnet=[Pnet(nLnet(:,1),1) Pnet(nLnet(:,1),2) ...   % segment 1st point
         Pnet(nLnet(:,2),1) Pnet(nLnet(:,2),2) ...    % segment 2nd point
         ((Pnet(nLnet(:,1),1)-Pnet(nLnet(:,1),2)).^2+(Pnet(nLnet(:,2),1)+Pnet(nLnet(:,2),2)).^2).^.5 ... % segment length
         ones(size(nLnet,1),1)];

% check what pair segments intersect
for k2=1:1:size(Lnet,1)
    allclear=ones(1,size(Lobst,1));
%     allclear=zeros(1,size(Lobst,1));
    for k1=1:1:size(Lobst,1)
        
        % segments are contained in lines : check lines intersect
        x1o=Lobst(k1,1);y1o=Lobst(k1,2);x2o=Lobst(k1,3);y2o=Lobst(k1,4);
        x1n=Lnet(k2,1);y1n=Lnet(k2,2);x2n=Lnet(k2,3);y2n=Lnet(k2,4);
        
        x1=x1o;x2=x2o;y1=y1o;y2=y2o;
        x3=x1n;x4=x2n;y3=y1n;y4=y2n;
        
        % t1 : x parameter
        t1=((x1-x3)*(y3-y4)-(y1-y3)*(x3-x4))/((x1-x2)*(y3-y4)-(y1-y2)*(x3-x4));  
        xp=x1+t1*(x2-x1);  % xp : crossing x coordinage
        
        % u1 : y parameter
        u1=-((x1-x2)*(y1-y3)-(y1-y2)*(x1-x3))/((x1-x2)*(y3-y4)-(y1-y2)*(x3-x4));
        yp=y1+t1*(y2-y1);  % yp : crossing y coordinate

        % checks
        plot(ax,x1o,y1o,'c*');plot(ax,x2o,y2o,'c*');plot(ax,[x1o x2o],[y1o y2o],'c')
        plot(ax,x1n,y1n,'m*');plot(ax,x2n,y2n,'m*'); % plot(ax2,[x1n x2n],[y1n y2n],'m')
         
        
        m1o=(y2o-y1o)/(x2o-x1o);       % slopes
        m2n=(y2n-y1n)/(x2n-x1n) ; 
  
        if (t1>=0 && t1<=1 && u1>=0 && u1<=1) && ...
           (xp>=0 || xp<=nx || yp>=0 || yp<=ny)
                allclear(k1)=0;  % then do not plot this segment
        end

    end
    
    if sum(allclear)==size(Lobst,1)   %k2-th net segment hits no obstacles
        plot(ax,x1n,y1n,'m*');plot(ax,x2n,y2n,'m*');plot(ax,[x1n x2n],[y1n y2n],'m')
        elseif sum(allclear)~=size(Lobst,1)
            Lnet(k2,end)=0;
    end
     
end

Comments

1.- Note I have added format short because with format long there is decimal accretion right at the bottom of a few intersection points, that believe it or not, cause some false results.

2.- I like Torsen's explanation to find line segment intersections available here.

3.- There are faster ways to implement the main loop, but I make it go through all net-segment vs obstacle-element in case you may need it this way, to for instance count hits.

4.- There's also room for improvement in the way Lobst is generated.

5.- These lines

x1=x1o;x2=x2o;y1=y1o;y2=y2o;
x3=x1n;x4=x2n;y3=y1n;y4=y2n;

it's just an easy way to plug in formulation to already written lines, reducing amount variables is also left for next version.