C ++: использование BFS для поиска пути через матрицу

Пусть задана матрица

1 1 0 0 0

0 1 1 0 0

1 0 1 1 0

0 1 1 0 0

0 0 1 1 1

и попросил найти путь, проходящий только через верхнюю левую точку (жирный курсив 1) до нижней правой точки (другой аналогичный 1). Как только такой путь будет выделен жирным шрифтом.

Я пытался реализовать это в C++, но я - неожиданно - segfault. Что еще более важно, я не могу найти способ сделать это, не дублируя много кода.

Вот мой код:

#include <iostream>
#include <vector>
#include <map>
#include <queue>
#include <algorithm>
#include <list>

using namespace std;

typedef pair<int, int> Coords;

struct Point {
    int x, y;
    int val;
    bool visited;
    Point (int a, int b, int v) : x(a), y(b), val(v), visited(false) {}
};

int main() {
    int n;
    cin >> n;

    vector<vector<Point> > pts(n, vector<Point>(n, Point(0, 0, 0)));
    int temp = 0;
    for(int i = 0; i < n; ++i) {
        for(int j = 0; j < n; ++j) {
            cin >> temp;
            pts[i][j] = Point(i, j, temp);
        }
    }

    queue<Coords> toVisit;

    toVisit.push(Coords(0,0));
    Coords cur(0, 0);
    vector<Coords> neighbors;

    while(!toVisit.empty()) {
        cur = toVisit.back();
        toVisit.pop();
        int x = cur.first, y = cur.second;
        pts[x][y].visited = true;

        bool xgt0 = x > 0, xltm = x < n;
        bool ygt0 = y > 0, yltm = y < n;

        //  x-1,y-1 x  ,y-1 x+1,y-1
        //  x-1,y   x  ,y   x+1,y
        //  x-1,y+1 x  ,y+1 x+1,y+1

        if(xgt0) {
                    if(!pts[x-1][y].visited)   toVisit.push(Coords(x-1,y));
            if(ygt0 && !pts[x-1][y-1].visited) toVisit.push(Coords(x-1,y-1));
            if(yltm && !pts[x-1][y+1].visited) toVisit.push(Coords(x-1,y+1));
        }

        if(ygt0 && !pts[x][y-1].visited) toVisit.push(Coords(x,y-1));
        if(yltm && !pts[x][y+1].visited) toVisit.push(Coords(x,y+1));

        if(xltm) {
                    if(!pts[x+1][y].visited)   toVisit.push(Coords(x+1,y));
            if(ygt0 && !pts[x+1][y-1].visited) toVisit.push(Coords(x+1,y-1));
            if(yltm && !pts[x+1][y+1].visited) toVisit.push(Coords(x+1,y+1));
        }
    }

    for(int i = 0; i < n; ++i) {
        for(int j = 0; j < n; ++j) {
            cout << pts[i][j].visited << " ";
        }
        cout << endl;
    }
}

Это не работает вообще. Как я могу это исправить?

И как я пишу эту программу, не дублируя так много кода в сегменте соседнего добавления BFS?

редактировать

Решенный, вот код (который теперь полностью решает исходную проблему).

Немного рефакторинга было сделано для уменьшения дублирования кода, но больше можно было бы сделать, например, if(something && proceed(...)) push(...) в if(something) pushIfOK(...), Это остается как упражнение для приключений читателя с немного больше времени на его руках, чем я на данный момент. :)

#include <iostream>
#include <vector>
#include <map>
#include <queue>
#include <algorithm>
#include <list>

using namespace std;

typedef pair<int, int> Coords;

struct Point {
    int x, y;
    bool val; //passable or not?
    bool visited;
    Point (int a, int b, bool v) : x(a), y(b), val(v), visited(false) {}
};

bool proceed(Point);

int main() {
    int n;
    cin >> n;

    vector<vector<Point> > pts(n, vector<Point>(n, Point(0, 0, true)));
    int temp = 0;
    for(int i = 0; i < n; ++i) {
        for(int j = 0; j < n; ++j) {
            cin >> temp;
            pts[i][j] = Point(i, j, temp == 1);
        }
    }

    queue<Coords> toVisit;

    toVisit.push(Coords(0,0));
    Coords cur(0, 0);
    vector<Coords> neighbors;

    while(!toVisit.empty()) {
        cur = toVisit.back();
        toVisit.pop();
        int x = cur.first, y = cur.second;
        pts[x][y].visited = true;

        bool xgt0 = x > 0, xltm = x < n - 1;
        bool ygt0 = y > 0, yltm = y < n - 1;

        //  x-1,y-1 x  ,y-1 x+1,y-1
        //  x-1,y   x  ,y   x+1,y
        //  x-1,y+1 x  ,y+1 x+1,y+1

        if(ygt0 && proceed(pts[x][y-1])) toVisit.push(Coords(x,y-1));
        if(yltm && proceed(pts[x][y+1])) toVisit.push(Coords(x,y+1));

        if(xgt0) {
            if(proceed(pts[x-1][y])) toVisit.push(Coords(x-1,y));
            if(ygt0 && proceed(pts[x-1][y-1])) toVisit.push(Coords(x-1,y-1));
            if(yltm && proceed(pts[x-1][y+1])) toVisit.push(Coords(x-1,y+1));
        }

        if(xltm) {
            if(proceed(pts[x+1][y])) toVisit.push(Coords(x+1,y));
            if(ygt0 && proceed(pts[x+1][y-1])) toVisit.push(Coords(x+1,y-1));
            if(yltm && proceed(pts[x+1][y+1])) toVisit.push(Coords(x+1,y+1));
        }
    }

    for(int i = 0; i < n; ++i) {
        for(int j = 0; j < n; ++j) {
            cout << pts[i][j].visited << " ";
        }
        cout << endl;
    }
}

bool proceed(Point p) { return p.val && !p.visited; }