private void FindPathAstar(int start)
{
var heuristic = getHeuristic(start);
PathCost = Constants.NO_COST;
var startNode = new AStarNode(start, 0, heuristic, CellStatus.Open);
openList.Enqueue(start, startNode.F);
openListLookup[start] = startNode;
while (openList.Count != 0)
{
var nodeId = openList.Dequeue();
var node = openListLookup[nodeId].Value;
//var pos = ((HTiling) map).Graph.GetNodeInfo(nodeId).Position;
if (node.Status == CellStatus.Closed)
continue;
openListLookup[nodeId] = new AStarNode(node.Parent, node.G, node.H, CellStatus.Closed);
if (isGoal(nodeId))
{
ReconstructPath(nodeId);
return;
}
var successors = map.GetNeighbours(nodeId, Constants.NO_NODE);
foreach (var successor in successors)
{
var newg = node.G + successor.Cost;
var successorTarget = successor.Target;
var targetAStarNode = openListLookup[successorTarget];
if (targetAStarNode.HasValue)
{
if (targetAStarNode.Value.Status == CellStatus.Closed || newg >= targetAStarNode.Value.G)
continue;
targetAStarNode = new AStarNode(nodeId, newg, targetAStarNode.Value.H, CellStatus.Open);
openListLookup[successorTarget] = targetAStarNode;
openList.UpdatePriority(successorTarget, targetAStarNode.Value.F);
}
else
{
var newHeuristic = getHeuristic(successorTarget);
var newAStarNode = new AStarNode(nodeId, newg, newHeuristic, CellStatus.Open);
openList.Enqueue(successorTarget, newAStarNode.F);
openListLookup[successorTarget] = newAStarNode;
}
}
}
}
/// <summary>
/// Performs an A* search following the Node Array A* implementation
/// </summary>
public Path FindPath(IMap map, int start, int target)
{
this.isGoal = nodeId => nodeId == target;
this.calculateHeuristic = nodeId => map.GetHeuristic(nodeId, target);
this.map = map;
var heuristic = calculateHeuristic(start);
var startNode = new AStarNode(start, 0, heuristic, CellStatus.Open);
var openQueue = new SimplePriorityQueue<int>();
openQueue.Enqueue(start, startNode.F);
// The open list lookup is indexed by the number of nodes in the graph/map,
// and it is useful to check quickly the status of any node that has been processed
var nodeLookup = new AStarNode?[map.NrNodes];
nodeLookup[start] = startNode;
while (openQueue.Count != 0)
{
var nodeId = openQueue.Dequeue();
var node = nodeLookup[nodeId].Value;
if (isGoal(nodeId))
{
return ReconstructPath(nodeId, nodeLookup);
}
ProcessNeighbours(nodeId, node, nodeLookup, openQueue);
// Close the node. I hope some day the will implement something
// like the records in F# with the "with" keyword
nodeLookup[nodeId] = new AStarNode(node.Parent, node.G, node.H, CellStatus.Closed);
}
// No path found. We could return a null, but since I read the book "Code Complete" I decided
// its best to return an empty path, and I'll return a -1 as PathCost
// TODO: Additionally, all those magic numbers like this -1 should be converted to explicit,
// clearer constants
return new Path(new List<int>(), -1);
}
/// <summary>
/// Reconstructs the path from the destination node with the aid
/// of the node Lookup that stored the states of all processed nodes
/// TODO: Maybe I should guard this with some kind of safetyGuard to prevent
/// possible infinite loops in case of bugs, but meh...
/// </summary>
private Path ReconstructPath(int destination, AStarNode?[] nodeLookup)
{
var pathNodes = new List<int>();
var pathCost = nodeLookup[destination].Value.F;
var currnode = destination;
while (nodeLookup[currnode].Value.Parent != currnode)
{
pathNodes.Add(currnode);
currnode = nodeLookup[currnode].Value.Parent;
}
pathNodes.Add(currnode);
return new Path(pathNodes, pathCost);
}
/// <summary>
/// Processes every open or unexplored successor of nodeId
/// </summary>
private void ProcessNeighbours(int nodeId, AStarNode node, AStarNode?[] nodeLookup, SimplePriorityQueue<int> openQueue)
{
var successors = map.GetNeighbours(nodeId);
foreach (var successor in successors)
{
var newg = node.G + successor.Cost;
var successorTarget = successor.Target;
var targetAStarNode = nodeLookup[successorTarget];
if (targetAStarNode.HasValue)
{
// If we already processed the neighbour in the past or we already found in the past
// a better path to reach this node that the current one, just skip it, else create
// and replace a new PathNode
if (targetAStarNode.Value.Status == CellStatus.Closed || newg >= targetAStarNode.Value.G)
continue;
targetAStarNode = new AStarNode(nodeId, newg, targetAStarNode.Value.H, CellStatus.Open);
nodeLookup[successorTarget] = targetAStarNode;
openQueue.UpdatePriority(successorTarget, targetAStarNode.Value.F);
}
else
{
var newHeuristic = calculateHeuristic(successorTarget);
var newAStarNode = new AStarNode(nodeId, newg, newHeuristic, CellStatus.Open);
openQueue.Enqueue(successorTarget, newAStarNode.F);
nodeLookup[successorTarget] = newAStarNode;
}
}
}
public void SetNodeValue(Id <TNode> nodeId, AStarNode <TNode> value)
{
_astarNodes[nodeId.IdValue] = value;
}
