Не могу найти правильный путь из лабиринта путем обхода в ширину

Здравствуйте. Если матрица:

matrix = [
    [0, 1, 0, 1, 0],
    [0, 0, 1, 0, 0],
    [0, 0, 1, 1, 0],
    [0, 0, 0, 0, 0],
    [0, 1, 0, 1, 0]
 ];

Нужно найти путь от точки start до точки end, но двигаться можно только по ячейкам, которые не содержат единицу.

var start = [4,0];
 var end = [3,4];

Написала код, но его проблема в том, что в массив посещенных вершин он добавляет все, в которые может зайти. А мне нужно, чтобы добавлялись только те, которые ведут к выходу.

matrix = [
    [0, 1, 0, 1, 0],
    [0, 0, 1, 0, 0],
    [0, 0, 1, 1, 0],
    [0, 0, 0, 0, 0],
    [0, 1, 0, 1, 0]
 ];
 var start = [4,0];
 var end = [3,4];

function findWay(position, end)
{
    var queue = [];
    var visited = [];
    matrix[position[0]][position[1]] = 1;
    visited.push(position);
    queue.push(position);
    while(queue.length > 0){
        var pos = queue.shift();
        var direction = [ [ pos[0] + 1, pos[1] ], [ pos[0], pos[1] + 1 ], 
                    [ pos[0] - 1, pos[1] ], [ pos[0], pos[1] - 1 ] ];
        for(var i = 0; i < direction.length; i++){
            if (direction[i][0] < 0 || direction[i][0] >= matrix[0].length) continue;
            if (direction[i][1] < 0 || direction[i][1] >= matrix[0].length) continue;
            if (direction[i][0] == end[0] && direction[i][1] == end[1]) return visited;
            if (matrix[direction[i][0]][direction[i][1]] != 0) continue;

            matrix[direction[i][0]][direction[i][1]] = 1;
            queue.push(direction[i]);
            visited.push(direction[i]);
        }
    }
    return visited;
}
findWay(start, end);