我正在尝试编写A *搜索算法，但似乎无法使其正常工作。我正在从Wikipedia复制伪代码。我的代码似乎只是搜索每个可能的节点。这是我的showPath（）函数：
public void showPath() {
Nodes current = end;
while(current.cameFrom!=null) {
current.isPath = true;
current = current.cameFrom;
}
}
起始节点的cameFrom为null，因为它是默认值。
public void A_Star() {
PriorityQueue<Nodes> closedSet = new PriorityQueue<Nodes>();
PriorityQueue<Nodes> openSet = new PriorityQueue<Nodes>();
closedSet.clear();
openSet.clear();
start.gScore = 0;
openSet.add(start);
start.fScore = getDist(start,end);
while(!(openSet.size() ==0)) {
Nodes curr = openSet.poll();
if(curr.x == end.x && curr.y == end.y) {
showPath();
}
closedSet.add(curr);
for(int i=0;i<curr.getNeighbourCount();i++) {
Nodes neighbour = curr.getNeighbour(i);
if(closedSet.contains(neighbour)) {
continue;
}
//isPassable is a boolean that is false if the Nodes is an obstacle
if(!openSet.contains(neighbour) && neighbour.isPassable) {
openSet.add(neighbour);
}
//It's a grid so every point is a distance of 1 from it's neighbours
else if((curr.gScore+1)>= neighbour.gScore){
continue;
}
neighbour.cameFrom = curr;
neighbour.gScore = curr.gScore+1;
neighbour.fScore = neighbour.gScore + getDist(neighbour,end);
}
}
}
编辑：我的getDist函数
public int getDist(Nodes node1, Nodes node2) {
return ( Math.abs(node1.x - node2.x) + Math.abs(node1.y - node2.y));
}