Как насчет построения графика из вашей Матрицы и использования первого поиска Breadth, первого поиска или алгоритма Дейкстры?