lab6 dop

Assets/Code/Lab6/Graph.cs

@@ -240,5 +240,128 @@ namespace Code.Lab6
             path.Reverse();
             return path;
         }
+        
+        public List<Vertex> GetPathWeights(GameObject srcObj, GameObject dstObj)
+        {
+            if (!srcObj || !dstObj)
+                return new List<Vertex>();
+
+            var src = GetNearestVertex(srcObj.transform.position);
+            var dst = GetNearestVertex(dstObj.transform.position);
+
+            if (!src || !dst)
+            {
+                Debug.LogWarning("Source or destination vertex is null");
+                return new List<Vertex>();
+            }
+
+            Debug.Log($"Dijkstra from {src.id} to {dst.id}");
+
+            var distances = new Dictionary<int, float>();
+            var previous = new Dictionary<int, int>();
+            var unvisited = new HashSet<int>();
+
+            foreach (var vertex in Vertices.Where(vertex => vertex))
+            {
+                distances[vertex.id] = float.MaxValue;
+                unvisited.Add(vertex.id);
+            }
+            
+            distances[src.id] = 0;
+            previous[src.id] = src.id;
+            
+            while (unvisited.Count > 0)
+            {
+                var minDist = float.MaxValue;
+                var currentId = -1;
+                
+                foreach (var id in unvisited)
+                {
+                    if (distances.TryGetValue(id, out var dist) && dist < minDist)
+                    {
+                        minDist = dist;
+                        currentId = id;
+                    }
+                }
+                
+                if (currentId == -1) break;
+
+                if (currentId == dst.id)
+                {
+                    Debug.Log($"Dijkstra: Path found! Total cost: {distances[dst.id]:F2}");
+                    return BuildPathWeights(src.id, dst.id, previous);
+                }
+                
+                unvisited.Remove(currentId);
+
+                var currentVertex = GetVertexById(currentId);
+                if (!currentVertex) continue;
+                
+                var edges = GetEdges(currentVertex);
+
+                foreach (var edge in edges)
+                {
+                    if (!edge.Vertex) continue;
+                    
+                    var neighbourId = edge.Vertex.id;
+                    if (!unvisited.Contains(neighbourId)) continue;
+                    
+                    var newDist = distances[currentId] + edge.cost;
+                    
+                    if (newDist < distances[neighbourId])
+                    {
+                        distances[neighbourId] = newDist;
+                        previous[neighbourId] = currentId;
+                    }
+                }
+            }
+            
+            Debug.LogWarning("Dijkstra: No path found");
+            return new List<Vertex>();
+        }
+
+        private List<Vertex> BuildPathWeights(int srcId, int dstId, Dictionary<int, int> previous)
+        {
+            var path = new List<Vertex>();
+
+            if (!previous.ContainsKey(dstId))
+            {
+                Debug.LogError("Destination not in previous dictionary");
+                return path;
+            }
+
+            var currentId = dstId;
+
+            while (currentId != srcId)
+            {
+                var currentVertex = GetVertexById(currentId);
+                if (!currentVertex)
+                {
+                    Debug.LogError($"Vertex with id {currentId} not found");
+                    break;
+                }
+
+                path.Add(currentVertex);
+
+                if (!previous.TryGetValue(currentId, out var prevId))
+                {
+                    Debug.LogError($"Previous not found for id {currentId}");
+                    break;
+                }
+
+                currentId = prevId;
+            }
+
+            var srcVertex = GetVertexById(srcId);
+            if (srcVertex is not null)
+            {
+                path.Add(srcVertex);
+            }
+
+            path.Reverse();
+
+            Debug.Log($"Path built with {path.Count} vertices");
+            return path;
+        }
     }
 }