Реализация алгоритма. 
Алгоритм планирования пути на местности

Ступень 1. На первой ступени фильтрации алгоритм распространяется от стартовых точек в виде волны. Фронт волны строится следующим образом. Множество стартовых точек рассматривается как начальный фронт волны. Маска позиционируется в стартовую точку карты и в матрицах затрат Z, Q просматриваются все точки, покрываемые маской. Точки, для которых значения в матрице Z равны Ѓ‡, а значения в матрице Q конечны, переносятся в список граничных точек. Просматриваются все стартовые точки и строится общий список. Далее список редактируется, из него удалятся повторяющиеся точки (редактирование может производиться и при смене позиции маски). Логические условия построения нового фронта имеют вид:

(1).

где координаты y и x принадлежат полю движущейся маски, y*, x* - координаты точек текущего фронта волны. Фильтрация выполняется для всего списка граничных точек. С этой целью маска позиционируется центром в граничную точку (y, x) и выполняется вычисление нового значения накопленных затрат по следующему правилу:

(2).

где j, i = 0, ±1 — индексы маски с ненулевыми значениями элементов. Полагается, что.

z00 (y, x)=0. В центральных областях карты матрица фильтрующей маски имеет вид:

Матрица маски наполняется нулями при пересечении границы карты. Значение соответствующего элемента матрицы шаговых управлений устанавливается по позиции точки минимума в выражении (2):

(3).

Полученный фронт используется для построения нового списка граничных точек. Процедура волновой фильтрации повторяется для нового списка граничных точек. Итерационный процесс заканчивается, когда новый фронт не содержит ни одной точки. После выполнения первой ступени все точки матрицы накопленных затрат, которые были равны ?, получают конечное положительное значение.

Ступень 2 и последующие. Последовательно просматриваются все точки карты, для которых q (y, x)? NaN и выполняется коррекция значений матриц Q и C, следуя формулам (2) и (3). Фильтрация заканчивается, когда матрицы Q двух смежных ступеней совпадают. Дополнительный эффект ускорения вычислений достигается за счет рекурсивного принципа фильтрации.

Построение оптимального маршрута. При построении маршрута возможны следующие варианты:

• 1. Стартовых точек несколько. Необходимо проложить маршрут от ближайшей стартовой точки к заданной конечной точке с координатами (yk, xk). В этом случае точки маршрута восстанавливаются по цепочке шаговых управлений:
• 2. Конечная точка не задана, но определено множество, которому она принадлежит. В этом случае на заданном множестве необходимо найти точку, в которой q (y, x)>min и далее повторить алгоритм пункта 1.
• 3. Если заданы начальная и конечная точки, то алгоритм необходимо выполнить для начального фронта, состоящего из одной стартовой точки.

Построение фронта транспортной доступности. В матрице накопленных затрат ищутся точки, удовлетворяющие условию: q (y, x)? L, где L — уровень затрат на линии фронта. Степень приближения может быть задана, например, в процентах от уровня L .