Отыскания прохода по лабиринту

template <class Ty>
class stack
{
private:
    class stack_node
    {
        Ty          data;
        stack_node* next;
 
    public:
        stack_node(stack_node* _next)
            :next(_next) {}
 
        friend class stack;
    };
 
    stack_node* first;
 
public:
    stack()
        :first(0) {}
 
    ~stack()
    {
        clear();
    }
 
public:
    void    push_front(const Ty& val)
    {
        first = new stack_node(first);
        first->data = val;
    }
 
    Ty      pop_front()
    {
        Ty tmp_val = first->data;
 
        stack_node* to_del = first;
        first = first->next;
  delete to_del;
 
        return tmp_val;
    }
 
    bool    empty() const
    {
        return (first == 0);
    }
 
    void    clear()
    {
        stack_node* to_del;
        for (stack_node* curr = first;curr;)
        {
            to_del = curr;  curr = curr->next;
            delete to_del;
        }
    }
};

/*Основной файл main.cpp*/
 
#include "StdAfx.h"
#include "stack.h"
#include <vector>
#include <iostream>
using namespace std;
 
/*
*   Перечисление field - поле лабиринта
*/
enum field
{
   f_free = ' ',    //free space
   f_wall = '#',    //wall
   f_exit = 'E',    //exit
   f_entr = 'S',    //start
};
 
 
/*  Функция print_position
*   Выводит текущую позицию в лабиринте
*/
void print_position(const vector<vector<char> >& labirint,const pair<int,int>& pos)
{
    system("cls");
    for (size_t i=0; i<labirint.size(); ++i)
    {
        for (size_t j=0; j<labirint[i].size(); ++j)
        {
            cout << ( (i==pos.first && j==pos.second)?
                        (char)0x15: labirint[i][j]);
        }
        cout <<' ';
    }
    cout << "Press any key";      cin.get();
}
 
/*  Функция find_way - находит путь в лабиринте
*
*/
bool find_way(const vector<vector<char> >& labirint,
              stack <pair<int,int> >& way)
{
    pair<int,int>   p_curr = way.pop_front();
 
    //проверяем текущую клетку
    switch (labirint[p_curr.first][p_curr.second])
    {
    case f_exit: way.push_front(p_curr);    return true;
        break;
    case f_wall: way.push_front(p_curr);    return false;
        break;
    default:
        break;
    }
 
    //выводим текущую позицию решения
//    print_position(labirint,p_curr);
 
    //пытаемся шагнуть дальше
    pair<int,int>    p_prev,p_next;
    if (way.empty())
    {
        p_prev = make_pair<int,int>(-10,-10);
        way.push_front(p_curr);
    }
    else
    {
        p_prev = way.pop_front();
 
        way.push_front(p_prev);
        way.push_front(p_curr);
    }
 
    //перебираем возможные ходы
    for (int dx = -1; dx <= 1; dx++)
        for (int dy = -1; dy <= 1; dy++)
        {
            //определяем следующую клетку
            if (!dx && !dy || dx && dy) continue;
            else
                p_next = make_pair(p_curr.first +dx,p_curr.second +dy);
 
            if (p_next != p_prev && 
                p_next.first >=0 && (unsigned)p_next.first <labirint.size() &&
                p_next.second>=0 && (unsigned)p_next.second<labirint[0].size())
            {
                way.push_front(p_next);
                if (find_way(labirint,way)) return true;
                else
                    way.pop_front();
            }
        }
 
    return false;
}
 
/*  Entry point
*
*/
void main()
{
    const char *labirint[] =
    {
        "##############",
        "#            E",
        "# #### #######",
        "#   #        #",
        "############ #",
        "#S           #",
        "##############",
    };
 
    //здесь будет храниться путь
    stack<pair<int,int> > way;
 
    //заполняем матрицу лабиринта
    vector <vector<char> > labirint_v(sizeof(labirint)/sizeof(char*));
    for (size_t i=0; i<sizeof(labirint)/sizeof(char*); ++i)
    {
        size_t length = strlen(labirint[i]);
        labirint_v[i].resize(length);
        for (size_t j=0; j<length; ++j)
        {
            if ( (labirint_v[i][j] = labirint[i][j]) == f_entr && way.empty())
                //точка старта
                way.push_front(make_pair(i,j));
        }
    }
 
    //ищем решение
    if (find_way(labirint_v,way))
    {
        //переворачиваем путь
        stack<pair<int,int> > way_inversed;
        while (!way.empty())
            way_inversed.push_front(way.pop_front());
 
        //печатаем путь
        while (!way_inversed.empty())
            print_position(labirint_v,way_inversed.pop_front());
    }
    else
        cout << "Way doesn't exist" << endl;
}