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);
}
/// <summary>
/// Generic search algorithm
/// </summary>
private static SearchResult GenericSearch(StateBase initialState, StateBase goalState, string algName, OpenListComparator comparator, CostFunc cost, IHeuristic heuristic, int? depthLimit = null)
{
var openList = new OpenList(comparator);
var closedList = new ClosedList();
var cache = new HeuristicCache(goalState, heuristic);
var nodesGenerated = 0;
var nodesPrevGenerated = 0;
// add initial node to open list
openList.Push(new SearchNode(cost, initialState, null, null, cache.Evaluate));
while (true)
{
// if nothing on open list, fail
if (openList.Count == 0)
{
return new SearchResult(null, nodesGenerated, nodesPrevGenerated, openList.Count, closedList.Count, algName, heuristic);
}
// get next node to expand
var node = openList.Pop();
closedList.Push(node);
// if at goal state, success
if (node.State.Equals(goalState))
{
return new SearchResult(node, nodesGenerated, nodesPrevGenerated, openList.Count, closedList.Count, algName, heuristic);
}
// if at depth limit, don't generate successors
if (depthLimit != null && node.Depth == depthLimit)
{
continue;
}
var daughters = node.Successors(cost, cache.Evaluate);
foreach (var daughter in daughters)
{
nodesGenerated++;
// if this state is already in open list, replace old node with new node if g-hat is better
var foundInOpen = openList.Find(daughter.State);
if (foundInOpen != null)
{
nodesPrevGenerated++;
if (daughter.Ghat < foundInOpen.Ghat)
{
openList.Replace(foundInOpen, daughter);
}
}
else
{
// else if this state is already in closed list, move from closed to open if g-hat is better
var foundInClosed = closedList.Find(daughter.State);
if (foundInClosed != null)
{
nodesPrevGenerated++;
if (daughter.Ghat < foundInClosed.Ghat)
{
openList.Push(daughter);
closedList.Remove(foundInClosed);
}
}
else
{
// else didn't find in open or closed, add to open
openList.Push(daughter);
}
}
}
}
}
/// <summary>
/// Generic search algorithm
/// </summary>
private static SearchResult GenericSearch(StateBase initialState, StateBase goalState, string algName, OpenListComparator comparator, CostFunc cost, IHeuristic heuristic, int?depthLimit = null)
{
var openList = new OpenList(comparator);
var closedList = new ClosedList();
var cache = new HeuristicCache(goalState, heuristic);
var nodesGenerated = 0;
var nodesPrevGenerated = 0;
// add initial node to open list
openList.Push(new SearchNode(cost, initialState, null, null, cache.Evaluate));
while (true)
{
// if nothing on open list, fail
if (openList.Count == 0)
{
return(new SearchResult(null, nodesGenerated, nodesPrevGenerated, openList.Count, closedList.Count, algName, heuristic));
}
// get next node to expand
var node = openList.Pop();
closedList.Push(node);
// if at goal state, success
if (node.State.Equals(goalState))
{
return(new SearchResult(node, nodesGenerated, nodesPrevGenerated, openList.Count, closedList.Count, algName, heuristic));
}
// if at depth limit, don't generate successors
if (depthLimit != null && node.Depth == depthLimit)
{
continue;
}
var daughters = node.Successors(cost, cache.Evaluate);
foreach (var daughter in daughters)
{
nodesGenerated++;
// if this state is already in open list, replace old node with new node if g-hat is better
var foundInOpen = openList.Find(daughter.State);
if (foundInOpen != null)
{
nodesPrevGenerated++;
if (daughter.Ghat < foundInOpen.Ghat)
{
openList.Replace(foundInOpen, daughter);
}
}
else
{
// else if this state is already in closed list, move from closed to open if g-hat is better
var foundInClosed = closedList.Find(daughter.State);
if (foundInClosed != null)
{
nodesPrevGenerated++;
if (daughter.Ghat < foundInClosed.Ghat)
{
openList.Push(daughter);
closedList.Remove(foundInClosed);
}
}
else
{
// else didn't find in open or closed, add to open
openList.Push(daughter);
}
}
}
}
}
