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

Взвешенный граф транспортной сети задается квадратной матрицей затрат Z = z (i, j). Размер матрицы определяется числом вершин в графе. Каждый элемент матрицы представляет собой затраты на перемещение между смежными вершинами. В общем случае матрица может быть не симметрична (z (i, j)? z (j, i)). Если вершины не являются смежными, то значение элемента матрицы равно?. Кроме того элемент матрицы может иметь 0, если i = j или значение NaN — такой сегмент не участвует в построении оптимального маршрута. Накопленные затраты сохраняются в массиве Q. Каждый элемент массива соответствует вершине графа. Первоначально всем элементам массива Q присваивается значение?. Элементу массива Q присваивается значение NaN, если все сегменты окружения для соответствующей вершины графа имеют значение NaN.

Ступень 1. На первой ступени алгоритма накопление затрат происходит в виде волны от стартовых точек. Фронт волны строится следующим образом. Множество стартовых точек рассматривается как начальный фронт волны. Алгоритм фокусируется в стартовой точке, в матрице весов Z и массиве накопленных затрат Q. Просматриваются все сегменты и узлы ближайшего окружения. Точки, для которых значения в массиве Q равны Ѓ‡, и при этом соответствующие значения в матрице сегментных затрат Z конечны, переносятся в список граничных точек. Просматриваются все стартовые точки и строится общий список. Далее список редактируется, из него удалятся повторяющиеся точки. Логическая формула построения нового фронта имеет вид: iЃё Front если{q (i) = Ѓ‡ & z (i, j) Ѓ‚ Ѓ‡, NaN}. (4).

Фильтрация первой ступени. Алгоритм фокусируется в граничную точку i и выполняется вычисление нового значения накопленных затрат, следуя правилу:

(5).

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

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

Ступень 2 и последующие.

Последовательно просматриваются все точки зоны, для которых q (i) Ѓ‚ NaN и выполняется коррекция значений, массивов Q и C, следуя выражениям (5) и (6). Фильтрация заканчивается либо когда массивы двух смежных ступеней совпадают, либо после выполнения заданного числа ступеней фильтрации.

Построение оптимального маршрута.

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

1. Стартовых точек несколько. Необходимо проложить маршрут от ближайшей стартовой точки.

заданной конечной точке В этом случае узловые точки маршрута восстанавливаются по цепочке шаговых управлений:

• 2. Конечная узловая точка не задана, но определено множество, которому она принадлежит. В этом случае на заданном множестве необходимо найти узловую точку, в которой q (ik)ЃЁ min и далее повторить алгоритм пункта 1.
• 3. Если заданы начальная и конечная узловые точки, то алгоритм формирования массива накопленных затрат необходимо выполнить для начального фронта, состоящего из одной стартовой точки.