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');
+