diff --git a/createVisibilityGraph.m b/createVisibilityGraph.m
new file mode 100644
index 0000000..2d71c50
--- /dev/null
+++ b/createVisibilityGraph.m
@@ -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
+
+
diff --git a/dijkstra.m b/dijkstra.m
new file mode 100644
index 0000000..03a919e
--- /dev/null
+++ b/dijkstra.m
@@ -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');
+