Поскольку, судя по кодам, это - демо вариант, видимо будут уместны философские обобщения алгоритма.
Для размышления. так сказать.

Чего мы делаем, в общем случае:
1) Добавляем в некоторый буфер точки для дальнейшего тестирования на цель, методами "допустимого" продолжения трассы из какой-то одной точки.
2) Из нашего множества точек, для повторения п.1 мы выбираем ту "одну", которая имеет минимальную оценку для целевой ф-ии.
3) И, естественно, при добавлении точек по п.1, мы заботимся о сохранении некой информации для того, чтобы "вспомнить" трассу, приведшую у этой точке.

Чего получается:
1) Мы должны иметь некий быстрый способ находить в буфере точку с "минимальной оценкой". В простейшем случае оценки, хватает просто FIFO - как у коллеги tsdima. Тут пользователи пожеланий по наставили - так они, сами того не подозревая, агитируют просто за более "продвинутую" целевую ф-ию. У меня, кажется - просто перебор точек шел, хотя и было упорядочивание по оценке... Но тоже не фонтан ведь - какие-нибудь кэши или бинарные деревья шустрее работают... Мягко говоря

2) Зачем нам матрица ??? Оказывается, это просто самый шустрый кэш по координатам объектов. Собственно, чего нам надо: при анализе, то ли допустимости новой точки (по п.1), то ли для определения ее прайса - узнать, чего в этом месте находится. Но быстро узнать: шерстить по каждому разу все объекты схемы - не будет признаком большого ума, естественно
Отсюда предложение: продумать, то ли двухкоординатный кэш, то-ли дерево для объкетов...
Которое строится вместо стартовых Clear и Fill...
И дополняется новыми объектами по п.1
Кажется, в AutoCad-овские технологии надо бы заглянуть....


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