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