nicolas.traglia
/
AdvancedRoboticsT5
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

+ Create functions to manage graph

This commit is contained in:
Guillaume GIBERT 2021-11-29 22:29:41 +01:00
parent 749425e5ed
commit e7b06aba37
2 changed files with 104 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
createVisibilityGraph.m Normal file
View File

@ -0,0 +1,34 @@
function [nbNodes, visibilityGraph] = createVisibilityGraph(connectionMatrix, points2D)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%function [nbNodes, visibilityGraph] = createVisibilityGraph(connectionMatrix, points2D)
%
% Task: Create a visibility graph from a connection matrix and a set of 2D points
%
% Inputs:
% -connectionMatrix: matrix of connection if cell is equal to 1 there is an edge between the corresponding points, cell is 0 otherwise
% -points2D: coordinates of the vertices of the graph
%
% Outputs:
% -nbNodes: the number of nodes of this graph
% -visibilityGraph: a matrix containing the distance between connected nodes
% (NaN refers to not connected nodes)
% The matrix has a size of (nbNodes+2)x(nbNodes+2)
%
% Guillaume Gibert (guillaume.gibert@ecam.fr)
% 19/03/2021
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbNodes = size(points2D,1)-2;
visibilityGraph = NaN(nbNodes+2, nbNodes+2);
for l_row=1:size(connectionMatrix,1)
for l_col=1:size(connectionMatrix,2)
if (connectionMatrix(l_row, l_col) == 1)
% computes the distance between the 2 points
distance = sqrt( (points2D(l_row,1)-points2D(l_col,1))^2 + (points2D(l_row,2)-points2D(l_col,2))^2);
visibilityGraph(l_row, l_col) =distance;
end
end
end

70
dijkstra.m Normal file
View File

@ -0,0 +1,70 @@
function [distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNodes, visibilityGraph)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%function [distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNodes, visibilityGraph)
%
% Task: Perform the Dijkstra algorithm on a given visibility graph
%
% Inputs:
% -nbNodes: number of nodes of the graph excluding the starting and goal points
% -visibilityGraph: a matrix containing the distance between connected nodes
% (NaN refers to not connected nodes)
% The matrix has a size of (nbNodes+2)x(nbNodes+2)
%
% Outputs:
% - distanceToNode: distance between the current node and its parent
% - parentOfNode: index of the parent node for each node
% - nodeTrajectory: best trajectory
%
% Guillaume Gibert (guillaume.gibert@ecam.fr)
% 17/03/2021
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
constantLargeDitance=10000;
visitedNodes = zeros(1, nbNodes+2);
distanceToNode = constantLargeDitance*ones(1, nbNodes+2);
distanceToNode(1) = 0;
parentOfNode = zeros(1, nbNodes+2);
fprintf('##Starting Dijkstra''s algorithm...\n')
while (sum(visitedNodes(:)==0))
thresholdDistance = constantLargeDitance+1;
for l_node=1:nbNodes+2
%l_node
if (visitedNodes(l_node)==0 && distanceToNode(l_node) < thresholdDistance)
minIndex = l_node;
thresholdDistance = distanceToNode(l_node);
end
end
fprintf('-->Visiting N%d\n', minIndex-1)
visitedNodes(minIndex) = 1;
for l_node=1:nbNodes+2
%l_node
if (l_node~=minIndex && ~isnan(visibilityGraph(minIndex, l_node)))
distance = distanceToNode(minIndex) + visibilityGraph(minIndex,l_node);
if (distance < distanceToNode(l_node))
distanceToNode(l_node) = distance;
parentOfNode(l_node) = minIndex;
end
end
end
end
fprintf('##Dijkstra''s algorithm is done!\n')
fprintf('##Results\n')
fprintf('Minimal distance to target: %d\n', distanceToNode(nbNodes+2))
nodeIndex = nbNodes+2;
nodeTrajectory = [];
while(nodeIndex~=1)
nodeIndex = parentOfNode(nodeIndex);
nodeTrajectory = [nodeTrajectory nodeIndex];
end
fprintf('S-->');
for l_node=2:length(nodeTrajectory)
fprintf('N%d-->', nodeTrajectory(length(nodeTrajectory)-(l_node-1))-1);
end
fprintf('G\n');
fprintf('########\n');