Либо я не так понял либо алгоритм косячный. Вот скрин
Вложение 3652. Ниже прелагаю схему по каторги все происходит. Красный это стены, зеленый начало пути, синий конец пути, розовым путь который строит алгоритм, серым путь который должен быть (просьба не предератся к точности серого пути, так как рисовал на скорую руку и не высчитывал детали) вот сама
Вложение 3653. Всем кто желает проверить вот вам файл с картой
Вложение 3654