AStar, final release [6x6]

// ConsoleApplication1.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include <iostream>
#include <vector>
#include <fstream>
#include <math.h>
#include <string>
#include <algorithm>

using namespace std;

class Punkt {
public:
    int x;
    int y;
    double f;
    double g;
    double h;
    Punkt* rodzic;
};

int mapa[6][6];

Punkt listaWszystkichPunktow[6][6];

double MetrykaEuklidesowa(Punkt* pierwszy, Punkt* drugi) {

    double czynnik1 = (pierwszy->x - drugi->x)*(pierwszy->x - drugi->x);
    double czynnik2 = (pierwszy->y - drugi->y)*(pierwszy->y - drugi->y);

    double h = sqrt(czynnik1 + czynnik2);

    return h;
}

Punkt Poszukiwany(vector<Punkt> lista, Punkt poszukiwany) {

    Punkt zListy = lista.front();

    for (int i = 0; i < lista.size(); i++)
    {
        if (lista[i].x == poszukiwany.x && lista[i].y == poszukiwany.y)
        {
            zListy = poszukiwany;
        }
    }

    return zListy;
}

vector<Punkt> GenerowanieDzieci(Punkt* punkt) {

    vector<Punkt> listaDzieci;

    if (punkt->y + 1 < 6)
    {
        Punkt gora;
        gora.x = punkt->x;
        gora.y = punkt->y + 1;
        gora.rodzic = punkt;
        if (mapa[gora.y][5-gora.x] == 0)
        {
            listaDzieci.push_back(gora);
        }
    }

    if (punkt->y - 1 >= 0)
    {
        Punkt dol;
        dol.x = punkt->x;
        dol.y = punkt->y - 1;
        dol.rodzic = punkt;
        if (mapa[dol.y][5-dol.x] == 0)
        {
            listaDzieci.push_back(dol);
        }
    }

    if (punkt->x - 1 >= 0)
    {
        Punkt lewy;
        lewy.x = punkt->x - 1;
        lewy.y = punkt->y;
        lewy.rodzic = punkt;
        if (mapa[lewy.y][5-lewy.x] == 0)
        {
            listaDzieci.push_back(lewy);
        }
    }

    if (punkt->x + 1 < 6)
    {
        Punkt prawy;
        prawy.x = punkt->x + 1;
        prawy.y = punkt->y;
        prawy.rodzic = punkt;
        if (mapa[prawy.y][5 - prawy.x] == 0)
        {
            listaDzieci.push_back(prawy);
        }
    }

    return listaDzieci;

}

vector<Punkt*> AStar(Punkt* start, Punkt* stop) {

    vector<Punkt*> listaOtwarta;
    vector<Punkt*> listaZamknieta;

    listaOtwarta.push_back(start);
    listaWszystkichPunktow[start->x][start->y].x = start->x;
    listaWszystkichPunktow[start->x][start->y].y = start->y;
    listaWszystkichPunktow[start->x][start->y].g = start->g;
    listaWszystkichPunktow[start->x][start->y].h = start->h;
    listaWszystkichPunktow[start->x][start->y].f = start->f;
    listaWszystkichPunktow[start->x][start->y].rodzic = start->rodzic;

    while (listaOtwarta.size() > 0)
    {
        Punkt* q = listaOtwarta.front();

        for (int i = 0; i < listaOtwarta.size(); i++) {
            if (listaOtwarta[i]->f <= q->f)
            {
                q = listaOtwarta[i];
            }
        }

        listaOtwarta.erase(remove(listaOtwarta.begin(), listaOtwarta.end(), q), listaOtwarta.end());

        vector<Punkt> dzieci = GenerowanieDzieci(q);

        for (int i = 0; i < dzieci.size(); i++) {

            if (dzieci[i].x == stop->x && dzieci[i].y == stop->y) {
                listaZamknieta.push_back(q);
                listaWszystkichPunktow[dzieci[i].x][dzieci[i].y].rodzic = dzieci[i].rodzic;
                stop->rodzic = listaWszystkichPunktow[dzieci[i].x][dzieci[i].y].rodzic;
                listaZamknieta.push_back(stop);
                goto skok2;
            }

            dzieci[i].g = q->g + 1;
            dzieci[i].h = MetrykaEuklidesowa(&dzieci[i], stop);
            dzieci[i].f = dzieci[i].g + dzieci[i].h;

            for (int j = 0; j < listaOtwarta.size(); j++) {

                if (listaOtwarta[j]->x == dzieci[i].x && listaOtwarta[j]->y == dzieci[i].y && listaOtwarta[j]->f < dzieci[i].f) {
                    goto skok;
                }
            }

            for (int j = 0; j < listaZamknieta.size(); j++) {

                if (listaZamknieta[j]->x == dzieci[i].x && listaZamknieta[j]->y == dzieci[i].y && listaZamknieta[j]->f < dzieci[i].f) {
                    goto skok;
                }
            }
            listaWszystkichPunktow[dzieci[i].x][dzieci[i].y].x = dzieci[i].x;
            listaWszystkichPunktow[dzieci[i].x][dzieci[i].y].y = dzieci[i].y;
            listaWszystkichPunktow[dzieci[i].x][dzieci[i].y].f = dzieci[i].f;
            listaWszystkichPunktow[dzieci[i].x][dzieci[i].y].g = dzieci[i].g;
            listaWszystkichPunktow[dzieci[i].x][dzieci[i].y].h = dzieci[i].h;
            listaWszystkichPunktow[dzieci[i].x][dzieci[i].y].rodzic = dzieci[i].rodzic;
            listaOtwarta.push_back(&listaWszystkichPunktow[dzieci[i].x][dzieci[i].y]);
        skok:;
        }
        listaZamknieta.push_back(q);
    }skok2:;

    return listaZamknieta;
}

vector<Punkt> OdwracanieKolejnosci(vector<Punkt> wektor) {
    std::reverse(wektor.begin(), wektor.end());
    return wektor;
}

int main(int argc, char** argv) {

    cout << "Prezentacja algorytmu A* - Kacper Jankowski, 2016\n";

    string nazwap = "grid.txt";
    int wym2 = 6;
    int wym1 = 6;

    //teraz deklarujemy dynamicznie tablice do, której wczytamyu nasz plik,
    int rows = wym2 + 1;
    double **G;
    G = new double*[rows];
    while (rows--) G[rows] = new double[wym1 + 1];

    cout << "\nNacisnij ENTER aby wczytac tablice o nazwie " << nazwap << "\n";
    getchar();

    cout << "\nWczytana mapa:\n\n";

    std::ifstream plik(nazwap.c_str());

    for (unsigned int i = 0; i<wym2; i++)
    {
        for (unsigned int j = 0; j<wym1; j++)
        {
            plik >> G[i][j];
        }
    }
    plik.close();

    for (unsigned int i = 0; i<wym2; i++)
    {
        for (unsigned int j = 0; j<wym1; j++)
        {
            int liczba;
            liczba = G[i][j];
            mapa[i][j] = liczba;
        }
    }

    for (int i = 0; i<wym2; i++)
    {
        for (int j = 0; j<wym1; j++)
        {
            cout << " " << mapa[i][j];
        }cout << "\n";
    }

    Punkt startowy;
    startowy.x = 1;
    startowy.y = 3;
    startowy.g = 0;

    Punkt cel;
    cel.x = 5;
    cel.y = 4;

    cout << "\nPrzyjety punkt startowy: (" << startowy.x << "," << startowy.y << ")\n";
    cout << "Przyjety punkt koncowy: (" << cel.x << "," << cel.y << ")";

    cout << "\n\nNacisnij ENTER aby wygenerowac sciezke przy pomocy algorytmu A*";
    getchar();

    vector<Punkt*> witam = AStar(&startowy, &cel);

    Punkt* start = &startowy;

    Punkt* poszukiwany = &cel;

    vector<Punkt> sciezka;

    sciezka.push_back(cel);

    do
    {
        for (int i = 0; i < witam.size(); i++)
        {
            if (poszukiwany->x == start->x && poszukiwany->y == start->y)
            {
                goto skok3;
            }

            if (witam[i]->x == poszukiwany->rodzic->x && witam[i]->y == poszukiwany->rodzic->y)
            {
                poszukiwany = &listaWszystkichPunktow[witam[i]->x][witam[i]->y];

                Punkt doDodania;
                doDodania.x = poszukiwany->x;
                doDodania.y = poszukiwany->y;
                sciezka.push_back(doDodania);
                i = witam.size();
            }
        }
    } while (poszukiwany != start);
skok3:;

    vector<Punkt> sciezkaUporzadkowana = OdwracanieKolejnosci(sciezka);

    cout << "\n\nMapa z zaznaczona sciezka:\n\n";

    for (int i = 0; i < sciezkaUporzadkowana.size(); i++)
    {
        mapa[5 - sciezkaUporzadkowana[i].y][sciezkaUporzadkowana[i].x] = 1;
    }

    for (int i = 0; i<wym2; i++)
    {
        for (int j = 0; j<wym1; j++)
        {
            cout << " " << mapa[i][j];
        }cout << "\n";
    }

    cout << "\nPunkty na sciezce:\n";

    for (int i = 0; i < sciezkaUporzadkowana.size(); i++)
    {
        cout << "(" << sciezkaUporzadkowana[i].x << "," << sciezkaUporzadkowana[i].y << ") ";
    }

    //na koniec czyscimy pamiec po naszej tablicy
    for (int i = 0; i<wym2 + 1; i++)
    {
        delete[] G[i];
    }//czyscimy wiersze
    delete[] G;//zwalniamy tablice wskaznikow do wierszy

    cout << "\n\nNacisnij ENTER aby zakonczyc";
    getchar();

    return 0;


}