public static Vector3Int[] GetShortestPath(MapUnit unit, GameTile start, GameTile end, bool findClosest = false) { if (!findClosest && end.isBlocked) { return(null); } HashSet <GameTile> closedSet = new HashSet <GameTile>(); HashSet <GameTile> openSet = new HashSet <GameTile>(); GameTile closestSquare = null; double closestDistance = 9999; int maxLoops = 500; int loops = 0; openSet.Add(start); ResetGameTile(); start.gScore = 0; start.fScore = start.Distance(end); double bestRouteDist = start.Distance(end); while (openSet.Count > 0) { var current = openSet.OrderBy(n => n.fScore).FirstOrDefault(); if (current == end) { return(GenerateWaypoints(start, current)); } openSet.Remove(current); closedSet.Add(current); foreach (var neighbour in current.neighbours) { //var neighbourInfo = pair.Value; //var neighbour = neighbourInfo.neighbour; var distance = 1;// neighbourInfo.distance; if (closedSet.Contains(neighbour)) { continue; } if (!IsGameTileReachable(unit, neighbour)) { continue; } var alternativeDistance = current.gScore + distance; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } else if (alternativeDistance >= neighbour.gScore) { continue; } var estimatedDistance = neighbour.Distance(end); neighbour.parent = current; neighbour.gScore = alternativeDistance; neighbour.fScore = alternativeDistance + estimatedDistance; if (closestDistance > estimatedDistance) { closestSquare = neighbour; closestDistance = estimatedDistance; } } if (loops > maxLoops) { break; } loops += 1; } if (closestSquare == null) { return(null); } if (findClosest && closestSquare.Distance(end) < start.Distance(end)) { return(GenerateWaypoints(start, closestSquare)); } else { return(null); } //return null; }