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);
}
