Приветствую Вас, Хабровчане!

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

Сразу должен сказать, что я далеко не являюсь экспертом в области алгоритмов, а просто люблю программировать и изучать для собственного развития новые разделы математики (к которым в свое время почему-то не дошли руки). Это моя первая более-менее серьезная статья, в которой я решаюсь реализовать всем известный алгоритм. Поэтому прошу не судить меня строго. Статья предназначается в основном для новичков и тех, кто делает первые шаги в мир алгоритмов.

В этой статье я попробую реализовать алгоритм Дейкстры, что называется "в лоб" согласно описанию на википедии с использованием очереди c приоритетом (PriorityQueue) для хранения непосещённых вершин и применить его для нахождение кратчайшего пути между двумя вершинами связного взвешенного графа. А в качестве бонуса попробую запустить алгоритм для нахождения оптимального маршрута между двумя точками на 3D поверхности и посмотреть на полученные результаты.

В статье также отсутствуют строгие математические доказательства существования и единственности найденного пути, вычисление сложности алгоритма, сравнение с другими алгоритмами поиска и т.п. О существовании и корректности решения можно будет судить только по полученным результатам.

Предполагается, что читатель все-таки знаком с базовыми понятиями теории графов. На одну из книг дам ссылку в конце статьи.

Писать будем на языке C#. Исходники найдете здесь.

Поехали!

Содержание

• Постановка задачи

• Превращаем поверхность в граф

• Численная реализация

• Результаты расчетов. Простые препятствия

• Лабиринты

• Эксперименты с параметром MaxSlope

• Поиск оптимального пути на поверхности

• Заключение

Постановка задачи

Чтобы работать с графом, нам его нужно как-то задать и хранить в памяти компьютера. Существует несколько общепринятых способов представления графа, как то матрица смежности и матрица инцидентности. Но для своих целей я пойду другим путем.

Начну, пожалуй, с постановки задачи о поиске оптимального пути между двумя точками на 3D-поверхности, т.к. представление графа, которое будет использоваться мною для реализации алгоритма Дейкстры будет получено именно из "превращения" данной поверхности в связный граф.

В декартовой прямоугольной системе координат на плоскости задана равномерная сетка:

где — узлы сетки; — шаги сетки;— количество точек по осии, соответственно. В каждом узле сетки задано значение , представляющее собой высоту карты (ландшафта) местности в рассматриваемой точке. Значения , образующие исследуемую поверхность, могут быть отрицательными — в таком случае высоту карты следует понимать как глубину относительно некоторой нулевой отметки 

Заданы стартовая и целевая точка искомого маршрутаи, рисунок 1.

На выходе мы должны получить конечный список (массив) координат, следуя по которым мы доберемся из точкив точкунаиболее оптимальным образом.

Здесь сразу стоит отметить — для "плоского" случая, т.е. когда координата  нам не важна и алгоритм не будет от нее зависеть — мы будем искать именно кратчайший путь. В трехмерном случае будет также учитываться параметр, задающий максимальный угол уклона при поиске соседей и в этом случае мы будем искать оптимальный путь — такой путь, который выбирает более пологие участки поверхности.

Превращаем поверхность в граф

Представлять нашу поверхность в виде графа будем следующим образом, — посмотрим на нашу поверхность сверху (перпендикулярно плоскости ), вид будет примерно такой, рисунок 2:

Точки поверхности будут спроецированы на плоскость— это и будут вершины нашего графа. Но откуда появились эти "крестики" в каждом квадратике, спросите Вы ?! Эти крестики — тоже ребра графа, которые добавлены для того, чтобы у нас была возможность ходить не только по горизонтали и вертикали, но и по диагонали. Да, это сильно увеличит сложность алгоритма и время расчета, но зато путь будет еще более оптимальным.

Таким образом, граф будет задан с помощью матрицы (обозначим ее ) размерности , элемент которой — это вершина с координатами на нашем графе, построенного из 3D поверхности (рисунок 2). В процессе реализации алгоритма суть такого представления будет более понятна.

Наш граф должен быть взвешенным. Весребра, соединяющего смежные вершины и , заданные своими координатамибудет не что иное как расстояние в пространстве между соответствующими точками поверхности, из которой наш граф был построен:

Численная реализация

Я создам в Visual Studio консольный проект (.NET Framework 4.7.2). Ключевыми классами будут Point2D.cs, Vertex.cs и Graph.cs. Начнем с самого простого.

Класс Point2D будет содержать всего два свойства для хранения координат вершины графа на узловой сетке:

public class Point2D
    {
        public int i { get; }
        public int j { get; }

        public Point2D(int i, int j)
        {
            this.i = i;
            this.j = j;
        }
    }

С классом Vertex уже чуть поинтереснее:

public class Vertex
    {
        public Point2D Coordinate { get; set; }
        public double Height { get; set; }
        public Point2D CameFrom { get; set; }
        public double Label { get; set; }
        public bool IsVisited { get; set; }
        public bool IsObstacle { get; set; }

        public Vertex(int i, int j, Point2D CameFrom = null, double Height = 0.0, double Label = double.MaxValue, bool IsVisited = false, bool IsObstacle = false)
        {
            Coordinate = new Point2D(i, j);
            this.CameFrom = CameFrom;
            this.Height = Height;           
            this.Label = Label;
            this.IsVisited = IsVisited;
            this.IsObstacle = IsObstacle;
        }
    }

Опишу кратко свойства.

Coordinate — координаты вершины графа на узловой сетке; Height — высота, значение функции в точке вершины, необходимое для расчета весового коэффициента и величины уклона между двумя смежными вершинами (необходимо только в случае нахождения пути на 3D поверхности); CameFrom — здесь в процессе работы алгоритма будут храниться координаты вершины из которой мы попали в текущую вершину — на основании этого свойства мы в конце работы алгоритма сформируем наш искомый маршрут; Label — метка, хранящая значение длины пути из стартовой вершины в текущую; IsVisited — говорит нам, посетили ли мы данную вершину в процессе расчета или нет; IsObstacle — это свойство позволит задать определенные вершины как вершины-препятствия, чтобы выбрасывать их из рассмотрения наряду с уже посещенными.

Теперь о классе Graph.

public class Graph
    {
        /// <summary>
        /// Шаг сетки по оси Ox
        /// </summary>
        public double dx { get; }
        /// <summary>
        /// Шаг сетки по оси Oy
        /// </summary>
        public double dy { get; }
        /// <summary>
        /// Количество вершин по оси Ox
        /// </summary>
        public int N { get; }
        /// <summary>
        /// Количество вершин по оси Oy
        /// </summary>
        public int M { get; }
        /// <summary>
        /// Матрица вершин графа
        /// </summary>
        public Vertex[,] Vertices { get; }
        /// <summary>
        /// Предельная величина уклона в градусах
        /// </summary>
        public double MaxSlope { get; }
	}

Для работы алгоритма реализуем несколько вспомогательных методов.

Для расчета весов и уклона нам нужно будет получать "реальные" координаты вершины графа на плоскости (с учетом величины шагов и ):

(double, double) GetRealXY(Vertex vertex)
        {
            double x = vertex.Coordinate.i * dx;
            double y = vertex.Coordinate.j * dy;

            return (x, y);
        }

Вес ребра между смежными вершинами (расстояние между точками поверхности) находим так:

double Weight(Vertex v1, Vertex v2)
        {
            (double, double) x1y1 = GetRealXY(v1);
            (double, double) x2y2 = GetRealXY(v2);

            double xDiff = x1y1.Item1 - x2y2.Item1;
            double yDiff = x1y1.Item2 - x2y2.Item2;
            double zDiff = v1.Height - v2.Height;

            double sumOfSquares = Math.Pow(xDiff, 2.0) + Math.Pow(yDiff, 2.0) + Math.Pow(zDiff, 2.0);

            return Math.Sqrt(sumOfSquares);
        }

Следующий метод возвращает величину уклона между смежными вершинами (в градусах):

private double Slope(Vertex v1, Vertex v2)
        {
            double hypotenuse = Weight(v1, v2); // Вес ребра - это и есть по факту расстояние между точками
            double zDiffAbs = Math.Abs(v1.Height - v2.Height); // Модуль разности по высоте

            return Math.Asin(zDiffAbs / hypotenuse) * 180.0 / Math.PI; // Переводим радианы в градусы
        } 

Для удобства реализуем 8 методов для получения соседней вершины в зависимости от направления (я привел один из них):

private Vertex GetTopVertex(Vertex v) => Vertices[v.Coordinate.i, v.Coordinate.j + 1];

И еще 8 методов для определения принадлежности вершины той или иной части сетки (здесь я привел два из них):

private bool IsTopRightVertex(Vertex v1) => v1.Coordinate.i == N - 1 && v1.Coordinate.j == M - 1;

private bool IsVertexOnTheRightSide(Vertex v1) => v1.Coordinate.i == N - 1;

Эти методы понадобятся для получения всех смежных вершин для рассматриваемой вершины. Вот тут нам придется нарубить if-ов, т.к. количество смежных вершин не всегда одно и то же:

private List<Vertex> GetAllAdjacentVertices(Vertex vertex)
        {
            #region Рассматриваем угловые вершины

            if (IsTopRightVertex(vertex))
                return new List<Vertex>
                {
                    GetLeftVertex(vertex),
                    GetBottomLeftVertex(vertex),
                    GetBottomVertex(vertex)
                };

            if (IsBottomRightVertex(vertex))
                return new List<Vertex>
                {
                    GetTopVertex(vertex),
                    GetTopLeftVertex(vertex),
                    GetLeftVertex(vertex)
                };

            if (IsBottomLeftVertex(vertex))
                return new List<Vertex>
                {
                    GetTopVertex(vertex),
                    GetTopRightVertex(vertex),
                    GetRightVertex(vertex)
                };

            if (IsTopLeftVertex(vertex))
                return new List<Vertex>
                {
                    GetBottomVertex(vertex),
                    GetBottomRightVertex(vertex),
                    GetRightVertex(vertex)
                };

            #endregion

            #region Рассматриваем боковые вершины

            if (IsVertexOnTheTopSide(vertex))
                return new List<Vertex>
                {
                    GetLeftVertex(vertex),
                    GetBottomLeftVertex(vertex),
                    GetBottomVertex(vertex),
                    GetBottomRightVertex(vertex),
                    GetRightVertex(vertex)
                };

            if (IsVertexOnTheRightSide(vertex))
                return new List<Vertex>
                {
                    GetTopVertex(vertex),
                    GetTopLeftVertex(vertex),
                    GetLeftVertex(vertex),
                    GetBottomLeftVertex(vertex),
                    GetBottomVertex(vertex)
                };

            if (IsVertexOnTheBottomSide(vertex))
                return new List<Vertex>
                {
                    GetLeftVertex(vertex),
                    GetTopLeftVertex(vertex),
                    GetTopVertex(vertex),
                    GetTopRightVertex(vertex),
                    GetRightVertex(vertex)
                };

            if (IsVertexOnTheLeftSide(vertex))
                return new List<Vertex>
                {
                    GetTopVertex(vertex),
                    GetTopRightVertex(vertex),
                    GetRightVertex(vertex),
                    GetBottomRightVertex(vertex),
                    GetBottomVertex(vertex)
                };

            #endregion

            // Иначе вершина лежит "в середине карты" и нужно вернуть все 8 смежных вершин
            return new List<Vertex>
                {
                    GetTopVertex(vertex),
                    GetRightVertex(vertex),
                    GetBottomVertex(vertex),
                    GetLeftVertex(vertex),

                    GetTopRightVertex(vertex),
                    GetBottomRightVertex(vertex),
                    GetBottomLeftVertex(vertex),
                    GetTopLeftVertex(vertex)
                };
        }

Напишем метод, который будет "отсеивать неподходящих" соседей:

private List<Vertex> GetValidNeighbors(Vertex current)
        {
            // Из всех смежных вершин оставляем только те, которые 
            // 1. Еще не посещены
            // 2. Не являются вершинами-препятствиями
            // 3. Наклон к которым меньше заданной величины (например, 30 градусов)
            return GetAllAdjacentVertices(current).Where(v => !v.IsVisited && !v.IsObstacle && Slope(v, current) < MaxSlope).ToList();
        }

И вот он! Метод для поиска кратчайшего (оптимального) пути (и его длины):

public List<Point2D> FindShortestPathAndLength(Point2D startPoint, Point2D goalPoint, 
                                               out double shortestPathLength)
        {
            shortestPathLength = 0.0; // Здесь будет храниться длина искомого пути

            // Стартовой вершине присваиваем нулевую метку
            Vertex start = Vertices[startPoint.i, startPoint.j];
            start.Label = 0.0;

            // Сохраним отдельно целевую вершину
            Vertex goal = Vertices[goalPoint.i, goalPoint.j];

            // Очередь с приоритетом
            ConcurrentPriorityQueue<Vertex, double> priorityQueue = 
              new ConcurrentPriorityQueue<Vertex, double>(new MyDoubleComparer());
            // Добавляем стартовую вершину в очередь
            priorityQueue.Enqueue(start, start.Label);

            // Цикл заканчивает свою работу, когда очередь с приоритетом пустая, 
            // либо когда целевая вершина оказалась посещена (ранний выход)
            while (priorityQueue.Any() && !goal.IsVisited)
            {
                // Получаем из очереди с приоритетом вершину с минимальной меткой
                // (и одновременно удаляем эту вершину из очереди)
                Vertex current = priorityQueue.Dequeue();

                if (current.IsVisited)
                    continue;

                current.IsVisited = true;

                // Находим подходящих соседей: 
                // которые еще не посещены, не являются препятствиями и т.п.
                List<Vertex> neighbors = GetValidNeighbors(current);

                foreach (Vertex neighbor in neighbors)
                {
                    double currentWeight = current.Label + Weight(current, neighbor);
                    if (currentWeight < neighbor.Label)
                    {
                        neighbor.Label = currentWeight;
                        neighbor.CameFrom = current.Coordinate;
                        // Добавляем соседа в очередь с приоритетом 
                        // используя в качестве приоритета значение его метки
                        priorityQueue.Enqueue(neighbor, neighbor.Label);
                    }                    
                }          
            }

            // В конце работы алгоритма в целевой вершине в свойстве Label
            // будет находиться длина искомого пути
            shortestPathLength = goal.Label;
            // А с помощью свойства CameFrom сформируем и вернем сам искомый путь
            return GetShortestPath(goal);
        }

Здесь стоит отметить использование такой структуры данных как очередь с приоритетом, о которой я уже упоминал в самом начале статьи. Использование данной структуры позволяет ускорить вычисления благодаря оптимизированным методам добавления и удаления элементов в эту очередь. Кстати в .NET начиная с версии 6 эта структура идет из коробки. А в своем проекте я использовал очередь с приоритетом из этого источника. В нем по умолчанию используется максимальный элемент в начале очереди. Для того, чтобы поменять приоритет на "минимальный" (т.к. в алгоритме Дейкстра мы берем из непосещенных вершин ту, у которой метка минимальная) был добавлен класс MyDoubleComparer.

Алгоритм Дейкстры, по умолчанию, находит кратчайший путь от стартовой вершины до всех остальных вершин графа. Для моих же целей нужно лишь найти путь между двумя заданными вершинами графа, поэтому в цикл while добавлено условие раннего выхода !goal.IsVisited , следовательно алгоритм завершится, когда целевая вершина будет посещена.

Следующий метод на основе свойства CameFrom класса Vertex "собирает" и возвращает нам искомый путь:

private List<Point2D> GetShortestPath(Vertex goal)
        {
            List<Point2D> path = new List<Point2D>();
            
            path.Add(goal.Coordinate);
            Point2D cameFrom = goal.CameFrom;

            while (cameFrom != null)
            {
                Vertex vertex = Vertices[cameFrom.i, cameFrom.j];
                path.Add(vertex.Coordinate);
                cameFrom = vertex.CameFrom;
            }

            return path;
        }

Надо заметить, что массив, содержащий координаты искомого маршрута будет сформирован "задом на перед". Т.е. первым элементом массива будут координаты целевой вершины, а последним элементом — координаты стартовой.

Ну вот, в принципе, и все!

Весь остальной код, классы и методы, — являются вспомогательными (вычисление двумерного Гауссиана, запись/чтение из файла и т.п.). Если Вы дошли до этого абзаца и хорошо понимаете, что происходит, остальную часть, я думаю, понять Вам не составит труда. Поэтому не буду на нем останавливаться, тем более что код подробно прокомментирован.

Стоит, наверное, все-таки упомянуть о двух перегруженных конструкторах класса Graph, один из которых инициализирует граф с помощью поверхности (для расчетов я использовал только Гауссиан, можете поиграться с другими поверхностями), а другой конструктор инициализирует граф с помощью матрицы из нулей и единичек (матрица-препятствий) для создания сложных лабиринтов.

Давайте наконец посмотрим на результаты!

Результаты расчетов. Простые препятствия

Начнем с простого примера. Создадим на сетке небольшое вертикальное препятствие, рисунок 3, и позволим алгоритму обойти его:

Вершины графа, которые попадают под черную линию имеют свойство IsObstacle = true, чтобы пометить их как вершины-препятствия. Параметр уклона MaxSlope здесь не имеет смысла, т.к. алгоритм работает в одной плоскости.

Для чтения матрицы из csv-файла и других вспомогательных действий создан класс Obstacle.

Рассмотрим следующий пример, рисунок 4:

В данном случае я бы не сказал, что это является ошибкой, ведь алгоритм учитывает как препятствие только те вершины графа, которые непосредственно помечены как вершины-препятствия (IsObstacle = true).

А вот если мы сформируем препятствие вот так, тогда алгоритм никуда не денется и ему придется его обойти, рисунок 5:

Лабиринты

Усложним работу для алгоритма и сформируем для него целый лабиринт препятствий, рисунок 6:

Увеличим лабиринт и изменим целевую вершину, рисунок 7:

Все исходные файлы по которым были построены все вышеперечисленные препятствия и лабиринты, а также координаты найденных путей находятся в проекте в соответствующих папках.

Эксперименты с параметром MaxSlope

Теперь рассмотрим 3D поверхность, заданную функцией Гаусса, которая имитирует собой одиноко стоящую гору и дадим возможность алгоритму ее обойти. При этом параметр, задающий предельную величину уклона для поиска соседей будем варьировать, рисунок 8:

Как мы видим, при уменьшении величины MaxSlope (в градусах) алгоритм выбирает более пологий маршрут, обходя "гору" все дальше. При этом длина самого пути увеличивается.

Поиск оптимального пути на поверхности

Ну и в конце приведу еще пример поиска оптимального пути на поверхности, куда кроме "гор" и "впадины" были добавлены искусственные сооружения, рисунок 9:

Для имитации холмов и оврага были использованы двумерные функции Гаусса с различными параметрами. Для имитации искусственных сооружений в класс Graph был добавлен метод:

public void CreateBuilding(Point2D bottomLeftCoordinate, int width, int length, double height)
        {
            for (int i = bottomLeftCoordinate.i; i < bottomLeftCoordinate.i + width; i++)
            {
                for (int j = bottomLeftCoordinate.j; j < bottomLeftCoordinate.j + length; j++)
                    Vertices[i, j].Height = height;
            }
        }

Замечу, что здесь ни одна вершина графа не помечена как препятствие, алгоритм находит оптимальный путь благодаря заданию параметра MaxSlope.

И еще картинки с других ракурсов.

Заключение

Спасибо за прочтение!

Надеюсь статья была интересной и полезной. Все доработки и оптимизацию алгоритма оставляю Вам.

Литература:

Робин Уилсон. Введение в теорию графов. Пятое издание. 2019

Всем добра и качественного кода!