Я посмотрел. У тебя целевая функция - расстояние между точками (горизонтальное или вертикальное, которое больше, если не рассматривать движение по диагонали). Т.е. идём туда, где вроде как ближе до нужной точки, а то, что из-за стенок возможно возвращаться придётся, по-моему не учитывается. У меня-же рассчитывается точное кратчайшее расстояние для каждой досягаемой точки, причём если есть несколько путей - выберется путь с минимальным количеством изгибов.

[size=-2]------ Добавлено в 17:36
У меня, кстати, подобная "целевая функция" используется на первом шаге, т.к. надо было определиться "куда же мы пойдём" Если направление задано заранее, то это можно выкинуть.

[size=-2]------ Добавлено в 17:41
Кстати, смысл точек, находящихся в очереди - это границы обработанной области - т.е. та самая "волна".