Automated adjecancy table generation for a mesh graph and finding the shortest path using the BFS algorithm on this mesh graph

Consider the following grid and its corresponding graph.

The following program will create the graph in an automated way. You can give the start and final goal nodes and the program will return the shortest path between these two nodes.

Familiarize yourself with the following Matlab commands:

inpolygon: https://www.mathworks.com/help/matlab/ref/inpolygon.html

This program is generalizable to any mesh, the only parts you need to specify are the obstacles (O), number of the segments in each side of the mesh (number_of_horiz_segment,number_of_vertical_segment), and start and final nodes.

close all
clear all
%this is the obstacles
O={[1 1;   %vertex 1
    2 1;   %vertex 2
    2 2;    %vertex 3
    1 2;    %vertex 3
    1 1],
    [3 1;   %vertex 1
    4 1;   %vertex 2
    4 2 ;   %vertex 3
    3 2;    %vertex 3
    3 1]};   %repeating vertex 1
number_of_obstcaqles=length(O);
k=1;
number_of_horiz_segment=4;
number_of_vertical_segment=5;
A=zeros(number_of_vertical_segment,number_of_horiz_segment);% this matrix keeps track of the lable of the nodes at each mesh cell
for i=1:number_of_vertical_segment
    for j=1:number_of_horiz_segment
        q=[(j-0.5) (i-0.5)];
        for n_o=1:number_of_obstcaqles
            in(n_o) = inpolygon(q(1,1),q(1,2),O{n_o}(:,1),O{n_o}(:,2)); %returns in indicating if the query point specified by q(1,1) and q(1,2) is insider the polygon degined by vertices
        end
        if max(in)~=1
            N{k}(1,:)=q;
            A(i,j)=k;
            k=k+1;
        end
    end
end
number_of_nodes_in_graph=k-1;

Now we have the node locations, next lets define the adjacency table automatically.

Note that the neighbors of node at mesh cell (i,j) are nodes at (i-1,j), (i+1,j), (i, j-1) and (i,j+1). For boundary points you have to exclude cells that end up outside the mesh.

Adj_table={};
for i=1:number_of_vertical_segment
    for j=1:number_of_horiz_segment
        l=1;
        if A(i,j)~=0  %only looks for mesh cells that have a node lable
            if i-1>0 %makes sure that the neighbor is in the mesh cell
                if A(i-1,j)~=0  %makes sure that the neighbor is not inside an obstacle
                    Adj_table{A(i,j)}(1,l)=A(i-1,j);
                    l=l+1;
                end
            end
            if i+1<=number_of_vertical_segment %makes sure that the neighbor is in the mesh cell
                if A(i+1,j)~=0   %makes sure that the neighbor is not inside an obstacle
                    Adj_table{A(i,j)}(1,l)=A(i+1,j);
                    l=l+1;
                end
            end
            if j-1>0  %makes sure that the neighbor is in the mesh cell
                if A(i,j-1)~=0  %makes sure that the neighbor is not inside an obstacle
                    Adj_table{A(i,j)}(1,l)=A(i,j-1);
                    l=l+1;
                end
            end
            if j+1<=number_of_horiz_segment  %makes sure that the neighbor is in the mesh cell
                if A(i,j+1)~=0  %makes sure that the neighbor is not inside an obstacle
                    Adj_table{A(i,j)}(1,l)=A(i,j+1);
                    l=l+1;
                end
            end
        end        
    end
end

Now let's plot the graph using the node location and the adjacency table

for k=1:number_of_nodes_in_graph

plot(N{k}(1,1),N{k}(1,2),'o','MarkerSize',30,'Color','b')

hold on

neibors_of_node_k=Adj_table{k}(1,:);

for l=1:length(neibors_of_node_k)

line([N{k}(1,1), N{neibors_of_node_k(l)}(1,1)],[N{k}(1,2), N{neibors_of_node_k(l)}(1,2)],'Color','b')

end

clear neibors_of_node_k;

text(N{k}(1,1)-0.02,N{k}(1,2),num2str(k),'FontSize',20)

end

set(gca,'visible','off') %turn off matlab axis

Finding the shortest path from the start to goal point to the graph point using the BFS algorithm

Now you can feed the adjacency table to the BFS algorithm along with the start and goal node to find the shortest path between the start and goal cells. Scroll down to see the BFS_search function.

v_start=2;
v_goal=15;
%the shortest path
path=BFS_search(number_of_nodes_in_graph,Adj_table,v_start,v_goal)
path = 1×6
     2     1     5     7    11    15

Now let's plot the path on the graph in red color.

for k=1:length(path)-1

line([N{path(k)}(1,1),N{path(k+1)}(1,1)],[N{path(k)}(1,2),N{path(k+1)}(1,2)],'Color','r')

end

BFS algorithm

This pseudocode is taken from https://ucsb.app.box.com/v/LecturesRobotics

function path=BFS_search(n,Adj_table,v_start,v_goal)
%n is the number of the nodes in the graph
%Adj_table adjacency table of the graph (assumes this table is defined as a matlab cell)
%v_start: start node
%v_goal: goal node
for i=1:n   %(line 1 on pseudo code)
    parent(i)=-1; %(line 2 on pseudo code) %-1 is equivalent of none, it means that node i does not have a parent
end
parent(v_start)=v_start; %(line 3 on pseudo code)
Q=[]; Q=[v_start Q]; %(line 4 on pseudo code)
while length(Q)~=0 %(line 5 on pseudo code)
    v=Q(end); Q(end)=[]; %(line 6 on pseudo code)
    for j=Adj_table{v} %(line 7 on pseudo code)
        if parent(j)==-1  %(line 8 on pseudo code)
            parent(j)=v; Q=[j Q];  %(line 9 on pseudo code)
        end
        if j==v_goal %(line 10 on pseudo code)
            path=extract_path(parent,v_goal); %(line 11 on pseudo code)
            Q=[]; break %(line 12 on pseudo code)
        end
    end  
end
end

Extract path

This pseudocode is taken from https://ucsb.app.box.com/v/LecturesRobotics

function P=extract_path(parent,v_goal)
P=[v_goal]; %(line 1 on pseudo code)
u=v_goal; %(line 2 on pseudo code)
while parent(u)~=u  %(line 3 on pseudo code)
    u=parent(u);  %(line 4 on pseudo code)
    P=[u P]; %(line 5 on pseudo code)
end
end

@ Solmaz Kia

April 28, 2023

Kia Cooeprative Systems Lab

University of California Irvine