Алгоритм C ++ A * не всегда имеет целевой узел в пути - PullRequest
Новая
       42

Алгоритм C ++ A * не всегда имеет целевой узел в пути

0 голосов
/ 25 февраля 2020

Я реализовал алгоритм A * в C ++, но вектор не всегда содержит целевой узел, что приводит к бесконечному l oop. Библиотека, которую я использую: https://nullptr.club/libkmint/index.html. Функция shorttestPathToDist () должна выполнять следующие действия: находить кратчайший путь от исходного узла к целевому узлу с помощью A * ad и возвращать этот путь. Это правильная реализация A * или я что-то не так сделал и что?

Helper.hpp: 

#ifndef UFO_HELPER_HPP
#define UFO_HELPER_HPP

#include <map>
#include <iostream>
#include "kmint/map/map.hpp"

namespace kmint {
    namespace ufo {
        class helper {
            public:
                helper();

                std::vector<const map::map_node*> shortestPathToDist(const kmint::map::map_node& source_node, const kmint::map::map_node& target_node);
            private:
                const map::map_node* helper::smallestDistance(std::map<const map::map_node*, float>& actualCost, std::map<const map::map_node*, float>& heuristicCost, std::vector<const kmint::map::map_node*>& queue);
                float heuristic(const map::map_node& source_node, const map::map_node& target_node);
            };
    }
}

#endif

Помощник. cpp: 

#include "kmint/ufo/helper.hpp"

namespace kmint {
    namespace ufo {
        helper::helper() {

        }

        std::vector<const map::map_node*> helper::shortestPathToDist(const map::map_node& source_node, const map::map_node& target_node)
        {
            std::vector<const map::map_node*> path;
            std::vector<const map::map_node*> visited;
            std::vector<const map::map_node*> queue;
            std::map<const map::map_node*, const map::map_node*> previous;
            std::map<const map::map_node*, float> cost;
            std::map<const map::map_node*, float> heuristic_cost;

            queue.push_back(&source_node);
            cost[&source_node] = 0;
            heuristic_cost[&source_node] = heuristic(source_node, target_node);

            while (queue.size() > 0) {
                const map::map_node* shortest_path_node = smallestDistance(cost, heuristic_cost, queue);

                for (int i = 0; i < shortest_path_node->num_edges(); i++) {
                    map::map_edge edge = (*shortest_path_node)[i];
                    const map::map_node *node_to = &edge.to();

                    if (std::find(visited.begin(), visited.end(), node_to) == visited.end()
                        && std::find(queue.begin(), queue.end(), node_to) == queue.end()) {
                        queue.push_back(node_to);
                    }

                    if (cost.find(node_to) == cost.end() || cost[node_to] > cost[shortest_path_node] + edge.weight())
                    {
                        cost[node_to] = cost[shortest_path_node] + edge.weight();
                        heuristic_cost[node_to] = heuristic(*shortest_path_node, target_node);
                        previous[node_to] = shortest_path_node;
                    }

                    if (node_to->node_id() == target_node.node_id())
                    {
                        cost[node_to] = cost[shortest_path_node] + edge.weight();
                        heuristic_cost[node_to] = heuristic(*shortest_path_node, target_node);
                        previous[node_to] = shortest_path_node;
                        break;
                    }
                }
                queue.erase(queue.begin());
                visited.push_back(shortest_path_node);
            }

            // shortest path to target_node
            const map::map_node* current_node = nullptr;

            for (auto const&[key, val] : previous) {
                if (key != nullptr && key != NULL) {
                    if (key->node_id() == target_node.node_id()) {
                        current_node = val;
                        break;
                    }
                }
            }

            path.clear();

            if (current_node == nullptr || current_node == NULL) {
                std::cout << "could not find target node\n";
                //this->shortest_path_to_target(source_node, target_node);
                return path;
            }

            while (current_node != &source_node) {
                if (current_node != nullptr && current_node != NULL) {
                    if (path.size() > 0) {
                        bool found = false;
                        for (auto &p : path) {
                            if (p != NULL && p != nullptr && p->node_id() == current_node->node_id()) {
                                found = true;
                                break;
                            }
                        }

                        if (!found) {
                            path.insert(path.begin(), current_node);
                        }
                    }
                    else {
                        path.insert(path.begin(), current_node);
                    }
                }

                for (auto const&[key, val] : previous) {
                    if (key != nullptr && key != NULL && current_node != nullptr && current_node != NULL) {
                        if (key->node_id() == current_node->node_id()) {
                            current_node = val;
                            break;
                        }
                    }
                }
            }

            return path;
        }
        // manhatan heuristic
        float helper::heuristic(const map::map_node& fNode, const map::map_node& sNode)
        {
            return std::abs(fNode.location().x() - sNode.location().x()) + std::abs(fNode.location().y() - sNode.location().y());
        }

        const map::map_node* helper::smallestDistance(std::map<const map::map_node*, float>& actualCost, std::map<const map::map_node*, float>& heuristicCost, std::vector<const map::map_node*>& queue)
        {
            const map::map_node* sDN = nullptr;
            for (int i = 0; i < queue.size(); i++)
            {
                if (sDN == nullptr || actualCost[queue[i]] + heuristicCost[queue[i]] < actualCost[sDN] + heuristicCost[sDN]) {
                    sDN = queue[i];
                }
            }

            return sDN;
        }
    }
}

1 Ответ

2 голосов
/ 25 февраля 2020

queue.erase(queue.begin());. Это ошибка. Вы удаляете самый старый добавленный объект, в то время как вы должны вытолкнуть текущий узел кратчайшего пути.

Вы также должны удалить узел кратчайшего пути из посещенного набора!

Реальная проблема заключается в том, что вы не используете правильные структуры данных. 

std::vector<const map::map_node*> queue;

должно стать 

using scored_node = std::pair<double, const map::map_node*>
std::priority_queue<scored_node, std::vector<scored_node>, std::greater<scored_node>> queue;

С этим изменением, вам больше не нужен smallestDistance, и вы должны использовать 

const auto shortest_path_scored_node = queue.top();
const auto shortest_path_node = shortest_path_scored_node.second;
queue.pop();
visited.erase(shortest_path_node);

Вместо того, чтобы использовать вектор 

std::vector<const map::map_node*> visited;

, вам следует использовать неупорядоченный набор, если вы просто заботитесь о том, элемент был посещен. Вам гарантирована уникальность и быстрый поиск без усилий. 

std::unodered_set<const map::map_node*> visited;
...