Algorytm szukania drogi

#include <iostream>
#include <vector>
#include <queue>
#include <unordered_map>
#include <algorithm>

struct Point {
    int x, y;
    Point()
        : x( 0 )
        , y( 0 )
    { }
    Point( int x, int y )
        : x( x )
        , y( y )
    { }
   
    bool operator ==( const Point & other ) const {
        return x == other.x && y == other.y;
    }
   
    bool operator !=( const Point & other ) const {
        return x != other.x || y != other.y;
    }
   
    bool operator <( const Point & other ) const {
        return std::tie( x, y ) < std::tie( other.x, other.y );
    }
};

namespace std {
    template < >
    struct hash < Point > {
        size_t operator()( const Point & p ) const {
            return hash < int >()( p.x ) ^ hash < int >()( p.y );
        }
    };
}

int heuristic( const Point & a, const Point & b ) {
    return abs( a.x - b.x ) + abs( a.y - b.y );
}

std::vector < Point > getNeighbors( const Point & p, const std::vector < std::vector < bool >> & map ) {
    std::vector < Point > neighbors;
    int dx[ 4 ] = { 1, 0, - 1, 0 };
    int dy[ 4 ] = { 0, 1, 0, - 1 };
    for( int i = 0; i < 4; ++i ) {
        int nx = p.x + dx[ i ], ny = p.y + dy[ i ];
        if( nx >= 0 && nx < map.size() && ny >= 0 && ny < map[ 0 ].size() && map[ nx ][ ny ] ) {
            neighbors.emplace_back( nx, ny );
        }
    }
    return neighbors;
}

std::vector < Point > aStar( const std::vector < std::vector < bool >> & map, const Point & start, const Point & goal ) {
    std::priority_queue < std::pair < int, Point >, std::vector < std::pair < int, Point >>, std::greater < >> openSet;
    std::unordered_map < Point, Point > cameFrom;
    std::unordered_map < Point, int > costSoFar;
   
    openSet.emplace( 0, start );
    cameFrom[ start ] = start;
    costSoFar[ start ] = 0;
   
    while( !openSet.empty() ) {
        Point current = openSet.top().second;
        openSet.pop();
       
        if( current == goal ) {
            std::vector < Point > path;
            while( current != start ) {
                path.push_back( current );
                current = cameFrom[ current ];
            }
            path.push_back( start );
            std::reverse( path.begin(), path.end() );
            return path;
        }
       
        for( const Point & next: getNeighbors( current, map ) ) {
            int newCost = costSoFar[ current ] + 1;
            if( costSoFar.find( next ) == costSoFar.end() || newCost < costSoFar[ next ] ) {
                costSoFar[ next ] = newCost;
                int priority = newCost + heuristic( next, goal );
                openSet.emplace( priority, next );
                cameFrom[ next ] = current;
            }
        }
    }
   
    return { }; // No path found
}

void drawMap( const std::vector < std::vector < bool >> & map, const Point & playerPos )
{
    std::cout << "=============\n";
    int x = 0;
    for( const auto & row: map )
    {
        int y = 0;
        for( const auto & cell: row )
        {
            if( playerPos.x == x && playerPos.y == y )
                 std::cout << "* ";
            else
                 std::cout <<( cell ? "_ ": "X " );
           
            ++y;
        }
        std::cout << "\n";
        x++;
    }
    std::cout << "\n";
}

int main() {
    std::vector < std::vector < bool >> map = {
        { true, true, true, false },
        { false, true, false, true },
        { true, true, true, true }
    };
   
    Point start( 0, 0 );
    Point goal( 2, 3 );
   
    std::vector < Point > path = aStar( map, start, goal );
   
    for( const auto & row: map )
    {
        for( const auto & cell: row )
             std::cout <<( cell ? "_ "
            : "X " );
       
        std::cout << "\n";
    }
   
    std::cout << "Path: ";
    for( const Point & p: path )
         std::cout << "(" << p.x << ", " << p.y << ") ";
   
    std::cout << "\n";
    for( const Point & p: path )
         drawMap( map, p );
   
   
    return 0;
}