A*

// 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[20][20];

Punkt listaWszystkichPunktow[20][20];

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 < 20)
    {
        Punkt gora;
        gora.x = punkt->x;
        gora.y = punkt->y + 1;
        gora.rodzic = punkt;
        if (mapa[gora.x][gora.y] == 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.x][dol.y] == 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.x][lewy.y] == 0)
        {
            listaDzieci.push_back(lewy);
        }
    }

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

    return listaDzieci;

}

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

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

    listaOtwarta.push_back(start);

    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) {
                break;
            }

            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);
    }

    return listaZamknieta;
}

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

    cout << "Wczytywanie danych z pliku\n";

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

    //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 << "\n\nNacisnij ENTER aby wczytac tablice o nazwie " << nazwap;
    getchar();

    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 = 1; i < wym2 + 1; i++)
    //{
    //  for (int j = 0; j < wym1 + 1; j++)
    //  {
    //      mapa[i][j] = G[i][j];
    //  }
    //}

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

    Punkt cel;
    cel.x = 19;
    cel.y = 19;

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

    int papaj = 2137;


    //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;
}