public QSearchResult AStar(QState startState, bool output = false, int maxQueue = 1000)
{
HashSet <QState> explored = new HashSet <QState>();
Dictionary <QState, decimal> bestSoFar = new Dictionary <QState, decimal>()
{
{ startState, 0 }
};
HeapPriorityQueue <QStateContainer> toDo = new HeapPriorityQueue <QStateContainer>(maxQueue);
toDo.Enqueue(new QStateContainer(startState), 0);
Dictionary <QState, QSearchResult> pathTo = new Dictionary <QState, QSearchResult>()
{
{ startState, new QSearchResult() }
};
if (output)
{
WriteOutput("Searching for shortest path via A-Star Search...");
}
int steps = 0;
while (toDo.Count > 0 && isRunning)
{
steps++;
QState current = toDo.Dequeue().qstate;
if (current.IsEnd())
{
if (output)
{
WriteOutput("Shortest path of " + pathTo[current].Count + " step(s) found after " + steps + " iteration(s).");
}
return(pathTo[current]);
}
else
{
explored.Add(current);
foreach (QAction action in current.GetActions())
{
QState newState = current.GetNewState(action);
if (!explored.Contains(newState))
{
decimal actualCost = bestSoFar[current] - current.GetValue();
if (!bestSoFar.ContainsKey(newState) || actualCost < bestSoFar[newState])
{
pathTo[newState] = new QSearchResult(pathTo[current]);
pathTo[newState].actionsList.Add(action);
pathTo[newState].QStatesList.Add(newState);
bestSoFar[newState] = actualCost;
toDo.Enqueue(new QStateContainer(newState), bestSoFar[newState] - 1 * newState.GetValueHeuristic());
}
}
}
}
}
if (output)
{
WriteOutput("No path found after " + steps + " iteration(s).");
}
return(null);
}
