public bool Search(Node start, Node goal) { this.goal = goal; PriorityQueue<Node> open = new PriorityQueue<Node>(); start.parent = null; start.cost = 0; open.Enqueue(graph[graph.IndexOf(start)]); while(!open.Empty()){ SearchState currentState = Step (open); if( currentState == SearchState.REACHED_THE_GOAL){ return true; } } return false; }
public List<Vector2> AStar(Vector2 startPosition, Vector2 targetPosition) { DNode2 startNode = graph.GetNode(startPosition); DNode2 targetNode = graph.GetNode(targetPosition); List<Vector2> positions = new List<Vector2>(); PriorityQueue<DNode2> pq = new PriorityQueue<DNode2>(); foreach (DNode2 d in graph.adjList.Keys) { d.Weight = 9999999; pq.Add(d); } startNode.Weight = 0; while (!pq.Empty()) { DNode2 n = pq.Remove(); positions.Add(n.Coords); if (n.Coords == targetPosition + new Vector2(80, 80)) { positions.TrimExcess(); return positions; } foreach (Edge e in graph.adjList[n]) { DNode2 z = e.GetOpposite(n); double r = e.Weight + Vector2.Distance(z.Coords, targetPosition); if (r < z.Weight) { z.Weight = r; try { pq.Remove(z); } catch (ArgumentException) { continue; } pq.Add(z); } } } return positions; }
public bool searchPath(Dictionary<string, string> pathMap) { PriorityQueue<Node> priorityQueue; priorityQueue = new PriorityQueue<Node>(); priorityQueue.Push(this.begainNode); while (!priorityQueue.Empty()) { Node topNode = priorityQueue.Pop(); #region 判断是否找到目状态 if (matched(topNode, this.targetNode)) { MessageBox.Show("Finished!"); return true; } #endregion int row = topNode.row_0; int col = topNode.col_0; if (row > 0 && topNode.cannotAct != Direction.up) { Node curNode = new Node(topNode); exchange(curNode, row, col, row - 1, col); curNode.ToString(); curNode.cannotAct = Direction.down; if (!pathMap.ContainsKey(curNode.state)) { curNode.deepth = topNode.deepth + 1; curNode.value = getValue(curNode, this.targetNode); curNode.row_0 = row - 1; curNode.col_0 = col; priorityQueue.Push(curNode); pathMap.Add(curNode.state, topNode.state); } } if (row < 2 && topNode.cannotAct != Direction.down) { Node curNode = new Node(topNode); exchange(curNode, row, col, row + 1, col); curNode.ToString(); curNode.cannotAct = Direction.up; if (!pathMap.ContainsKey(curNode.state)) { curNode.deepth = topNode.deepth + 1; curNode.value = getValue(curNode, this.targetNode); curNode.row_0 = row + 1; curNode.col_0 = col; priorityQueue.Push(curNode); pathMap.Add(curNode.state, topNode.state); } } if (col > 0 && topNode.cannotAct != Direction.left) { Node curNode = new Node(topNode); exchange(curNode, row, col, row, col - 1); curNode.ToString(); curNode.cannotAct = Direction.left; if (!pathMap.ContainsKey(curNode.state)) { curNode.deepth = topNode.deepth + 1; curNode.value = getValue(curNode, this.targetNode); curNode.row_0 = row; curNode.col_0 = col - 1; priorityQueue.Push(curNode); pathMap.Add(curNode.state, topNode.state); } } if (col < 2 && topNode.cannotAct != Direction.right) { Node curNode = new Node(topNode); exchange(curNode, row, col, row, col + 1); curNode.ToString(); curNode.cannotAct = Direction.right; if (!pathMap.ContainsKey(curNode.state)) { curNode.deepth = topNode.deepth + 1; curNode.value = getValue(curNode, this.targetNode); curNode.row_0 = row; curNode.col_0 = col + 1; priorityQueue.Push(curNode); pathMap.Add(curNode.state, topNode.state); } } } return false; }
static void Main(string[] args) { search begainSearch = new search(); //测试优先队列功能 PriorityQueue<int> que; que = new PriorityQueue<int>(); que.Push(12); que.Push(132); que.Push(123); que.Push(212); que.Push(322); que.Push(126); que.Push(13); que.Push(189); while (!que.Empty()) { Console.Write(que.Pop().ToString()+" "); } }
bool searchPath(Dictionary<string, string> pathMap, Node result) { PriorityQueue<Node> priorityQueue; Stack<Node> pathStack; priorityQueue = new PriorityQueue<Node>(); pathStack = new Stack<Node>(); priorityQueue.Push(this.begainNode); pathStack.Push(this.begainNode); int cycle = 0; while (!priorityQueue.Empty()) { cycle++; // Console.WriteLine("第 "+cycle.ToString()+" 步"); // Console.WriteLine("队列中的元素 " + priorityQueue.Count); Node topNode = priorityQueue.Top(); priorityQueue.Pop(); #region 判断是否找到目状态 if (matched(topNode, this.targetNode)) { printState(targetNode); Console.WriteLine("搜索完成"); printState(topNode); result = topNode; return true; } #endregion int row = topNode.row_0; int col = topNode.col_0; if (row > 0 && topNode.cannotAct != Direction.up) { Node curNode = new Node(topNode); // Console.WriteLine("当前状态"); // printState(topNode); // Console.WriteLine(row.ToString()+" "+col.ToString()+" 空格上移后状态"); exchange(curNode, row, col, row - 1, col); curNode.ToString(); curNode.cannotAct = Direction.down; if (!pathMap.ContainsKey(curNode.state)) { // printState(curNode); curNode.deepth = topNode.deepth + 1; curNode.value = getValue(curNode, this.targetNode); // Console.WriteLine("当前代价值:"+(curNode.value + curNode.deepth).ToString()); curNode.father = topNode; curNode.row_0 = row - 1; curNode.col_0 = col; priorityQueue.Push(curNode); pathStack.Push(curNode); pathMap.Add(curNode.state, topNode.state); } } if (row < 2 && topNode.cannotAct != Direction.down) { Node curNode = new Node(topNode); // Console.WriteLine("当前状态"); // printState(topNode); // Console.WriteLine(row.ToString()+" "+col.ToString()+" 下移后状态"); exchange(curNode, row, col, row + 1, col); curNode.ToString(); curNode.cannotAct = Direction.up; if (!pathMap.ContainsKey(curNode.state)) { // printState(curNode); curNode.deepth = topNode.deepth + 1; curNode.value = getValue(curNode, this.targetNode); // Console.WriteLine("当前代价值:"+(curNode.value + curNode.deepth).ToString()); curNode.father = topNode; curNode.row_0 = row + 1; curNode.col_0 = col; priorityQueue.Push(curNode); pathStack.Push(curNode); pathMap.Add(curNode.state, topNode.state); } } if (col > 0 && topNode.cannotAct != Direction.left) { Node curNode = new Node(topNode); // Console.WriteLine("当前状态"); // printState(topNode); // Console.WriteLine(row.ToString()+" "+col.ToString()+" 左移之后的状态"); exchange(curNode, row, col, row, col - 1); curNode.ToString(); curNode.cannotAct = Direction.left; if (!pathMap.ContainsKey(curNode.state)) { // printState(curNode); curNode.deepth = topNode.deepth + 1; curNode.value = getValue(curNode, this.targetNode); // Console.WriteLine("当前代价值:"+(curNode.value + curNode.deepth).ToString()); curNode.father = topNode; curNode.row_0 = row; curNode.col_0 = col - 1; priorityQueue.Push(curNode); pathStack.Push(curNode); pathMap.Add(curNode.state, topNode.state); } } if (col < 2 && topNode .cannotAct != Direction.right) { Node curNode = new Node(topNode); // Console.WriteLine("当前状态"); // printState(topNode); // Console.WriteLine(row.ToString()+" "+col.ToString()+" 右移后状态"); exchange(curNode, row, col, row, col + 1); curNode.ToString(); curNode.cannotAct = Direction.right; if (!pathMap.ContainsKey(curNode.state)) { // printState(curNode); curNode.deepth = topNode.deepth + 1; curNode.value = getValue(curNode, this.targetNode); // Console.WriteLine("当前代价值:"+(curNode.value+curNode.deepth).ToString()); curNode.father = topNode; curNode.row_0 = row; curNode.col_0 = col + 1; priorityQueue.Push(curNode); pathStack.Push(curNode); pathMap.Add(curNode.state, topNode.state); } } } return false; }
void UpdateGraphWithDistancesFromNode(Node source) { PriorityQueue<Node> unVisitedNodes = new PriorityQueue<Node>(Nodes); // Time to create a min heap - O(n) // Does this update the value of 'source' in Nodes ? source.Distance = 0; while (!unVisitedNodes.Empty()) // O(n) { Node current = unVisitedNodes.Peek(); if (current.Distance == Constants.INFINITY) { break; } foreach (Node neighbor in current.Neighbors) // O(nm) { if (unVisitedNodes.Contains(neighbor)) { int tentative = 0; Edge edge = FindEdge(current, neighbor); // O(nml) tentative = current.Distance + edge.Cost; if (tentative < neighbor.Distance) { neighbor.Distance = tentative; neighbor.Previous = current; } } } unVisitedNodes.Dequeue(); } }