Shortest Path Dynamic Program

Here's a short program I wrote for my Operations Research class. It computes the shortest path from anywhere in a grid to an endpoint. The inputs are C,c2,M,N,sp,L where C is an MxN grid where every point should be some large integer (L)  except for a group of points on the boundary, which should be zero and represent the endpoint. c2 is a secondary cost matrix size MxN which stores obstacles. Every point should be zero except for the points that represent obstacles which should again be a large integer (L). sp should be a 2x1 vector that contains the coordinates of the starting point. I used a helper function function [C,c2,M,N,sp,L]=conditions() to call my inputs, and used L=20,000 for the large integer. Here's a figure showing the optimal path:

And here's another:

Here's a surf graph that shows how the program computes the shortest distance to any point. It uses dynamic programming to compute the distance from each point to the end zone, by starting from the points that border the end zone and working outward. Because the algorithm can't see around corners very well, it is iterated multiple times, seeing one square further each time, until the whole grid is reached:

Here's the code. The main function is called robot and the helper is called conditions. Run time is about 30s:

Note: the program assumes a border surrounding the grid of width 1, which is why the for  loop runs from M-1:2 and N-1:2. 










function []=Robot(C,c2,M,N,sp,L)

% Computes the shortest distance and route from any point to an end state

% by dynamic programming, runtime is ~20s with 200x150 grid

% This function takes inputs C,c2,M,N,sp 

% C-> MxN grid with implicit boundary around edges

% the value of C should be some large integer (20000 in this case)

% everywhere except at the desired end state, which should be 0

% c2-> secondary cost, MxN grid that is 0 everywhere and some large integer (20000)

% wherever there is an obstacle

% sp-> starting point on the MxN grid

clear all

% I call the given conditions, to use a different starting point redefine

% sp after conditions line

[C,c2,M,N,sp,L]=conditions();

count=0;

a=1;

b=0;

% This will iteratively loop the grid C to compute the shortest distance

% it stops when the # of points in C>20000 stops changing

while a~=b

a=sum(sum(C>=L));

% This computes the shortest distance to the end state by looking at each

% of the 8 directions, it stores the distance in C and the direction in D

% The boundary around the grid is implied by the bounds of the for loops

% It works through the entire grid backwords from the end state

for i=M-1:-1:2

    for j=N-1:-1:2

       m(1)=C(i+1,j)+c2(i+1,j)+1;

       m(2)=C(i,j+1)+c2(i,j+1)+1;

       m(3)=C(i-1,j)+c2(i-1,j)+1;

       m(4)=C(i,j-1)+c2(i,j-1)+1;

       m(5)=C(i+1,j+1)+c2(i+1,j+1)+sqrt(2);

       m(6)=C(i+1,j-1)+c2(i+1,j-1)+sqrt(2);

       m(7)=C(i-1,j-1)+c2(i-1,j-1)+sqrt(2);

       m(8)=C(i-1,j+1)+c2(i-1,j+1)+sqrt(2);

       [Y,I]=min(m); 

       C(i,j)=Y;

       D(i,j)=I;

    end

end

b=sum(sum(C>=20000))

count=count+1

end

% This part of the code uses the matrix D, which stores the direction of

% the shortest path from any point in the grid, and computes and plots

% the path by starting at sp

G=zeros(M,N);

G(sp(1),sp(2))=1;

while C(sp(1),sp(2))~=0

    if D(sp(1),sp(2))==1

        sp=sp+[1,0];

    elseif D(sp(1),sp(2))==2

        sp=sp+[0,1];

    elseif D(sp(1),sp(2))==3

        sp=sp+[-1,0];

    elseif D(sp(1),sp(2))==4

        sp=sp+[0,-1];

    elseif D(sp(1),sp(2))==5

        sp=sp+[1,1];

    elseif D(sp(1),sp(2))==6

        sp=sp+[1,-1];

    elseif D(sp(1),sp(2))==7

        sp=sp+[-1,-1];

    else 

        sp=sp+[-1,1];

    end

    G(sp(1),sp(2))=1;

end

figure(1)

% This plots the trajectory

hold on

for i=1:M

    for j=1:N

        if G(i,j)==1

            plot(i,j,'.')

        end

    end

end

figure(2)

surf(C','LineStyle','none')

view(2)

colorbar

axis tight

caxis([0 500])

function [C,c2,M,N,sp,L]=conditions();

clear;

M=200;

N=150;

%%% starting point sp

sp = [130,40];

L=20000;    % Large number L instead of infinity

%%% Terminal C(s) function %%%

C=L*ones(M,N);

C(M,N-4:N)=0;

%%% obstacle condition = c2(s)%%%

c2 = zeros(M,N);

c2(M-30:M,129:130)=L;

c2(40:42,30:N)=L;

c2(10:30,120:140)=L;

c2(60:100,40:100)=L;

c2(140:142,1:50)=L;

c2(160:162,20:N)=L;

c2(162:190,110:112)=L;

c2(170:200,90:92)=L;

c2(162:190,70:72)=L;

c2(100:140,45:47)=L;

figure(1);

hold on;

grid on;

% plot obstacle

for i=1:M;

    for j=1:N;

        if c2(i,j) ~= 0;

            plot(i,j,'gs','linewidth',1)

        end

    end

end

% plot starting point with black dot

plot(sp(1),sp(2),'ko','linewidth',7);

% plot door (exit point) with red squares

plot(M,N-4:N,'rs','linewidth',3);

% plot wall

line([1,M],[1,1],'color','b','linewidth',2);

line([1,M],[N,N],'color','b','linewidth',2);

line([1,1],[1,N],'color','b','linewidth',2);

line([M,M],[1,N],'color','b','linewidth',2);

xlim([0 M+1])

ylim([0 N+1])