algorithm - 在 A* 搜索中提取路径

Question

我正在尝试使用以下代码实现 A* 搜索:

void Map::findPath(Robot robot, Model target, vector<Node> nodeList) {
Node targetNode = target.getCurrentNode();
Node startNode = robot.getCurrentNode();

vector<Node> openList;
vector<Node> closedList;
openList.push_back(startNode);
while (!openList.empty()) {
    Node curNode = nodeWithLowestFScore(openList);
    if (curNode.equal(targetNode)) {
        /*cout << "WTF" << endl;
         Node *p = curNode.getParent();
         int i = 0;
         while (!p->equal(startNode)) {
         cout << p->getX() << " " << p->getY() << endl;
         p = p->getParent();
         cout << i++ << endl;
         }*/

        break;
    }

    closedList.push_back(curNode);
    removeFromVector(openList, curNode);
    vector<Node> adjNodes;
    curNode.getWalkableAdjacentNodes(nodeList, adjNodes);
    for (int i = 0; i < adjNodes.size(); i++) {
        if (inVector(closedList, adjNodes[i])) {
            continue;
        }
        if (!inVector(closedList, adjNodes[i])) {
            adjNodes[i].setParent(&curNode);
            cout << "Child: " <<adjNodes[i].getX() << " " << adjNodes[i].getY() << endl;
            cout << "Parent: " << adjNodes[i].getParent()->getX()
                    << " " << adjNodes[i].getParent()->getY() << endl;
            adjNodes[i].setG(curNode.getG() + 1);
            adjNodes[i].setH(adjNodes[i].getDistance(targetNode, 'm'));
            adjNodes[i].setF();
            openList.push_back(adjNodes[i]);
        }
        if (inVector(closedList, adjNodes[i])) {
            if (curNode.getG() + 1 < adjNodes[i].getG()) {
                adjNodes[i].setParent(&curNode);
                adjNodes[i].setG(curNode.getG() + 1);
                adjNodes[i].setF();
            }
        }
    }

}

这是此代码的控制台输出(StartP = [x:-7, y:6], EndP = [x:0, y:-2]:

1. Child: -1 -2
2. Parent: 0 -2
3. Child: 1 -2
4. Parent: 0 -2
5. Child: -2 -2
6. Parent: -1 -2
7. Child: -1 -3
8. Parent: -1 -2
9. Child: -3 -2
10. Parent: -2 -2
11. Child: -3 -1
12. Parent: -3 -2
13. Child: -3 0
14. Parent: -3 -1
15. Child: -3 1
16. Parent: -3 0
17. Child: -4 0
18. Parent: -3 0
19. Child: -5 0
20. Parent: -4 0
21. Child: -5 1
22. Parent: -5 0
23. Child: -5 -1
24. Parent: -5 0
25. Child: -5 2
26. Parent: -5 1
27. Child: -5 3
28. Parent: -5 2
29. Child: -5 4
30. Parent: -5 3
31. Child: -5 5
32. Parent: -5 4
33. Child: -6 4
34. Parent: -5 4
35. Child: -4 4
36. Parent: -5 4
37. Child: -7 4
38. Parent: -6 4
39. Child: -8 4
40. Parent: -7 4
41. Child: -5 6
42. Parent: -5 5
43. Child: -5 7
44. Parent: -5 6
45. Child: -4 6
46. Parent: -5 6
47. Child: -3 2
48. Parent: -3 1
49. Child: -3 3
50. Parent: -3 2
51. Child: -2 2
52. Parent: -3 2
53. Child: -3 4
54. Parent: -3 3
55. Child: -4 4
56. Parent: -3 4
57. Child: -2 4
58. Parent: -3 4
59. Child: -1 4
60. Parent: -2 4
61. Child: -1 2
62. Parent: -2 2
63. Child: -3 6
64. Parent: -4 6
65. Child: -5 8
66. Parent: -5 7
67. Child: -9 4
68. Parent: -8 4
69. Child: -5 -2
70. Parent: -5 -1
71. Child: -1 -4
72. Parent: -1 -3
73. Child: 2 -2
74. Parent: 1 -2
75. Child: 3 -2
76. Parent: 2 -2
77. Child: 2 -3
78. Parent: 2 -2
79. Child: -2 -4
80. Parent: -1 -4
81. Child: -3 -4
82. Parent: -2 -4
83. Child: -3 -5
84. Parent: -3 -4
85. Child: -5 -3
86. Parent: -5 -2
87. Child: -9 5
88. Parent: -9 4
89. Child: -9 6
90. Parent: -9 5
91. Child: -8 6
92. Parent: -9 6
93. Child: -7 6
94. Parent: -8 6

有点乱，但我照做了。它到达起点。 (-7, 6)-> (-8, 6) -> (-9, 6) ... (-1, -2)->(0, -2)。

但是，如果我取消注释以“WTF”开头的行并想回溯它，它只会进入一个打印 (-7, 6) 的无限循环，这是我的 EndP。

非常感谢任何帮助。

Answer 1

好的，有问题的行是这样的:adjNodes[i].setParent(&curNode); 我只需要重新分配一个新节点，将 curNode 的内容复制到它，并将其设置为父节点。算法很好。