public List <DijkstraStep> GetSubGraph(Vector2Int from, List <PathStep> graph) { // SubGraph and a copy to keep track of processed elements List <DijkstraStep> subGraph = new List <DijkstraStep>(); List <DijkstraStep> unprocessed = new List <DijkstraStep>(); // Subgraphes initialization foreach (PathStep step in graph) { DijkstraStep ds = new DijkstraStep { pathStep = step, weight = (step.pos == from) ? 0 : int.MaxValue, prev = null }; subGraph.Add(ds); unprocessed.Add(ds); } // Computes subgraph values: shorter distances, previous element while (unprocessed.Count > 0) { DijkstraStep current = new DijkstraStep { pathStep = null, prev = null, weight = int.MaxValue }; foreach (DijkstraStep ds in unprocessed) { if (ds.weight < current.weight) { current = ds; } } foreach (PathStep next in current.pathStep.reach) { DijkstraStep dijkstraNext = subGraph.Find(n => n.pathStep == next); if (dijkstraNext.weight > current.weight + PathStep.Distance(current.pathStep, dijkstraNext.pathStep)) { dijkstraNext.weight = current.weight + PathStep.Distance(current.pathStep, dijkstraNext.pathStep); dijkstraNext.prev = current; } } unprocessed.Remove(current); } return(subGraph); }
public List <Vector2Int> FindShorterPathFromTo(Vector2Int from, Vector2Int to, List <PathStep> graph) { // Find from and end postitions in graph PathStep start = null, end = null; if (graph != null) { start = graph.Find(step => step.pos == from); end = graph.Find(step => step.pos == to); } if (graph == null || start == null || end == null) { Debug.Log("No path!"); return(null); } // Output initialization List <Vector2Int> path = new List <Vector2Int>(); // Build Subgraph List <DijkstraStep> subGraph = GetSubGraph(from, graph); // Retrieves shorter path step by step, from end to start DijkstraStep shortestPathStep = subGraph.Find(s => s.pathStep == end); while (shortestPathStep.pathStep != start) { path.Add(shortestPathStep.pathStep.pos); shortestPathStep = shortestPathStep.prev; } path.Add(start.pos); // Revert path so it's from start to end path.Reverse(); return(path); }