Przechodzenie grafu metodą BFS - najkrótsza droga w labiryncie

// Demonstracja działania procedury BFS
// wykorzystanej do znajdowania najkrótszej
// drogi w labiryncie.
//-------------------------------------
// (C)2011 Koło Informatyczne
// I LO w Tarnowie
//-------------------------------------

#include <iostream>
#include <string>
#include <ctime>
#include <queue>
#include "newconio.h"

using namespace std;

// Stałe

const int PUSTE      =  0;  // pusta komora/korytarz
const int SCIANA     =  1;  // komórka zawierająca ścianę
const int W_LEWO     =  2;  // komórka z nitką w lewo
const int W_PRAWO    =  3;  // komora z nitką w prawo
const int W_GORE     =  4;  // komora z nitką w górę
const int W_DOL      =  5;  // komora z nitką w dół
const int ODWIEDZONE =  6;  // komora już odwiedzona, bez nitki

const int KROK       = 10;  // opóźnienie kroków w ms

const int LX         = 79;  // rozmiar labiryntu w poziomie = 1 + 2 * k
const int LY         = 47;  // rozmiar labiryntu w pionie   = 1 + 2 * k

// Zmienne globalne

int labirynt[LX][LY];       // labirynt
int xs,ys;                  // współrzędne punktu startowego
int xk,yk;                  // współrzędne punktu końcowego
int xp,yp;                  // współrzędne poszukiwacza drogi
int lk;                     // licznik kroków poszukiwacza

queue<int>Q;                // kolejka z STL do przechowywania współrzędnych

// Funkcja wyświetla przekazany jej tekst na środku ostatniego
// wiersza ekranu, oczekuje na wciśnięcie przez użytkownika
// klawisza i zwraca jego kod.
//------------------------------------------------------------

int czekaj(string t)
{
  int klawisz;

  fillrect(' ',0xf0,0,49,79,49);
  textcolor(RED); textbackground(WHITE);
  center(49,_pl(t));

  while(!(klawisz = getch()));

  return klawisz;
}

// Funkcja tworzy labirynt w tablicy labirynt
//-------------------------------------------

void tworz_labirynt()
{
  int x,y,c;

  // wypełniamy cały labirynt ścianami;

  for(x=0; x<LX; x++)
    for(y=0; y<LY; y++) labirynt[x][y] = SCIANA;

  // tworzymy komory i korytarze. Komory tworzone są co 2 komórki.
  // Korytarze łączą komory z lewej i z góry

  for(x=1; x<LX; x+=2)
    for(y=1; y<LY; y+=2)
    {
        labirynt[x][y] = PUSTE;        // komora
        c = 0;                         // licznik korytarzy
        if(rand() % 3 == 0)
        {
            if(x > 1)
              labirynt[x-1][y] = PUSTE; // korytarz z lewej
            c++;                        // zwiększamy licznik korytarzy
        }
        if(rand() % 3 == 0)
        {
            if(y > 1)
              labirynt[x][y-1] = PUSTE;  // korytarz z góry
            c++;                         // zwiększamy licznik korytarzy
        }
        if(c == 0)                     // jeśli nie został utworzony korytarz,
        {                              // tworzymy go z lewej albo u góry
            if(rand() % 2)
            {
                if(x > 1) labirynt[x-1][y] = PUSTE;
            }
            else
            {
                if(y > 1) labirynt[x][y-1] = PUSTE;
            }
        }
    }

    // zerujemy współrzędne początku, końca ścieżki, poszukiwacza
    // oraz licznik kroków

    xs = ys = xk = yk = xp = yp = lk = 0;
}

// Procedura wyświetla labirynt na ekranie
//----------------------------------------

void pokaz_labirynt()
{
    int x,y,rxs,rxk,rys,ryk;

    if(xp)                      // jeśli poszukiwacz jest w labiryncie,
    {
        rxs = xp-1; rxk = xp+1; // to wyświetlimy tylko fragment
        rys = yp-1; ryk = yp+1; // wokół niego - reszta labiryntu już wyświetlona
    }
    else
    {
        rxs = 0; rxk = LX-1;
        rys = 0; ryk = LY-1;
    }

    for(x = rxs; x <= rxk; x++)
      for(y = rys; y <= ryk; y++)
        switch(labirynt[x][y])
        {
            case PUSTE      : putxy(' ',0xe0,x,y); break;
            case SCIANA     : putxy(' ',0x10,x,y); break;
            case ODWIEDZONE : putxy(' ',0xa0,x,y); break;
            default         : putxy(' ',0x40,x,y); break;
        }

     if(xs + ys > 0) putxy('S',0x0c,xs,ys);  // Start
     if(xk + yk > 0) putxy('K',0x0a,xk,yk);  // Koniec
     if(lk)                                  // Licznik kroków
     {
         gotoxy(0,48);
         textattr(0x0e);
         cout << _pl("Liczba kroków : ");
         textcolor(LIGHTRED);
         cout << lk;
     }
}

// Procedura umożliwia użytkownikowi wybranie
// pozycji startowej i końcowej w labiryncie
//------------------------------------------

void ustaw_start_koniec(string t,int & x, int & y)
{
    x = y = 1;                             // rozpoczynamy w lewym górnym rogu

    while(true)
    {
        pokaz_labirynt();                  // wyświetlamy labirynt

        switch(czekaj(t))                  // czekamy na klawisz
        {                                  // i przesuwamy punkt wg klawisza
            case 72:  if(y > 1) y -= 2;    // w górę
                      break;
            case 77:  if(x < LX-2) x += 2; // w w prawo
                      break;
            case 80:  if(y < LY-2) y += 2; // w dół
                      break;
            case 75:  if(x > 1) x -= 2;    // w lewo
                      break;
            case 13:  return;              // klawisz ENTER kończy
        }
    }
}

// Procedura BFS szukająca drogi w labiryncie
// od punktu startowego do punktu końcowego
//-------------------------------------------

void szukaj_wyjscia()
{
  fillrect(' ',0xf0,0,49,79,49);
  textcolor(BLACK); textbackground(WHITE);
  center(49,_pl("Poszukiwanie najkrótszej ścieżki do punktu końcowego"));

  labirynt[xs][ys] = ODWIEDZONE; // zaznaczamy komórkę startową jako odwiedzoną

  Q.push(xs); Q.push(ys);        // współrzędne S do kolejki

  // Teraz w pętli wykonujemy przejście BFS

  while(!Q.empty())
  {
      lk++;                      // zwiększamy licznik

      xp = Q.front(); Q.pop();   // odczytujemy z kolejki współrzędne
      yp = Q.front(); Q.pop();

      pokaz_labirynt();          // wyświetlamy labirynt (fragment)

      delay(KROK);               // odczekujemy

      if(xp == xk && yp == yk)   // sprawdzamy, czy doszliśmy do punktu K
      {
          // cofamy się do punktu S, wymazując kroki

          while(labirynt[xp][yp] != ODWIEDZONE)
          {
              switch(labirynt[xp][yp])
              {
                  case W_LEWO  : labirynt[xp++][yp] = ODWIEDZONE; break;
                  case W_GORE  : labirynt[xp][yp++] = ODWIEDZONE; break;
                  case W_PRAWO : labirynt[xp--][yp] = ODWIEDZONE; break;
                  case W_DOL   : labirynt[xp][yp--] = ODWIEDZONE; break;
              }

              pokaz_labirynt(); // wymuszamy wyświetlenie całego labiryntu

              delay(KROK);
          }

          break;                // wychodzimy z pętli
      }

      // szukamy wszystkich dróg wyjścia z bieżącej pozycji. Współrzędne
      // komórek umieszczamy w kolejce, w komórkach umieszczamy kierunek przejścia

      if(labirynt[xp-1][yp] == PUSTE)
      {
          labirynt[xp-1][yp] = W_LEWO;  // ślad przejścia
          Q.push(xp-1); Q.push(yp);     // współrzędne do kolejki
      }

      if(labirynt[xp][yp-1] == PUSTE)
      {
          labirynt[xp][yp-1] = W_GORE;  // ślad przejścia
          Q.push(xp); Q.push(yp-1);     // współrzędne do kolejki
      }

      if(labirynt[xp+1][yp] == PUSTE)
      {
          labirynt[xp+1][yp] = W_PRAWO; // ślad przejścia
          Q.push(xp+1); Q.push(yp);     // współrzędne do kolejki
      }

      if(labirynt[xp][yp+1] == PUSTE)
      {
          labirynt[xp][yp+1] = W_DOL;   // ślad przejścia
          Q.push(xp); Q.push(yp+1);     // współrzędne do kolejki
      }
  }
}

int main()
{
    _cinit();
    srand(time(NULL));
    fullscreen(true);
    cursoroff();
    tworz_labirynt();
    ustaw_start_koniec("Wybierz punkt startowy i naciśnij ENTER",xs,ys);
    ustaw_start_koniec("Wybierz punkt końcowy i naciśnij ENTER",xk,yk);
    szukaj_wyjscia();
    czekaj("Koniec przejścia. Naciśnij dowolny klawisz, aby zakończyć program.");
    textattr(7);
    cursoron();
    fullscreen(false);
    return 0;
}