first commit

addstartendpoint2ungraph.m

@@ -0,0 +1,38 @@
+function [extungraph,exnodelocation,exunedges ]=addstartendpoint2ungraph(map,undirectedGraph,nodelocation,unedges,startp,endp)
+% this function add two new nodes on undirected graoh.
+% not need to construct again, just add new connections.
+
+extungraph=undirectedGraph;
+exnodelocation=nodelocation;
+nnode=size(undirectedGraph,1);
+% new coming nodes take place at new 2 order.
+% it it has 100 node then 101 and 102 nodes are new adding node
+exnodelocation(nnode+1,:)=startp;
+exnodelocation(nnode+2,:)=endp;
+exunedges=unedges;
+
+for i=1:nnode
+    x1=[nodelocation(i,1);startp(1)];
+    y1=[nodelocation(i,2);startp(2)];
+    % if any nodes and first newcoming nodes have a connection or not.
+    [xi,yi] = polyxpoly(x1,y1,map.obsx,map.obsy);
+    % if there is not any intersection with obstacle
+    if length(xi)==0
+        % add this connection to graph
+        extungraph(nnode+1,i)=1;
+        extungraph(i,nnode+1)=1;
+        exunedges=[exunedges;i nnode+1;nnode+1 i];
+    end
+end
+for i=1:nnode+1    
+    x1=[exnodelocation(i,1);endp(1)];
+    y1=[exnodelocation(i,2);endp(2)];
+    % if any nodes and second newcoming nodes have a connection or not.
+    [xi,yi] = polyxpoly(x1,y1,map.obsx,map.obsy);
+     % if there is not any intersection with obstacle
+    if length(xi)==0
+        extungraph(nnode+2,i)=1;
+        extungraph(i,nnode+2)=1;
+        exunedges=[exunedges;i nnode+2;nnode+2 i];
+    end
+end

astar.m

@@ -0,0 +1,56 @@
+function [route] = astar2(exbigraph,exbiloc,startnode,endnode)
+
+graph=exbigraph;
+Loc=exbiloc;
+n=size(graph,1);
+Cost=inf(n,1);
+
+curr=startnode;
+Route=cell(n,1);
+Cost(curr)=0;
+Tabu=[curr];
+cRoute=[curr];
+
+
+% pre computed heuristic dist
+% every nodes euristic cost save on matirx
+for i=1:n
+    HCost(i,1)=costcal(Loc(endnode,:),Loc(i,:));
+end
+
+i=1;
+% if the endnode is found or all possibility is finished
+while i<n & curr ~=endnode    
+    i=i+1;
+    T=1:n;
+    T(Tabu)=[];  
+    % find all possibility from current node
+    I=find(graph(curr,T)==1);
+    % find all possibility's cost from current to concerned node
+    cost=costcal(Loc(curr,:),Loc(T(I),:)) + Cost(curr);
+    % find if calculated cost is less than the way before explored
+    J=find(Cost(T(I))>cost);
+    % if new calculated is less than before one then set less one
+    Cost(T(I(J)))=cost(J);  
+    % if new calculated way is less than the before one than set new route
+    % too
+    for j=1:length(J)
+        Route{T(I(J(j)))}=[cRoute T(I(J(j)))];        
+    end  
+    % find minimum of normal cost plus heuristic cost of possible node
+    [mn cr]=min(Cost(T)+HCost(T));
+    if isinf(mn)
+        i=n;
+    end
+    % set minimum total cost's node as current node
+    curr=T(cr);    
+    cRoute=Route{curr};
+    % add current node to tabu list to prevent loops
+    Tabu=[Tabu curr];
+end
+% if current node is equal endnode means route is found
+if curr ==endnode
+    route=cRoute;
+else % i not means there is no way
+    route=[];
+end

costcal.m

@@ -0,0 +1,14 @@
+function sc=costcal(a,b)
+tp=b-a;
+
+sc=sqrt(tp(:,1).^2+tp(:,2).^2);
+
+
+% n=size(b,1);
+% df=repmat(a,n,1)-b;
+% df(:,3)=abs(df(:,3));
+% I=find(df(:,3)>pi);
+% df(I,3)=2*pi-df(I,3);
+% c=df(:,1).*df(:,1)+df(:,2).*df(:,2);
+% sc=sqrt(sum(c,2))+df(:,3)+0.001;
+% %sc=ones(n,1);

dijkstra.m

@@ -0,0 +1,45 @@
+function [Route, Cost] = dijkstra(exbigraph,exbiloc,startnode)
+% calculate route for given graph and starting point.
+% this function finds all targets so you do not need to support end point
+
+graph=exbigraph;
+Loc=exbiloc;
+n=size(graph,1);
+Cost=inf(n,1);
+curr=startnode;
+Route=cell(n,1);
+Cost(curr)=0;
+Tabu=[curr];
+cRoute=[curr];
+i=1;
+% for number of nodes
+while i<n
+    i=i+1;
+    % take all possible node list
+    T=1:n;
+    % delete already visited node
+    T(Tabu)=[];   
+    % find connected node from current node which are not visited yet
+    I=find(graph(curr,T)==1);
+    % calculate connected nodes euclid distance from current node and add
+    % current nodes cost
+    cost=costcal(Loc(curr,:),Loc(T(I),:)) + Cost(curr);
+    % if new finding coset is less than old cost then replace
+    J=find(Cost(T(I))>cost);
+    Cost(T(I(J)))=cost(J);   
+    % update new comming node rote if the new cost is more efficient
+    for j=1:length(J)
+        Route{T(I(J(j)))}=[cRoute T(I(J(j)))];        
+    end  
+    % find minimum costed node which is not visited yet in this iteration
+    [mn cr]=min(Cost(T));
+    if isinf(mn)
+        i=n;
+    end
+    curr=T(cr);
+    % select selected route as forbitten route to not to select this nodes
+    % again
+    cRoute=Route{curr};
+    Tabu=[Tabu curr];
+end
+

drawRoute.m

@@ -0,0 +1,30 @@
+function drawRoute(method,startnode,endnode,exnodelocation,exnodIndex,exunedges,rt,Cost)
+% draw graph nodes and selected way.
+
+ns=size(exnodelocation,1);
+map_definition();
+hold on;
+
+title([ method '  Cost:' num2str(Cost)]);
+sn=exnodIndex(startnode);
+en=exnodIndex(endnode);
+plot(exnodelocation(1:ns,1),exnodelocation(1:ns,2),'b*');
+plot(exnodelocation(sn,1),exnodelocation(sn,2),'rx','markersize',10);
+plot(exnodelocation(en,1),exnodelocation(en,2),'rx','markersize',10);
+
+for i=1:2:size(exunedges,1)    
+    x1=exnodelocation(exunedges(i,1),1);
+    x2=exnodelocation(exunedges(i,2),1);   
+    y1=exnodelocation(exunedges(i,1),2);
+    y2=exnodelocation(exunedges(i,2),2);  
+    line([x1;x2],[y1;y2],'linewidth',0.1);    
+end
+
+for i=1:length(rt)-1
+    x1=exnodelocation(exnodIndex(rt(i)),1);
+    x2=exnodelocation(exnodIndex(rt(i+1)),1);   
+    y1=exnodelocation(exnodIndex(rt(i)),2);
+    y2=exnodelocation(exnodIndex(rt(i+1)),2);  
+    line([x1;x2],[y1;y2],'color','r','linewidth',2);
+end
+hold off

dynamicpathplanning.m

@@ -0,0 +1,32 @@
+function [parent, route, cost]=dynamicpathplanning(Graph,Loc,Ind,startnode,endnode)
+% calculate minimum costs path with recursive manner.
+
+route=[];
+cost=0;
+dist=[startnode 0];
+parent=[startnode nan];
+n=size(Graph,1);
+deep=0;
+% call main recursive function, this function will call itself and give us
+% route
+[ds dist parent ] = sp_dp(Graph, endnode, dist,parent,Loc,Ind,deep); 
+
+Route=[];
+Route=[endnode ];
+next=1;
+while Route(end)~=startnode && ~isnan(next)
+    curr=Route(end);
+    xx=find(parent(:,1)==curr);
+    next=parent(xx,2);
+    if ~isnan(next)        
+        cost=cost+costcal(Loc(Ind(curr),:),Loc(Ind(next),:));
+        Route=[Route next];
+    end
+end
+if Route(end)==startnode
+    route=Route;    
+else
+    route=[];
+    cost=inf;
+end
+

generate_node.m

@@ -0,0 +1,31 @@
+function [map, nodelocation]= generate_node(map,nnode)
+
+% merge vertices of all obstacle
+obsx=map.pgx{1};
+obsy=map.pgy{1};
+for i=2:length(map.pgx)
+    obsx=[obsx NaN map.pgx{i}];
+    obsy=[obsy NaN map.pgy{i}];
+end
+map.obsx=obsx;
+map.obsy=obsy; 
+% set nodelocation to all zero
+nodelocation=zeros(nnode,2);
+% generate nodes
+n=1;
+while (n<=nnode)
+    % generate random two number in range of map's border
+    rx=rand* (map.xrange(2)-map.xrange(1)) + map.xrange(1);
+    ry=rand* (map.yrange(2)-map.yrange(1)) + map.yrange(1);
+    state=0;
+    % if this node is not inside any obstacle
+    if ~inpolygon(rx,ry,obsx,obsy)
+        % add this location to nodelocation list
+        nodelocation(n,1)=rx;
+        nodelocation(n,2)=ry;
+        n=n+1;
+    end
+end
+hold on;
+plot(nodelocation(:,1),nodelocation(:,2),'r*');
+hold off;

generate_undirected_graph.m

@@ -0,0 +1,36 @@
+function [undirectedGraph,unedges]=generate_undirected_graph(map,nodelocation)
+% takes map and produce undirected map and its adge set
+
+% take number of node
+nnode=size(nodelocation,1);
+% set graph and edges as zero
+undirectedGraph=zeros(nnode,nnode);
+unedges=[];
+
+%generate undirected graph
+
+for i=1:nnode-1 
+    for j=i+1:nnode
+        % take each pair of node^s location x and y
+        x1=[nodelocation(i,1);nodelocation(j,1)];
+        y1=[nodelocation(i,2);nodelocation(j,2)];  
+        % check this two nodes intersect any obstacle or not
+        [xi,yi] = polyxpoly(x1,y1,map.obsx,map.obsy);
+        % if there is not any intersection
+        if length(xi)==0
+            % connect this two nodes in graph
+            undirectedGraph(i,j)=1;
+            undirectedGraph(j,i)=1;
+            % add this connection to adge set
+            unedges=[unedges;i j;j i];
+        end
+    end
+end
+% plot graph
+hold on;
+for i=1:2:size(unedges,1)
+    x1=[nodelocation(unedges(i,1),1);nodelocation(unedges(i,2),1)];
+    y1=[nodelocation(unedges(i,1),2);nodelocation(unedges(i,2),2)]; 
+    line(x1,y1);
+end
+hold off;

main.m

@@ -0,0 +1,56 @@
+clear
+close all
+clc
+
+
+% number of nodes 
+ns=100;
+map=map_definition();
+% 
+% generate random nodes
+[map, nodelocation]= generate_node(map,ns);
+
+
+% create undirected graph and its edges
+[undirectedGraph,unedges]=generate_undirected_graph(map,nodelocation);
+
+
+% define start and end point of simulation
+startp=[5, 29];
+endp=[29, 20];
+
+% add start and end location as a new 2 nodes in undirected map.
+% n+1 th node is start point, n+2th node is end point
+[extungraph,exnodelocation,exunedges ]=addstartendpoint2ungraph(map,undirectedGraph,nodelocation,unedges,startp,endp);
+exundnodIndex=[1:ns+2];
+snodeund=ns+1;
+enodeund=ns+2;
+
+
+% optimal path with dijkstra on un-directional map
+[Route Cost] = dijkstra(extungraph,exnodelocation,snodeund);
+rt=Route{enodeund};
+
+dijkstra_route=exnodelocation(rt,:);
+cost=pathcost(dijkstra_route);
+drawRoute('dijkstra on undirected map',snodeund,enodeund,exnodelocation,exundnodIndex,exunedges,rt,cost);
+
+
+
+% optimal path with astar on un-directional map
+[Route] = astar(extungraph,exnodelocation,snodeund,enodeund);
+astar_route=exnodelocation(Route,:);
+cost=pathcost(astar_route);
+drawRoute('Astar on undirected map',snodeund,enodeund,exnodelocation,exundnodIndex,exunedges,Route,cost);
+
+
+% optimal path with dynamic programming on un-directional map
+[parent, Route]=dynamicpathplanning(extungraph,exnodelocation,exundnodIndex,snodeund,enodeund);
+dpundirected_route=exnodelocation(Route,:);
+cost=pathcost(dpundirected_route);
+drawRoute('Dynamic Programming on undirected map',snodeund,enodeund,exnodelocation,exundnodIndex,exunedges,Route,cost);
+
+
+
+
+

map_definition.m

@@ -0,0 +1,45 @@
+function map=map_definition()
+
+Npoly=13;
+map.pgx{1}=[2 8.5 8.5 4 2 2 1 1 2 4 2];
+map.pgy{1}=[8 10 1 3 3 1 1 6 6 5 8];
+map.pgx{2}=[2 8 2 15 2 1 1 2 2];
+map.pgy{2}=[10 16 22 15.5 9 10 16 16 10];
+map.pgx{3}=[0 0 5 0];
+map.pgy{3}=[25 30 30 25];
+map.pgx{4}=[7 7 10 10 13 13 11 7];
+map.pgy{4}=[23 26 29 25 26 23 21 23];
+map.pgx{5}=[13 17 15 13];
+map.pgy{5}=[30 30 25  30];
+map.pgx{6}=[17 17 30 30 17];
+map.pgy{6}=[23 27 27 23 23];
+map.pgx{7}=[12 14 15 16 29 23 27 19 12];
+map.pgy{7}=[20 21 23 21.3 22 10 21.3 14 20];
+map.pgx{8}=[10 15 17.5 10 10];
+map.pgy{8}=[8 14 10 0 8];
+map.pgx{9}=[19 24 19 19];
+map.pgy{9}=[12.5 17 1 12.5];
+map.pgx{10}=[21 30 26 21];
+map.pgy{10}=[3 16 1 3];
+map.pgx{11}=[4 7 8 6 5 10 7 4 4];
+map.pgy{11}=[25 30 30 26 23 20 21 23 25];
+map.pgx{12}=[0 0 4 5 4 3 3 0];
+map.pgy{12}=[17 18 18 16 14 14 17 17];
+map.pgx{13}=[3 3 4 4 3];
+map.pgy{13}=[0 2 2 0 0];
+map.xrange=[0 30];
+map.yrange=[0 30];
+
+figure;
+plot([0 30 30 0 0],[0 0 30 30 0]);
+hold on
+
+excluded_area=0;
+
+for ii=1:Npoly    
+    excluded_area=excluded_area+polyarea(map.pgx{ii},map.pgy{ii});
+    fill(map.pgx{ii},map.pgy{ii},'g')
+end
+hold off
+
+