Нахождение кратчайшего пути в граф с помощью задачи BFS

Question

я работаю в HomeWork из школы.Я должен найти кратчайший путь в граф, используя алгоритм BFS.Начальная точка - 2, 0 - стена, 1 - путь, 3 - конечная точка.Я прочитал из txt файла 2D Matrix, мне нужно преобразовать эту матрицу в график, используя список смежности, а затем применить алгоритм BFS для приведенного графика.Поэтому моя проблема в том, что когда я печатаю путь, он отображает только «18».Я думаю, что мой алгоритм BPS не заполняет resultList, но я просто не могу понять, почему.Кто-нибудь может мне помочь?

struct Graf {

int matrix[200][200];
int numberOfLines;
int numberOfColumns;
int intrare;

std::vector<int>iesiri;
std::map<int, std::vector<int>> grafNeor;
std::map<int, std::vector<int>>::iterator it;

void citire() {

    std::ifstream f("Matrix.txt");
    f >> numberOfLines >> numberOfColumns;
    std::cout << "numberOfLines: " << numberOfLines << std::endl;
    std::cout << "numberOfColumns: " << numberOfColumns << std::endl;

    for (int lineNumber = 0; lineNumber < numberOfLines; lineNumber++)
        for (int columnNumber = 0; columnNumber < numberOfColumns; columnNumber++)
            f >> matrix[lineNumber][columnNumber];

    f.close();

    for (int i = 0; i < numberOfLines; ++i) {
        for (int j = 0; j < numberOfColumns; ++j) {
            if (matrix[i][j] == 3) {
                intrare = numberOfColumns * i + j;
            }
            if (matrix[i][j] == 2) {
                iesiri.push_back(numberOfColumns * i + j);
            }
        }
    }

    for (int lineNumber = 0; lineNumber < numberOfLines; lineNumber++) {
        for (int columnNumber = 0; columnNumber < numberOfColumns; columnNumber++)
            std::cout << matrix[lineNumber][columnNumber] << " ";
        std::cout << std::endl;
    }
}
void tranzitie() {

    for (int i = 0; i < numberOfColumns * numberOfLines; i++)
        grafNeor.insert(std::pair<int, std::vector<int> >(i, std::vector<int>()));

    for (int i = 0; i < numberOfLines; i++) {
        for (int j = 0; j < numberOfColumns; j++) {
            int key = numberOfColumns * i + j;
            if (matrix[i][j] != 0) {
                // stanga
                if (j > 0 && matrix[i][j - 1] != 0) {
                    int currentValue = numberOfColumns * i + j - 1;
                    it = grafNeor.find(key);
                    if (it != grafNeor.end())
                        grafNeor.insert(std::pair<int, std::vector<int> >(currentValue, std::vector<int>()));

                }
                // dreapta
                if (j < numberOfColumns - 1 && matrix[i][j + 1] != 0) {
                    int currentValue = numberOfColumns * i + j + 1;
                    it = grafNeor.find(key);
                    if (it != grafNeor.end())
                        grafNeor.insert(std::pair<int, std::vector<int> >(currentValue, std::vector<int>()));
                }
                // sus
                if (i > 0 && matrix[i - 1][j] != 0) {
                    int currentValue = numberOfColumns * (i - 1) + j;
                    it = grafNeor.find(key);
                    if (it != grafNeor.end())
                        grafNeor.insert(std::pair<int, std::vector<int> >(currentValue, std::vector<int>()));
                }
                // jos
                if (i < numberOfLines - 1 && matrix[i + 1][j] != 0) {
                    int currentValue = numberOfColumns * (i + 1) + j;
                    it = grafNeor.find(key);
                    if (it != grafNeor.end())
                        grafNeor.insert(std::pair<int, std::vector<int> >(currentValue, std::vector<int>()));
                }
            }
        }
    }
}
void PBF() {

    std::vector<int> noduri;

    for (int i = 0; i < numberOfColumns * numberOfLines; i++)
        noduri.push_back(i);

    std::vector<int> noduriNevizitate;
    noduriNevizitate.assign(noduri.begin(), noduri.end());
    noduriNevizitate.erase(std::remove(noduriNevizitate.begin(), noduriNevizitate.end(), intrare));

    std::vector<int> noduriCurente;
    noduriCurente.push_back(intrare);

    std::vector<int> noduriVizitate;
    std::vector<int> drum;

    for (int i = 0; i < numberOfColumns * numberOfLines; i++)
        drum.push_back(0);

    for (int i = 0; i < noduriNevizitate.size(); i++) {
        drum.assign(noduriNevizitate.at(i), -1);
    }

    int current = 0;

    while (noduriCurente.size() > 0)
    {
        current = noduriCurente.at(0);
        for (int i = 0; i < iesiri.size(); i++) {
            if (iesiri[i] == current)
                break;
        }

        for (int vecin : grafNeor.at(current)) {
            for (int i = 0; i <= noduriNevizitate.size(); i++) {
                if (noduriNevizitate[i] == vecin) {
                    noduriNevizitate.erase(std::remove(noduriNevizitate.begin(), noduriNevizitate.end(), vecin));
                    noduriCurente.push_back(vecin);
                    drum.assign(vecin, current);
                }
            }
        }
        noduriCurente.erase(std::remove(noduriCurente.begin(), noduriCurente.end(), current));
        noduriVizitate.push_back(current);
    }
    int before = current;
    std::vector<int> resultList;

    while (before != current) {
        resultList.push_back(current);
        before = drum.at(current);
        current = before;
    }

    resultList.push_back(intrare);
    std::cout << "drum: ";

    for (int i = 0; i < resultList.size(); i++)
        std::cout << resultList[i];
}

};

int main () {

Graf graf;

graf.citire();
graf.tranzitie();
graf.PBF();
system("pause");
return 0;

}