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 -2

父母：0 -2

儿童：1 -2

父母：0 -2

儿童：-2 -2

父母：-1 -2

儿童：-1 -3

父母：-1 -2

儿童：-3 -2

父母：-2 -2

儿童：-3 -1

父母：-3 -2

儿童：-3 0

父母：-3 -1

儿童：-3 1

父母：-3 0

儿童：-4 0

父母：-3 0

儿童：-5 0

家长：-4 0

儿童：-5 1

家长：-5 0

儿童：-5 -1

家长：-5 0

儿童：-5 2

家长：-5 1

儿童：-5 3

家长：-5 2

儿童：-5 4

家长：-5 3

儿童：-5 5

家长：-5 4

儿童：-6 4

家长：-5 4

儿童：-4 4

家长：-5 4

孩子：-7 4

家长：-6 4

孩子：-8 4

家长：-7 4

儿童：-5 6

家长：-5 5

儿童：-5 7

家长：-5 6

儿童：-4 6

家长：-5 6

儿童：-3 2

家长：-3 1

儿童：-3 3

家长：-3 2

儿童：-2 2

家长：-3 2

儿童：-3 4

家长：-3 3

儿童：-4 4

家长：-3 4

儿童：-2 4

家长：-3 4

儿童：-1 4

家长：-2 4

儿童：-1 2

家长：-2 2

儿童：-3 6

家长：-4 6

儿童：-5 8

家长：-5 7

孩子：-9 4

家长：-8 4

儿童：-5 -2

父母：-5 -1

儿童：-1 -4

父母：-1 -3

孩子：2 -2

父母：1 -2

孩子：3 -2

父母：2 -2

儿童：2 -3

父母：2 -2

儿童：-2 -4

家长：-1 -4

儿童：-3 -4

父母：-2 -4

儿童：-3 -5

父母：-3 -4

儿童：-5 -3

父母：-5 -2

孩子：-9 5

家长：-9 4

孩子：-9 6

家长：-9 5

孩子：-8 6

家长：-9 6

孩子：-7 6

家长：-8 6

它有点乱，但我跟着它。它达到了起点。（-7,6） - ＆gt; （-8,6） - ＆gt; （-9,6）......（ - 1，-2） - >（0，-2）。

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

非常感谢任何帮助。

Answer 1

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

在A *搜索中提取路径

1 个答案: