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