DijkstraAlgorithm

Реализация алгоритма Дейкстры для поиска кратчайшего пути между двумя вершинами и оптимального маршрута на 3D поверхности с использованием очереди с приоритетом (PriorityQueue).

Пример 1 (Обход простого препятствия)

Исходный файл obstacle1.csv:

0;0;0;0;0;0;0;0;0;0;0;0;0;0;00;0;0;0;0;0;0;0;0;0;0;0;0;0;00;0;0;0;0;0;0;1;0;0;0;0;0;0;00;0;0;0;0;0;0;1;0;0;0;0;0;0;00;0;0;0;0;0;0;1;0;0;0;0;0;0;00;0;0;0;0;0;0;1;0;0;0;0;0;0;00;0;0;0;0;0;0;1;0;0;0;0;0;0;00;0;0;0;0;0;0;1;0;0;0;0;0;0;00;0;0;0;0;0;0;0;0;0;0;0;0;0;00;0;0;0;0;0;0;0;0;0;0;0;0;0;0

Program.cs:

staticvoidMain(string[]args){// Создаем матрицу-препятствий из csv-файлаstringdocPath=Environment.GetFolderPath(Environment.SpecialFolder.MyDocuments);int[,]obstacleMatrix=Obstacle.CreateObstacleMatrixFromCSVFile(Path.Combine(docPath,"obstacle1.csv"));// Инициализируем граф с помощью этой матрицыGraphgraph=newGraph(obstacleMatrix);// Вычисляем кратчайший путьdoubleshortestPathLength=0.0;Point2DstartPoint=newPoint2D(3,4);Point2DgoalPoint=newPoint2D(12,4);List<Point2D>shortestPath=graph.FindShortestPathAndLength(startPoint,goalPoint,outshortestPathLength);// Записываем найденный путь в файл//WriteShortestPathToFile(shortestPath, Path.Combine(docPath, "shortestPath.txt"));Console.WriteLine("Coordinates of shortest path: ");foreach(Point2DpinshortestPath)Console.WriteLine(string.Format("({0}, {1})",p.i,p.j));Console.WriteLine(string.Format("Length of shortest path: {0}",shortestPathLength));Console.ReadLine();}

Вывод в консоль:

Coordinatesof shortest path:(12,4)(11,3)(10,2)(9,1)(8,1)(7,1)(6,2)(5,3)(4,4)(3,4)Length of shortestpath:11,4852813742386

Результат:

Пример 2 (Поиск оптимального пути на 3D-поверхности)

Program.cs:

staticvoidMain(string[]args){// Создаем несколько экземпляров параметров для Гауссиана для имитации гор (холмов) и одного оврагаGaussianParametergaussianParameter1=newGaussianParameter(1.5,0.5,0.5,2.0,4.0);GaussianParametergaussianParameter2=newGaussianParameter(1.0,0.5,0.5,7.5,1.0);GaussianParametergaussianParameter3=newGaussianParameter(-0.5,0.2,1.0,5.0,0.5);GaussianParametergaussianParameter4=newGaussianParameter(1.0,0.5,0.8,3.5,2.2);// Инициализируем графGraphgraph=newGraph(0.1,0.1,101,51,20.0,gaussianParameter1,gaussianParameter2,gaussianParameter3,gaussianParameter4);// Создаем искуственные сооружения на картеgraph.CreateBuilding(newPoint2D(57,20),2,20,0.3);graph.CreateBuilding(newPoint2D(64,16),3,5,0.3);graph.CreateBuilding(newPoint2D(18,4),2,5,0.3);graph.CreateBuilding(newPoint2D(10,14),4,2,0.4);graph.CreateBuilding(newPoint2D(64,29),5,2,0.4);graph.CreateBuilding(newPoint2D(14,24),5,2,0.3);// Записываем получившуюся поверхность в файлstringdocPath=Environment.GetFolderPath(Environment.SpecialFolder.MyDocuments);graph.WriteSurfaceToFile(Path.Combine(docPath,"surface.txt"));// Вычисляем кратчайший путьdoubleshortestPathLength=0.0;Point2DstartPoint=newPoint2D(92,7);Point2DgoalPoint=newPoint2D(14,21);List<Point2D>shortestPath=graph.FindShortestPathAndLength(startPoint,goalPoint,outshortestPathLength);// Записываем найденный путь в файлWriteShortestPathToFile(shortestPath,graph,Path.Combine(docPath,"shortestPath.txt"));Console.ReadLine();}

Результат работы программы:

• surface.txt --- Матрица со значениями 3D поверхности;
• shortestPath.txt --- файл с трехмерными координатами оптимального пути

Визуализация:

Ссылки

Хабр