Найти самый короткий путь от точки до точки в матрице

#include <cstdlib>
#include <vector>
 
typedef unsigned /*short*/ dist_t;
typedef unsigned short fsize_t;
 
struct Field {
    dist_t dist;
    bool pathable;
 
    Field() {
        dist = dist_t(-1);//Максимальное значение
        pathable = true;
    }
};
 
class Graph {
    Field* f;
    fsize_t w, h;
 
protected:
    Field& F(fsize_t x, fsize_t y) {
        return f[x + y * w];
    }
 
    void SetDist(fsize_t x, fsize_t y, dist_t d) {
        if (d < F(x, y).dist) {
            F(x, y).dist = d;
            if (x && F(x - 1, y).pathable)
                SetDist(x - 1, y, d + 1);
            if (x < w - 1 && F(x + 1, y).pathable)
                SetDist(x + 1, y, d + 1);
            if (y && F(x, y - 1).pathable)
                SetDist(x, y - 1, d + 1);
            if (y < h - 1 && F(x, y + 1).pathable)
                SetDist(x, y + 1, d + 1);
        }
    }
 
public:
    Graph() {
        f = NULL;
        w = h = 0;
    }
 
    Graph(fsize_t width, fsize_t height) {
        f = new Field[width * height];
        w = width;
        h = height;
    }
 
    ~Graph() { delete[ ] f; }
 
    void Resize(fsize_t width, fsize_t height) {
        delete[ ] f;
        f = new Field[width * height];
        w = width;
        h = height;
    }
 
    void SetPathable(fsize_t x, fsize_t y, bool state) {
        F(x, y).pathable = state;
    }
 
    /*
        0 - вверх
        1 - влево
        2 - вправо
        3 - вниз
    */
 
    std::vector<char> Find(fsize_t x0, fsize_t y0, fsize_t x1, fsize_t y1) {
        SetDist(x0, y0, 0);
        dist_t i = F(x1, y1).dist;
        std::vector<char> v(i);
        //Идем в обратном направлении, от конца к началу
        while (i--)
            if (y1 && F(x1, y1 - 1).dist < F(x1, y1).dist) {
                y1--;
                v[i] = 3;
            } else if (x1 && F(x1 - 1, y1).dist < F(x1, y1).dist) {
                x1--;
                v[i] = 2;
            } else if (x1 < w - 1 && F(x1 + 1, y1).dist < F(x1, y1).dist) {
                x1++;
                v[i] = 1;
            } else {
                y1++;
                v[i] = 0;
            }
        return v;
    }
};

#include <algorithm>
#include <iostream>
#include <vector>
 
using namespace std;
 
void PrintPath(char d) {
    switch (d) {
        case 0: cout << "up\n"; break;
        case 1: cout << "left\n"; break;
        case 2: cout << "right\n"; break;
        default: cout << "down\n";
    }
}
 
int main() {
    /*
          0 1 2
        0 @ # $
        1 . # .
        2 . . .
    */
    Graph g(3, 3);
    g.SetPathable(1, 0, false);
    g.SetPathable(1, 1, false);
    vector<char> v = g.Find(0, 0, 2, 0);
    for_each(v.begin(), v.end(), PrintPath);
    cout << endl;
    /*
          0 1 2 3
        0 $ # . .
        1 . . # .
        2 # . . .
        3 . # . #
        4 @ . . .
    */
    g.Resize(4, 5);
    g.SetPathable(1, 0, false);
    g.SetPathable(2, 1, false);
    g.SetPathable(0, 2, false);
    g.SetPathable(1, 3, false);
    g.SetPathable(3, 3, false);
    v = g.Find(0, 4, 0, 0);
    for_each(v.begin(), v.end(), PrintPath);
    cin.get();
}

void SetDist(fsize_t x, fsize_t y, dist_t d) {
    while (d < F(x, y).dist) {
        F(x, y).dist = d;
        if (x && F(x - 1, y).pathable)
            SetDist(x - 1, y, d + 1);
        if (x < w - 1 && F(x + 1, y).pathable)
            SetDist(x + 1, y, d + 1);
        if (y && F(x, y - 1).pathable)
            SetDist(x, y - 1, d + 1);
        if (!(y < h - 1 && F(x, y + 1).pathable))
            return;
        y++;
        d++;
    }
}

#include <stdio.h>
 
char a[10][10];
const maxQ=100;
int q1,q2,qx[maxQ],qy[maxQ],qz[maxQ],ans=-1,x1,y1,x2,y2;
 
int add(int y, int x, int z){   // y,x - координаты рассматриваемой точки, z - шаг на котором до неё дошли.
    if (a[y,x]!="*") return 0;  // если по клетке пойти нельзя (преграда) выход из процедуры
    a[y][x]='#';     // мусорим клетку чтоб не пойти второй раз
    qx[q1]=x;
    qy[q1]=y;        // запоминаем клетку и с какого раза пришли
    qz[q1]=z;
    if (q1>maxQ) q1=0;     // опять же зацикливание... внизу писал
    if( (y==y2)&(x==x2)){  // проверка если вдруг это клетка куда надо прийти то запоминаем на каком щаге и выходим.
        q1=q2+1;
        ans=z;
    }
    return 1;
}
 
void main(){
//ну тут чтение размера массива, массив, стартовую точку (y1,x1) и конечную (y2,x2)
    q1=0; q2=0;
    add(y1,x1,0);   
    while (q1!=q2){  //пока начало очереди не дошло до конца
        x=qx[q2];
        y=qx[q2];            // присваиваем точку из очереди
        z=qz[q2];
        add(y-1,x,z+1);     // попытка пойти во все 4 направления вверх, вниз, влево, вправо
        add(y+1,x,z+1);
        add(y,x-1,z+1);
        add(y,x+1,z+1);
        q2++;           // отодвигаем конец
        if (q2>maxQ) q2=0;   // зацикливание очереди, в принципе если не разбираешься можно опустить.
    }
    printf("%d",ans);
}