Algorytm A* / AStar

#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <math.h>

using namespace std;

//nazwa pliku z którego pobieramy trasê
string nazwap = "grid.txt";

//wymiary trasy
int wym_x = 20;
int wym_y = 20;

//punkty pocz¹tkowy i koñcowy
int start[ 2 ] = { 0, 0 };
int koniec[ 2 ] = { 19, 19 };

//definicja wez³ów
struct wezel {
    int wsp[ 2 ];
    float G;
    int parent[ 2 ];
    float H;
    float F;
};

//vector <int> string;
//funkcja szukaj¹ca danego elementu w danej liœcie
int find( vector < wezel > tab, int x, int y ) {
    for( int i = 0; i < tab.size(); i++ ) {
        if(( tab[ i ].wsp[ 0 ] == x ) &&( tab[ i ].wsp[ 1 ] == y ) ) {
            return true;
        }
    }
    return false;
}
//funkcja zwracaj¹ca index danego elementu tablicy jednowymiarowej
int index( vector < wezel > tab, int x, int y ) {
    for( int i = 0; i < tab.size(); i++ ) {
        if(( tab[ i ].wsp[ 0 ] == x ) &&( tab[ i ].wsp[ 1 ] == y ) ) {
            return i - 1;
        }
    }
}
//fukcja zwracajaca heurystyke pubktu na podstawie euklidesa
float heurestyka( int x1, int y1, int x2, int y2 ) {
    return( float )( sqrt(( pow(( x1 - x2 ), 2 ) + pow(( y1 - y2 ), 2 ) ) ) );
   
}
//funkcja zwracaj¹ca siatkê koñcow¹
int trasa( int siatka[][ 20 ], vector < wezel > tab, int x, int y ) {
    for( int i = 0; i < tab.size(); i++ ) {
        if( tab[ i ].wsp[ 0 ] == x && tab[ i ].wsp[ 1 ] == y ) {
            siatka[ x ][ y ] = 3;
            if( tab[ i ].parent[ 0 ] == x && tab[ i ].parent[ 1 ] == y ) {
                siatka[ 0 ][ 0 ] = 3;
                return 1;
            }
            trasa( siatka, tab, tab[ i ].parent[ 0 ], tab[ i ].parent[ 1 ] );
        }
    }
}

int main( int argc, char ** argv ) {
   
    //definicja trasy
    int siatka[ wym_x ][ 20 ];
    std::ifstream plik( nazwap.c_str() );
    for( unsigned int i = 0; i < wym_y; i++ )
    {
        for( unsigned int j = 0; j < wym_x; j++ )
        {
            plik >> siatka[ i ][ j ];
        }
    }
    plik.close();
    cout << "Siatka:\n\n";
    for( int i = 0; i < wym_x; i++ )
    {
        for( int j = 0; j < wym_y; j++ )
        {
            cout << " " << siatka[ i ][ j ];
        } cout << "\n";
    }
    cout << "\n";
   
    //deklaracja podstawowych zmiennych
    float H = heurestyka( start[ 0 ], start[ 1 ], koniec[ 0 ], koniec[ 1 ] );
    float G = 0;
    float F = H + G;
    int parent[ 2 ];
   
    vector < wezel > LO;
    vector < wezel > LZ;
   
    LO.push_back( { { start[ 0 ], start[ 1 ] }, G, { }, H, F } );
    wezel Q = { { LO[ 0 ].wsp[ 0 ], LO[ 0 ].wsp[ 1 ] }, LO[ 0 ].G, { }, LO[ 0 ].H, LO[ 0 ].F };
    if( siatka[ koniec[ 0 ] ][ koniec[ 1 ] ] == 5 ) {
        cout << "Nie mozna isc na zamkniete pole!";
        getchar();
        return 0;
    }
    if( siatka[ start[ 0 ] ][ start[ 1 ] ] == 5 ) {
        cout << "Nie mozna ruszyc z zamknietego pola!";
        getchar();
        return 0;
    }
   
    //g³owy algorytm
    while( LO.size() > 0 ) {
        int n = 0;
        //szukanie min
        float min = LO[ 0 ].F;
        for( int i = 0; i < LO.size(); i++ ) {
            if( LO[ i ].F <= min && LO[ i ].F != 0 ) {
                n = i;
                min = LO[ i ].F;
                Q = { { LO[ i ].wsp[ 0 ], LO[ i ].wsp[ 1 ] }, LO[ i ].G, { LO[ i ].parent[ 0 ], LO[ i ].parent[ 1 ] }, LO[ i ].H, LO[ i ].F };
                LZ.push_back( Q );
                cout << "min " << min << endl;
                break;
            }
        }
        //zwracanie wyniku
        if( Q.wsp[ 0 ] == koniec[ 0 ] && Q.wsp[ 1 ] == koniec[ 1 ] ) {
            trasa( siatka, LZ, koniec[ 0 ], koniec[ 1 ] );
            cout << "Siatka z trasa:\n\n";
            for( int i = 0; i < wym_x; i++ )
            {
                for( int j = 0; j < wym_y; j++ )
                {
                    cout << " " << siatka[ i ][ j ];
                } cout << "\n";
            }
            getchar();
            return 1;
        }
        cout << "WSP:(" << Q.wsp[ 0 ] << ',' << Q.wsp[ 1 ] << ')' << endl;
       
        //sprawdzanie s¹siadów
        for( int row = Q.wsp[ 0 ] - 1; row <= Q.wsp[ 0 ] + 1; row++ ) {
            for( int kol = Q.wsp[ 1 ] - 1; kol <= Q.wsp[ 1 ] + 1; kol++ ) {
                if(( row >= 0 && kol >= 0 ) &&( row < wym_x && kol < wym_y ) &&( row + kol != Q.wsp[ 0 ] + Q.wsp[ 1 ] ) &&( row + kol != Q.wsp[ 0 ] + Q.wsp[ 1 ] + 2 ) &&( row + kol != Q.wsp[ 0 ] + Q.wsp[ 1 ] - 2 ) ) {
                    //jeœli pole zabronione lub w LZ
                    if( siatka[ row ][ kol ] == 5 || find( LZ, row, kol ) == true ) {
                        cout << "Zamkniete: " << row << ',' << kol << endl;
                    }
                    //jeœli pola nie ma w LO
                    else if( find( LO, row, kol ) == false ) {
                        G = Q.G + 1;
                        H = heurestyka( row, kol, koniec[ 0 ], koniec[ 1 ] );
                        F = H + G;
                        LO.push_back( { { row, kol }, G, { Q.wsp[ 0 ], Q.wsp[ 1 ] }, H, F } );
                        cout << "Dodane:" << row << ',' << kol << endl;
                    }
                    else {
                        int m = index( LO, row, kol );
                        float new_G = Q.G + 1;
                        if( new_G < LO[ m ].G ) {
                            LO[ m ].parent[ 2 ] = Q.wsp[ 2 ];
                            LO[ m ].G = new_G;
                            LO[ m ].F = new_G + H;
                            cout << "Zmienione:" << row << ',' << kol << endl;
                        }
                    }
                }
            }
        }
        LO.erase( LO.begin() + n );
    }
   
    cout << "Brak trasy z punktu " << start[ 0 ] << ',' << start[ 1 ] << " do punktu " << koniec[ 0 ] << ',' << koniec[ 1 ];
    getchar();
    return 0;
}