public Stack <PathNode> FindPath(State state, Piece startPiece, Piece endPiece) { openList.Clear(); closedHashSet.Clear(); openList.Add(new PathNode(startPiece.position)); while (openList.Count > 0) { PathNode currentNode = openList.Pop(); if (currentNode.Position == endPiece.position) { return(ReconstructPath(currentNode, endPiece)); } closedHashSet.Add(currentNode); foreach (Vector2i neighbor in GetNeighbors(state, endPiece, currentNode.Position)) { if (closedHashSet.Contains(neighbor)) { continue; } float gValue = currentNode.g + neighbor.Distance(currentNode.Position); if (openList.Update(neighbor, gValue)) { continue; } float hValue = GetH(neighbor, endPiece.position); PathNode currentNeighbor = new PathNode(neighbor) { g = gValue, h = hValue, f = gValue + hValue, Parent = currentNode }; openList.Add(currentNeighbor); } } return(null); }