astar

/*
Copyright (C) 2018-2024 Geoffrey Daniels. https://gpdaniels.com/

This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, version 3 of the License only.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.  If not, see <https://www.gnu.org/licenses/>.
*/

#pragma once
#ifndef GTL_ALGORITHM_ASTAR_HPP
#define GTL_ALGORITHM_ASTAR_HPP

// Summary: Implementation of the astar algorithm used to solve pathfinding problems.

#if defined(_MSC_VER)
#pragma warning(push, 0)
#endif

#include <functional>
#include <queue>
#include <unordered_set>
#include <unordered_map>
#include <vector>

#if defined(_MSC_VER)
#pragma warning(pop)
#endif

namespace gtl {
    template <typename coordinate_type, typename cost_type>
    class astar final {
    public:
        using search_type = std::function<std::vector<coordinate_type>(const coordinate_type& current)>;
        using heuristic_type = std::function<cost_type(const coordinate_type& current, const coordinate_type& end)>;

    private:
        class node_type final {
        public:
            coordinate_type coordinate_parent;
            coordinate_type coordinate_self;
            cost_type cost;
            bool operator==(const node_type& other) const {
                return (this->coordinate_self == other.coordinate_self);
            }
            bool operator<(const node_type& other) const {
                return this->cost > other.cost;
            }
        };

        template <typename type, typename container = std::vector<type>, typename comparitor = std::less<typename container::value_type> >
        class heap_queue final
            : public std::priority_queue<type, container, comparitor> {
        public:
            typename container::iterator begin() {
                return this->c.begin();
            }
            typename container::iterator end() {
                return this->c.end();
            }
            void sort() {
                std::make_heap(this->c.begin(), this->c.end(), this->comp);
            }
        };

    public:
        static bool solve(
            const coordinate_type& start,
            const coordinate_type& end,
            const search_type& search,
            const heuristic_type& heuristic,
            std::vector<coordinate_type>& path
        ) {
            // Clear outputs.
            path.clear();

            // Prepare structures.
            heap_queue<node_type> open;
            std::unordered_map<coordinate_type, node_type, typename coordinate_type::hash_function> closed;

            // Add start node to open list.
            open.push(node_type{start, start, heuristic(start, end)});

            while (!open.empty()) {
                // Get next best node.
                node_type current = open.top();
                open.pop();
                closed[current.coordinate_self] = current;

                // Check for end.
                if (current.coordinate_self == end) {
                    path.push_back(end);
                    while ((path.back() == start) == false) {
                        path.push_back(closed.find(path.back())->second.coordinate_parent);
                    }
                    std::reverse(path.begin(), path.end());
                    return true;
                }

                // Get next nodes.
                std::vector<coordinate_type> next_coordinates = search(current.coordinate_self);

                // See if we need to add these new nodes to the open list.
                for (const coordinate_type& next_coordinate : next_coordinates) {

                    // Check if the node is already in the closed list.
                    if (closed.find(next_coordinate) != closed.end()) {
                        // It is assumed you have an admissible heuristic.
                        // Therefore if the node is already closed there is no better path to it.
                        // So we skip adding it to the open list.
                        // If you have an inadmissible heuristic.
                        // Need to check the cost of the node here.
                        // If `next` is lower cost we need to remove it from the closed list.
                        continue;
                    }

                    // Create a temporary node and its cost.
                    node_type next = { current.coordinate_self, next_coordinate, current.cost + heuristic(next_coordinate, end) };

                    // Check if the node is already in the open list.
                    bool found = false;
                    for (auto node = open.begin(); node != open.end(); ++node) {
                        if (node->coordinate_self == next_coordinate) {
                            // Update the route if it has a lower cost from this parent.
                            if (next.cost < node->cost) {
                                // If it does, update the cost and parent.
                                node->cost = next.cost;
                                node->coordinate_parent = next.coordinate_parent;
                                open.sort();
                            }
                            found = true;
                            break;
                        }
                    }

                    if (!found) {
                        // If the node is not in the open list, add it to be searched in a future iteration.
                        open.push(next);
                    }
                }
            }

            return false;
        }
    };
}

#endif // GTL_ALGORITHM_ASTAR_HPP