public static List <Pos> findAStarPath(Pos start, Pos end, int maxIter = 100000) { Dictionary <Pos, float> DistanceFromStart = new Dictionary <Pos, float>(); Dictionary <Pos, float> DistanceToEnd = new Dictionary <Pos, float>(); Dictionary <Pos, Pos> FastestPath = new Dictionary <Pos, Pos>(); List <Pos> Searched = new List <Pos>(); List <Pos> path = new List <Pos>(); if (start != end) { // Create the queue of pos to check List <Pos> nextStep = new List <Pos>(); // Add start pos' neighbors to queue foreach (Pos p in start.neighbors) { DistanceFromStart[p] = p.getMoveToCost(start); DistanceToEnd[p] = Pos.DistanceMinkowski(p, end); FastestPath[p] = start; nextStep.Add(p); } bool pathFound = false; int iter = 0; while (!pathFound && iter < maxIter) { // Order queue by distance nextStep = nextStep.OrderBy(p => DistanceFromStart[p] + DistanceToEnd[p]).ToList(); // Pull next pos to search Pos thisStep = nextStep[0]; //Debug.Log("thisStep is at " + thisStep.loc.x + " , " + thisStep.loc.y); // Mark pos as searched Searched.Add(thisStep); if (thisStep.neighbors.Contains(end)) { pathFound = true; Pos p = end; float newPathCost = p.getMoveToCost(thisStep) + DistanceFromStart[thisStep]; if (!DistanceFromStart.ContainsKey(p) || newPathCost < DistanceFromStart[p]) { DistanceFromStart[p] = newPathCost; FastestPath[p] = thisStep; } if (DistanceToEnd.ContainsKey(p)) { DistanceToEnd[p] = Pos.DistanceMinkowski(p, end); } } else { foreach (Pos p in thisStep.neighbors) { float newPathCost = p.getMoveToCost(thisStep) + DistanceFromStart[thisStep]; if (!DistanceFromStart.ContainsKey(p) || newPathCost < DistanceFromStart[p]) { DistanceFromStart[p] = newPathCost; FastestPath[p] = thisStep; } if (!DistanceToEnd.ContainsKey(p)) { DistanceToEnd[p] = Pos.DistanceMinkowski(p, end); } if (!nextStep.Contains(p) && !Searched.Contains(p)) { nextStep.Add(p); //Debug.Log("Added to nextStep Pos at " + p.loc.x + " , " + p.loc.y); } } nextStep.Remove(thisStep); //Debug.Log("Removed from nextStep Pos at " + thisStep.loc.x + " , " + thisStep.loc.y); } iter++; } //Debug.Log("Completed with " + iter + " / " + maxIter + " iterations."); Pos pathStep = end; while (pathStep != start) { path.Add(pathStep); if (FastestPath.ContainsKey(pathStep)) { pathStep = FastestPath[pathStep]; } else { return(null); } } path.Add(start); path.Reverse(); } return(path); }