我正在为具有不同距离的简单多出口迷宫实现 A* 路径查找算法,但是我无法找到合适的启发式算法,它似乎执行广度优先搜索。
成本初始设置为 1
这是我的尝试:
public void search(Node startNode, Node goalNode){
System.out.println("Search Started");
boolean found = false;
boolean noPath = false;
Queue<Node> open = new PriorityQueue<Node>(10,new Comparator<Node>(){
@Override
public int compare(Node t, Node t1) {
return Double.compare(t.getCost(), t1.getCost());
}
});
visited[startNode.getX()][startNode.getY()] = true;
order[startNode.getX()][startNode.getY()] = 0;
startNode.setCost(h(startNode, goalNode));
open.add(startNode);
while (found == false && noPath == false) {
if (open.isEmpty()) {
noPath = true;
}else{
Node current = open.remove();
if(current.getX() == goalNode.getX() && current.getY() == goalNode.getY()){
System.out.println("found Node");
printOrder();
found = true;
break;
}else{
for (int i = 0; i < 4; i++){
Node nextNode = getAction(current.getX(),current.getY());
System.out.println("getting node:" + nextNode.getX() + " " + nextNode.getY());
int g2 = current.getCost()+cost+h(current, goalNode);
System.err.println(g2);
nextNode.setCost(g2);
if (nextNode.getX() != -1) {
count++;
visited[nextNode.getX()][nextNode.getY()] = true;
order[nextNode.getX()][nextNode.getY()] = count;
open.add(nextNode);
}
}
}
}
}
}
启发式函数
public int h(Node current, Node goal){
return (goal.getX() - current.getX()) + (goal.getY() - current.getY());
}
帮助将不胜感激