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