Implemented Dijkstra algorithm (#18)

* New Dijkstra class

* Dijkstra algorithm implemented

* Removing unnecessary project files

---------

Co-authored-by: Michał Święciło <michal_00-00@o2.pl>

SouvlakMVP/SouvlakMVP.sln

@@ -3,7 +3,7 @@ Microsoft Visual Studio Solution File, Format Version 12.00
 # Visual Studio Version 17
 VisualStudioVersion = 17.4.33403.182
 MinimumVisualStudioVersion = 10.0.40219.1
-Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "SouvlakMVP", "SouvlakMVP\SouvlakMVP.csproj", "{80C34D17-2188-4541-BB3C-8DBBB3CF0907}"
+Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "SouvlakMVP", "SouvlakMVP\SouvlakMVP.csproj", "{80C34D17-2188-4541-BB3C-8DBBB3CF0907}"
 EndProject
 Global
 	GlobalSection(SolutionConfigurationPlatforms) = preSolution

SouvlakMVP/SouvlakMVP/Dijkstra.cs

@@ -0,0 +1,95 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+using System.Numerics;
+
+using indexT = System.Int32;
+using distanceT = System.Single;
+using System.Runtime.InteropServices;
+
+namespace SouvlakMVP;
+
+internal class Dijkstra
+{
+    public static (List<indexT>, distanceT) FindShortestPath(Map graph, indexT startVertex, indexT endVertex)
+    {
+        int verticesN = graph.Count();
+
+        // Array containing the minimum costs to reach each vertex from the starting vertex
+        distanceT[] minCostToVertex = new distanceT[verticesN];
+        // Array containing the preceding vertices on the path from the starting vertex
+        indexT?[] precedingVertices = new indexT?[verticesN];
+
+        // Working lists of vertices
+        List<indexT> verticesToProcess = new List<indexT>();
+        List<indexT> processedVertices = new List<indexT>();
+
+        // Filing out verticesToProcess and minCostToVertex
+        for (int i = 0; i < verticesN; i++)
+        {
+            minCostToVertex[i] = (i == startVertex) ? 0f : distanceT.MaxValue;
+            verticesToProcess.Add(i);
+        }
+
+        while (verticesToProcess.Count > 0)
+        {
+            indexT processedVertex = indexT.MaxValue;
+            distanceT tempMinCost = distanceT.MaxValue;
+
+            // Finding the vertex to process (with the minimum cost to reach from the starting vertex)
+            foreach (indexT vertex in verticesToProcess)
+            {
+                if (minCostToVertex[vertex] < tempMinCost)
+                {
+                    tempMinCost = minCostToVertex[vertex];
+                    processedVertex = vertex;
+                }
+            }
+
+            // Moving the current vertex to processed vertices
+            if (verticesToProcess.Remove(processedVertex))
+            {
+                processedVertices.Add(processedVertex);
+            }
+
+            // Reviewing all neighbors of the relocated vertex
+            for (indexT i = 0; i < graph[processedVertex].roads.Count(); i++)
+            {
+                Map.Road edge= graph[processedVertex].roads[i];
+                indexT nextVertex = edge.targetIdx;
+                distanceT edgeCost = edge.distance;
+
+                // Check if neighbour has not yet been processed
+                if (verticesToProcess.Contains(nextVertex))
+                {
+                    distanceT CostToVertex = minCostToVertex[processedVertex] + edgeCost;
+
+                    // Check the new cost and update if it is smaller than the old one
+                    if (minCostToVertex[nextVertex] > CostToVertex)
+                    {
+                        minCostToVertex[nextVertex] = CostToVertex;
+                        precedingVertices[nextVertex] = processedVertex;
+                    }
+                }
+            }
+        }
+
+        indexT? tempVertex = endVertex;
+        List<indexT> shortestPathFromEnd = new List<indexT>();
+
+        // Create the shortest path
+        while (tempVertex != null)
+        {
+            shortestPathFromEnd.Add(tempVertex.Value);
+            tempVertex = precedingVertices[tempVertex.Value];
+        }
+
+        // Prepare finall results 
+        distanceT totalCost = minCostToVertex[endVertex];
+        List<indexT> shortestPathFromStart = Enumerable.Reverse(shortestPathFromEnd).ToList();
+
+        return (shortestPathFromStart, totalCost);
+    }
+}

SouvlakMVP/SouvlakMVP/Map.cs

@@ -181,6 +181,11 @@ public override string ToString()
         return str;
     }
 
+    public int Count() 
+    {
+        return this.map.Count;
+    }
+
     public bool ContainsIntersection(Intersection intersection)
     {
         return this.map.Contains(intersection);         // This should work thanks to custom hash functions

SouvlakMVP/SouvlakMVP/Program.cs

@@ -1,16 +1,46 @@
 // See https://aka.ms/new-console-template for more information
+using System;
 using System.Numerics;
 
+using indexT = System.Int32;
+using distanceT = System.Single;
+
 namespace SouvlakMVP;
 
 class Program
 {
     static void Main(string[] args)
     {
+        // Example map (graph) with 6 intersections (vertices) 
         Map map = new Map();
+        // Intersections from 0 to 5 
         map.AddIntersection(new Map.Intersection(new Vector2(0, 0)));
-        map.AddIntersection(new Vector2(1, 0), new Map.Road[1] { new Map.Road(0, 1f) });
-        map.AddIntersection(new Vector2(1, 1), new Map.Road[2] { new Map.Road(0, 1.44f), new Map.Road(1, 1f) });
+        map.AddIntersection(new Map.Intersection(new Vector2(1, 0)));
+        map.AddIntersection(new Map.Intersection(new Vector2(0, 1)));
+        map.AddIntersection(new Map.Intersection(new Vector2(1, 1)));
+        map.AddIntersection(new Map.Intersection(new Vector2(2, 1)));
+        map.AddIntersection(new Map.Intersection(new Vector2(1, 2)));
+        // 9 roads and their distances 
+        map.AddRoad(0, 1, 3f);
+        map.AddRoad(0, 5, 6f);
+        map.AddRoad(0, 4, 3f);
+        map.AddRoad(1, 2, 1f);
+        map.AddRoad(1, 3, 3f);
+        map.AddRoad(2, 3, 3f);
+        map.AddRoad(2, 5, 1f);
+        map.AddRoad(3, 5, 1f);
+        map.AddRoad(4, 5, 2f);
+
+        // Map visualization
         Console.WriteLine(map.ToString());
+
+        indexT startIntersection = 0;
+        indexT endIntersection = 5;
+        (List<indexT>, distanceT) result = Dijkstra.FindShortestPath(map, startIntersection, endIntersection);
+        // Co właściwie powinna zwracać metoda? Obecnie zwraca zwykłą listę indexT oraz koszt distanceT
+
+        Console.WriteLine("Order of intersections: " + String.Join(" -> ", result.Item1)); ;
+        Console.WriteLine("Minimal cost: " + result.Item2);
+
     }
 }