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