Разработать программу, находящую путь из начальной точки в конечную

#include <iostream>
#include <sstream>
#include <fstream>
#include <vector>
#include <queue>
typedef std::vector<char>::size_type stype;
 
struct point {
    stype x, y;
    point(void){}
    point(stype _x, stype _y):x(_x), y(_y){}
 
    bool operator == (const point& p) const {
        return ((x == p.x) && (y == p.y));
    }
 
    bool operator != (const point& p) const {
        return !(*this == p);
    }
};
 
void maze_findpath(std::ostream& _out, std::istream& _in);
bool path_find(std::vector<std::vector<char> >& mat);
 
int main(void){
    char s[] =
    "5 7\n"\
    "      X\n"\
    "SX  XXX\n"\
    " X XX F\n"\
    "    X  \n"\
    " XX   X";
    std::istringstream sp(s);
    maze_findpath(std::cout, sp);
 
/*  файловый ввод и вывод
    std::ifstream fin("input.txt");
    std::ofstream fout("output.txt");
    maze_findpath(fout, fin);
    fin.close();
    fout.close();
*/
    std::cin.get();
    return 0;
}
 
//формат: на первой строке кол-во строк и кол-во столбцов далее матрица
void maze_findpath(std::ostream& _out, std::istream& _in){
    int N, M;
    if(!(_in >> N >> M) || (N <= 0) || (M <= 0))
        return;
    _in.get();
 
    std::vector<std::vector<char> > mat(N);
    char c;
    for(int i = 0; (i < N) && !_in.fail(); ++i){
        mat[i].resize(M);
        for(int j = 0; (j < M) && _in.get(c); ++j)
            mat[i][j] = c;
 
        if(_in.eof())
            break;
        _in.get();
    }
 
    //найти путь
    if(path_find(mat)){
        //вывести
        for(stype i = 0; i < mat.size(); ++i){
            for(stype j = 0; j < mat[i].size(); ++j)
                _out << mat[i][j] << ' ';
            _out << std::endl;
        }
    } else
        _out << "Путь не найден!" << std::endl;
 
    for(stype r = 0; r < mat.size(); ++r)
        mat[r].clear();
    mat.clear();
}
 
//нахождение пути
bool path_find(std::vector<std::vector<char> >& mat){
    const int dirs[8][2] = {{1,0}, {0,1}, {-1,0}, {0,-1}, {1,1}, {-1,1}, {1,-1}, {-1,-1}};
    point cur, start, end;
    const stype rows = mat.size();
    const stype cols = (rows > 0) ? mat[0].size() : 0;
    bool  ok = false;
 
    std::vector<std::vector<short> > visit(rows, std::vector<short>(cols, 0x7FFF));
    std::vector<std::vector<point> > parent(rows, std::vector<point>(cols));
 
    //находим начальную и конечную точку
    for(stype i = 0; i < rows; ++i){
        for(stype j = 0; j < cols; ++j){
            switch(mat[i][j]){
            case 'S':
                start.x = j;
                start.y = i;
                mat[i][j] = ' ';
                break;
            case 'F':
                end.x   = j;
                end.y   = i;
                mat[i][j] = ' ';
                break;
            }
        }
    }
 
    std::queue<point> qs;
    visit[start.y][start.x] = 0;
    qs.push(start);
 
    while(! qs.empty()){
        cur = qs.front();
        qs.pop();
        if(cur == end){
            ok = true;
            break;
        }
 
        for(stype i = 0; i < 8; ++i){
            stype x = cur.x + dirs[i][0];
            stype y = cur.y + dirs[i][1];
            if((x < 0) || (y < 0) || (x >= cols) || (y >= rows) || (mat[y][x] == 'X'))
                continue;
 
            if(visit[y][x] > (visit[cur.y][cur.x] + 1)){
                parent[y][x] = cur;
                visit[y][x]  = visit[cur.y][cur.x] + 1;
                qs.push(point(x, y));
            }
        }
    }
 
    if(ok){
        mat[end.y][end.x] = '+';
        while(start != end){
            end = parent[end.y][end.x];
            mat[end.y][end.x] = '+';
        }
    }
 
    for(stype r = 0; r < visit.size(); ++r){
        visit[r].clear();
        parent[r].clear();
    }
    parent.clear();
    visit.clear();
    return ok;
}