У схемы-же есть границы, плюс минус пару шагов для случая если линк в обход схемы пойдёт.

Нафига, у каждого элемента тоже есть границы (в координатах матрицы, например реальные координаты / 7). Линки тоже состоят из линий. Отрисовку из моего примера можешь выкинуть (кроме, конечно, найденной линии - тут отрисовку надо заменить на формирование координат линий линка, т.е. пока направление совпадает - идёшь дальше, а на повороте - формируешь точку линка)

Но вообще, надо бы оптимизировать алгоритм, т.е. вместо очереди использовать список, отсортированный по возрастанию "расстояния" до целевой точки. Выбирать всегда первый элемент (или последний, если по другому отсортировать - удобно если хранится в массиве - никаких сдвигов), а вставку делать с применением бинарного поиска. Ну или как у Galkov-а - использовать хеш.