public static Cell DijkstraNextCell(int startI, int startJ, int endI, int endJ, Cell[] forbidden) { if (_distMap == null) { _distMap = new double[world.Height, world.Width]; _distPrev = new Cell[world.Height, world.Width]; } var q = new PriorityQueue<Pair<double, Cell>>(); q.Push(new Pair<double, Cell>(0.0, new Cell(endI, endJ))); for (var i = 0; i < world.Height; i++) for (var j = 0; j < world.Width; j++) _distMap[i, j] = Infinity; _distMap[endI, endJ] = 0; while (q.Count > 0) { var cur = q.Top().Second; var minDist = -q.Top().First; q.Pop(); if (minDist > _distMap[cur.I, cur.J]) continue; EnumerateNeigbours(cur, to => { if (!CanPass(cur.I, cur.J, to.I, to.J) || forbidden.Any(x => x.Equals(to.I, to.J))) return; var distTo = _distMap[cur.I, cur.J] + GetCost(to); if (distTo < _distMap[to.I, to.J]) { _distMap[to.I, to.J] = distTo; _distPrev[to.I, to.J] = cur; q.Push(new Pair<double, Cell>(-distTo, to)); } }); } if (_distPrev[startI, startJ] == null) { if (forbidden.Length == 0) throw new Exception("path not found"); return DijkstraNextCell(startI, startJ, endI, endJ, new Cell[] {}); } return _distPrev[startI, startJ]; }
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; }