public List <Vector2> GetPath(Vector2 from, Vector2 to) { List <Vector2> returnList = new List <Vector2>() { from }; Vector2 vectorFrom = TranslateToNavGrid(from); NavigationGridCell cellFrom = GetCell((short)vectorFrom.X, (short)vectorFrom.Y); Vector2 vectorTo = TranslateToNavGrid(to); NavigationGridCell goal = GetCell((short)vectorTo.X, (short)vectorTo.Y); if (cellFrom != null && goal != null) { SimplePriorityQueue <Stack <NavigationGridCell> > priorityQueue = new SimplePriorityQueue <Stack <NavigationGridCell> >(); Stack <NavigationGridCell> start = new Stack <NavigationGridCell>(); Dictionary <int, NavigationGridCell> closedList = new Dictionary <int, NavigationGridCell>(); Stack <NavigationGridCell> path = null; start.Push(cellFrom); priorityQueue.Enqueue(start, NavigationGridCell.Distance(cellFrom, goal)); closedList.Add(cellFrom.ID, cellFrom); // while there are still paths to explore while (true) { if (!priorityQueue.TryFirst(out _)) { // no solution return(null); } float currentCost = priorityQueue.GetPriority(priorityQueue.First); priorityQueue.TryDequeue(out path); NavigationGridCell cell = path.Peek(); currentCost -= (NavigationGridCell.Distance(cell, goal) + cell.Heuristic); // decrease the heuristic to get the cost // found the min solution return it (path) if (cell.ID == goal.ID) { break; } foreach (NavigationGridCell neighborCell in GetCellNeighbors(cell)) { // if the neighbor in the closed list - skip if (closedList.TryGetValue(neighborCell.ID, out _)) { continue; } // not walkable - skip if (neighborCell.HasFlag(NavigationGridCellFlags.NOT_PASSABLE) || neighborCell.HasFlag(NavigationGridCellFlags.SEE_THROUGH)) { closedList.Add(neighborCell.ID, neighborCell); continue; } // calculate the new path and cost +heuristic and add to the priority queue Stack <NavigationGridCell> npath = new Stack <NavigationGridCell>(new Stack <NavigationGridCell>(path)); npath.Push(neighborCell); // add 1 for every cell used priorityQueue.Enqueue(npath, currentCost + 1 + neighborCell.Heuristic + neighborCell.ArrivalCost + neighborCell.AdditionalCost + NavigationGridCell.Distance(neighborCell, goal)); closedList.Add(neighborCell.ID, neighborCell); } } NavigationGridCell[] pathArray = path.ToArray(); Array.Reverse(pathArray); List <NavigationGridCell> pathList = SmoothPath(new List <NavigationGridCell>(pathArray)); pathList.RemoveAt(0); // removes the first point foreach (NavigationGridCell navGridCell in pathList.ToArray()) { returnList.Add(TranslateFrmNavigationGrid(navGridCell.Locator)); } return(returnList); } return(null); }