通过回溯获得A星算法中的路径

Question

我想写A*(根据维基百科(，我想通过回溯检索路径，但这就是我最终的结果。(我发现了一个类似的问题，但那里的算法没有基于f-cost的优先级(

因此，就上下文而言：我有一个邻接矩阵，它指定哪些节点连接，哪些节点不连接，并尝试基于此编写A*。

HashSet<Vector3f> openSet = new HashSet();
LinkedHashMap<Vector3f,Vector3f> cameFrom = new LinkedHashMap();// 1 - node / 2 - nodes parent
HashMap<Vector3f,Float> gScore = new HashMap();
HashMap<Vector3f,Float> fScore = new HashMap();

gScore.put(actor.getStartingNode(), 0f);
fScore.put(actor.getStartingNode(), (abs(actor.getStartingNode().x-actor.getEndingNodePos().x)+abs(actor.getStartingNode().z-actor.getEndingNodePos().z)));
openSet.add(actor.getStartingNode());
while(!actor.getFoundPath() && !openSet.isEmpty() && !actor.hasLineOfSight){ // pathfinding loop
float lowestF =  Collections.min(fScore.values());
Vector3f currentNode = null;
for(Object entry: fScore.entrySet()) {  // get lowest Fcost node 
Entry<Vector3f,Float>entry1 = (Entry) entry;
if(entry1.getValue().equals(lowestF)) {
currentNode = entry1.getKey();
break;
}
} // get lowest Fcost node 
if(currentNode.equals(actor.getEndingNodePos())){ // found path- quit and backtrack the path
actor.setFoundPath(true);
reconstructPath(cameFrom,currentNode); // doesnt work
break;
}

openSet.remove(currentNode);


for(Object adjacentToCurrent : adjacencyMatrix.get(currentNode)){ // nodes adjacent to currentNode
Vector3f neighbor = (Vector3f) adjacentToCurrent;
float tentativeScore = gScore.get(currentNode)+(abs(currentNode.x-neighbor.x)+abs(currentNode.z-neighbor.z));
if(!gScore.containsKey(neighbor)){
gScore.put(neighbor, 1000000f);  // setting the value as 1000000f as a replacement for infinity
}

if(tentativeScore<gScore.get(neighbor)){
cameFrom.replace(neighbor, currentNode);
gScore.replace(neighbor, tentativeScore);
fScore.replace(neighbor,tentativeScore+(abs(neighbor.x-actor.getEndingNodePos().x)+abs(neighbor.z-actor.getEndingNodePos().z)));
if(!openSet.contains(neighbor)){
cameFrom.putIfAbsent(neighbor, currentNode);
gScore.putIfAbsent(neighbor, tentativeScore);
fScore.putIfAbsent(neighbor,tentativeScore+(abs(neighbor.x-actor.getEndingNodePos().x)+abs(neighbor.z-actor.getEndingNodePos().z)));
openSet.add(neighbor);
}
}


}
fScore.remove(currentNode);
gScore.remove(currentNode);
}

}

路径应该存储在cameFrom中(使其成为LinkedHashMap，以便保持顺序(，然而，这就是我的cameFrom图形表示：cameFrom.png

当然，这意味着最终找到了路径，但这并不是最短的路径。有人能告诉我这条路是从哪里来的吗；书写"；错误的(算法基本上是维基百科的1:1，但我不得不更改一些内容，因为它是用伪代码编写的(

提前感谢

Answer 1

以下是工作版本：a*算法

public static void aStarSearch(GameObject actor){
HashSet<Vector3f> openSet = new HashSet();
HashSet<Vector3f> closedSet = new HashSet();
LinkedHashMap<Vector3f,Vector3f> cameFrom = new LinkedHashMap();// 1 - node / 2 - nodes parent
HashMap<Vector3f,Float> gScore = new HashMap();
HashMap<Vector3f,Float> fScore = new HashMap();

gScore.put(actor.getStartingNode(), 0f);
fScore.put(actor.getStartingNode(), (abs(actor.getStartingNode().x-actor.getEndingNodePos().x)+abs(actor.getStartingNode().z-actor.getEndingNodePos().z)));
openSet.add(actor.getStartingNode());
//    long sum = 0;
//   System.out.println("ending node " + actor.getEndingNodePos());
while(!actor.getFoundPath() && !openSet.isEmpty() && !actor.hasLineOfSight){ // # 4 a) b)
float lowestF =  Collections.min(fScore.values());
Vector3f currentNode = null;
//    long time1 = System.currentTimeMillis();
for(Object entry: fScore.entrySet()) {
Entry<Vector3f,Float>entry1 = (Entry) entry;
if(entry1.getValue().equals(lowestF)) {
currentNode = entry1.getKey();
break;
}
}
//    long time2 = System.currentTimeMillis()-time1;
//    sum += time2;

if(currentNode.equals(actor.getEndingNodePos())){
actor.setFoundPath(true);
reconstructPath(cameFrom,currentNode);
break;
}

openSet.remove(currentNode);
closedSet.add(currentNode);

for(Object adjacentToCurrent : adjacencyMatrix.get(currentNode)){
Vector3f neighbor = (Vector3f) adjacentToCurrent;
if(!closedSet.contains(neighbor)){
float tentativeScore = gScore.get(currentNode)+(abs(currentNode.x-neighbor.x)+abs(currentNode.z-neighbor.z));
boolean tentativeScoreIsBetter = false;


if(!openSet.contains(neighbor)){    
//    cameFrom.putIfAbsent(neighbor, currentNode);
//    gScore.putIfAbsent(neighbor, tentativeScore);
//    fScore.putIfAbsent(neighbor,tentativeScore+(abs(neighbor.x-actor.getEndingNodePos().x)+abs(neighbor.z-actor.getEndingNodePos().z)));
openSet.add(neighbor);
tentativeScoreIsBetter = true;
//    System.out.println("dodaje");
} else if(tentativeScore<gScore.get(neighbor)){
tentativeScoreIsBetter = true;
}
if(tentativeScoreIsBetter){
if(cameFrom.keySet().contains(neighbor)){
cameFrom.replace(neighbor, currentNode);
gScore.replace(neighbor, tentativeScore);
fScore.replace(neighbor,tentativeScore+(abs(neighbor.x-actor.getEndingNodePos().x)+abs(neighbor.z-actor.getEndingNodePos().z)));
}else{
cameFrom.put(neighbor,currentNode);
gScore.put(neighbor, tentativeScore);
fScore.put(neighbor,tentativeScore+(abs(neighbor.x-actor.getEndingNodePos().x)+abs(neighbor.z-actor.getEndingNodePos().z)));
}
}

}
}
fScore.remove(currentNode);
gScore.remove(currentNode);
}

}

重建路径

public static void reconstructPath(HashMap cameFrom,Vector3f currentNode){

LinkedList<Vector3f>finalPath = new LinkedList();
while(cameFrom.keySet().contains(currentNode)){
currentNode = (Vector3f)cameFrom.get(currentNode);
finalPath.addFirst(currentNode);
}

}