private void GetGraphPath(PathRegion pathRegion, int pEnd, int pStart) { List <Point3D> testFirstManipulator = new List <Point3D>(); int countGraphVerticesBeforeAdding = _graphVertices.Count; rbLog.AppendText("Добавляем вершины в граф..\n"); rbLog.ScrollToCaret(); rbLog.Refresh(); for (int i = 0; i < _iterations; i++) { Point3D currentPoint = new Point3D(TrueRandom.GetRandomInt(pathRegion.minX, pathRegion.maxX), TrueRandom.GetRandomInt(pathRegion.minY, pathRegion.maxY), TrueRandom.GetRandomInt(pathRegion.minZ, pathRegion.maxZ)); testFirstManipulator.Add(currentPoint); _graphVertices.Add(new GraphVertex(countGraphVerticesBeforeAdding + i, currentPoint, true)); } rbLog.AppendText("Удаление некорректных вершин..\n"); rbLog.ScrollToCaret(); rbLog.Refresh(); rbLog.AppendText("Расширяем матрицу весов..\n"); rbLog.ScrollToCaret(); rbLog.Refresh(); IsCorrectVertex(ref _graphVertices, _safeDistance, pEnd, pStart); _adjencyMatrixManipulator = ResizeAdjencyMatrixManipulator(_adjencyMatrixManipulator, _graphVertices.Count - _adjencyMatrixManipulator.GetLength(0)); int countLastAdjencyMatrix = _adjencyMatrixManipulator.GetLength(0); for (int i = countGraphVerticesBeforeAdding; i < _adjencyMatrixManipulator.GetLength(0); i++) { _graphVertices[i].Number = i; } rbLog.AppendText("Соединяем новые вершины..\n"); rbLog.ScrollToCaret(); rbLog.Refresh(); TryToConnect(_graphVertices, _distanceBetweenPoints, _adjencyMatrixManipulator, AlgoritmTryType.CleanDataWork); }
private void GetGraphPath(int pEnd, int pStart, PathRegion pathRegion) { List <Point3D> testFirstManipulator = new List <Point3D>(); rbLog.AppendText("Добавляем вершины..\n"); rbLog.ScrollToCaret(); rbLog.Refresh(); for (int i = 0; i < _iterations; i++) { Point3D currentPoint = new Point3D(TrueRandom.GetRandomInt(pathRegion.minX, pathRegion.maxX), TrueRandom.GetRandomInt(pathRegion.minY, pathRegion.maxY), TrueRandom.GetRandomInt(pathRegion.minZ, pathRegion.maxZ)); testFirstManipulator.Add(currentPoint); _graphVertices.Add(new GraphVertex(i, currentPoint, true)); } rbLog.AppendText("Удаление некорректных вершин..\n"); rbLog.ScrollToCaret(); rbLog.Refresh(); IsCorrectVertex(ref _graphVertices, _safeDistance, pEnd, pStart); _adjencyMatrixManipulator = new int[_graphVertices.Count, _graphVertices.Count]; for (int i = 0; i < _graphVertices.Count; i++) { _graphVertices[i].Number = i; } rbLog.AppendText("Составление матрицы смежности..\n"); rbLog.ScrollToCaret(); rbLog.Refresh(); TryToConnect(_graphVertices, _distanceBetweenPoints, _adjencyMatrixManipulator, AlgoritmTryType.CleanDataWork); }
public PathResultInformation(PathRegion region, List <Point3D> obstacles, int[,] matrix, List <GraphVertex> graphVerts, int safeDist) { regionGraph = region; obstaclesPoint = FormInfo(InformationType.obstacleList, obstacles); adjencyMatrix = FormInfo(InformationType.matrix, null, matrix); graphVertexes = FormInfo(InformationType.grapgVertexes, GetCoordFromGraphVertex(graphVerts)); safeDistance = safeDist; }
private static bool IsVertexInRegionCheck(Point3D vertex, PathRegion pathRegion) { if (vertex.X >= pathRegion.minX && vertex.X <= pathRegion.maxX && vertex.Y >= pathRegion.minY && vertex.Y <= pathRegion.maxY && vertex.Z >= pathRegion.minZ && vertex.Z <= pathRegion.maxZ) { return(true); } return(false); }
public static PathRegion ConnectTwoPathRegions(PathRegion oldRegion, PathRegion extraRegion) { return(new PathRegion( oldRegion.minX <extraRegion.minX?oldRegion.minX : extraRegion.minX, oldRegion.maxX> extraRegion.maxX ? oldRegion.maxX : extraRegion.maxX, oldRegion.minY <extraRegion.minY?oldRegion.minY : extraRegion.minY, oldRegion.maxY> extraRegion.maxY ? oldRegion.maxY : extraRegion.maxY, oldRegion.minZ <extraRegion.minZ?oldRegion.minZ : extraRegion.minZ, oldRegion.maxZ> extraRegion.maxZ ? oldRegion.maxZ : extraRegion.maxZ)); }
private void WorkAtOverAgain(ref Stack <int> stackFirstManipulator, ref PathResultInformation pathResultInformation) { int iteration = 0; Initalize(); int pathStackCapacity = -1; Tuple <int, int, int, int, int, int> region = GetRegionMarks(_startPoint, _endPoint); rbLog.AppendText("Определяем регион вхождения точек..\n"); rbLog.ScrollToCaret(); rbLog.Refresh(); PathRegion pathRegion = new PathRegion(region.Item1, region.Item4, region.Item2, region.Item5, region.Item3, region.Item6); do { rbLog.AppendText("Составляем вероятностную дорожную карту..\n"); rbLog.ScrollToCaret(); rbLog.Refresh(); GetGraphPath(1, 0, pathRegion); rbLog.AppendText("Обработка графа алгоритмом Дейкстры..\n"); rbLog.ScrollToCaret(); rbLog.ScrollToCaret(); rbLog.Refresh(); Deikstra(ref _graphVertices, _adjencyMatrixManipulator); rbLog.AppendText("Попытка найти путь в графе..\n"); rbLog.ScrollToCaret(); rbLog.Refresh(); stackFirstManipulator = GetPathAfterDeikstra(_graphVertices, _adjencyMatrixManipulator); pathStackCapacity = stackFirstManipulator.Count; if (stackFirstManipulator.Count > 1) { rbLog.AppendText("Путь найден..\n"); rbLog.ScrollToCaret(); rbLog.Refresh(); GetFinalPathAndDraw(stackFirstManipulator, Brushes.Purple, _graphVertices, _startPoint, _endPoint); } iteration++; if (iteration >= 2 && iteration <= 4) { rbLog.Text += "Расширяем регион карты..\n"; rbLog.ScrollToCaret(); rbLog.Refresh(); pathRegion = new PathRegion((pathRegion.minX - _safeDistance) > 0 ? pathRegion.minX - _safeDistance : 0, pathRegion.maxX + _safeDistance, (pathRegion.minY - _safeDistance) > 0 ? pathRegion.minY - _safeDistance : 0, pathRegion.maxY + _safeDistance, (pathRegion.minZ - _safeDistance) > 0 ? pathRegion.minZ - _safeDistance : 0, pathRegion.maxZ + _safeDistance); } }while (pathStackCapacity <= 1); rbLog.AppendText("Сохраняем решение в базу данных..\n"); rbLog.ScrollToCaret(); rbLog.Refresh(); pathResultInformation = new PathResultInformation( new PathRegion(region.Item1, region.Item4, region.Item2, region.Item5, region.Item3, region.Item6), _obstaclesPoints, _adjencyMatrixManipulator, _graphVertices, _safeDistance); }
public static RegionType GetRegionType(Point3D vertex, PathRegion regionDb) { if (vertex.X < regionDb.minX && vertex.Y > regionDb.minY && vertex.Y < regionDb.maxY && vertex.Z > regionDb.minZ && vertex.Z < regionDb.maxZ) { return(RegionType.One); } if (vertex.X > regionDb.minX && vertex.X < regionDb.maxX && vertex.Y < regionDb.minY && vertex.Z > regionDb.minZ && vertex.Z < regionDb.maxZ) { return(RegionType.Two); } if (vertex.X > regionDb.maxX && vertex.Y > regionDb.minY && vertex.Y < regionDb.maxY && vertex.Z > regionDb.minZ && vertex.Z < regionDb.maxZ) { return(RegionType.Three); } if (vertex.X > regionDb.minX && vertex.X < regionDb.maxX && vertex.Y > regionDb.minY && vertex.Y < regionDb.maxY && vertex.Z > regionDb.maxZ) { return(RegionType.Four); } if (vertex.X > regionDb.minX && vertex.X < regionDb.maxX && vertex.Y > regionDb.minY && vertex.Y < regionDb.maxY && vertex.Z < regionDb.minZ) { return(RegionType.Five); } if (vertex.X > regionDb.minX && vertex.X < regionDb.maxX && vertex.Y > regionDb.maxY && vertex.Z > regionDb.minZ && vertex.Z < regionDb.maxZ) { return(RegionType.Six); } return(RegionType.None); }
private static IList <Point3D> GetVertexesFromPointCloudInRegion(IList <Point3D> vertexes, PathRegion pathRegion) { List <Point3D> pointsInRegion = new List <Point3D>(); for (int i = 0; i < vertexes.Count; i++) { if (IsVertexInRegionCheck(vertexes[i], pathRegion)) { pointsInRegion.Add(vertexes[i]); } } return(pointsInRegion); }
public static int ComparerPointCloudComparerInPercenrage(IList <Point3D> currentPointCloud, IList <Point3D> databasePointCloud, PathRegion pathRegion) { int resultPercentage = 0; int countMatchingVertexes = 0; IList <Point3D> currentPointCloudVertexesInRegion = GetVertexesFromPointCloudInRegion(currentPointCloud, pathRegion); IList <Point3D> databasePointCloudVertexesInRegion = GetVertexesFromPointCloudInRegion(databasePointCloud, pathRegion); foreach (var point in currentPointCloudVertexesInRegion) { if (databasePointCloudVertexesInRegion.Contains(point)) { countMatchingVertexes++; } } try { resultPercentage = (countMatchingVertexes * 100) / databasePointCloudVertexesInRegion.Count; } catch (DivideByZeroException e) { resultPercentage = 0; } return(resultPercentage); }
public static Tuple <bool, bool> RegionCompare(Point3D startPoint, Point3D endPoint, PathRegion pathRegion, int safeDistance) { if (pathRegion.maxX + safeDistance > startPoint.X && pathRegion.maxX + safeDistance > endPoint.X && pathRegion.maxY + safeDistance > startPoint.Y && pathRegion.maxY + safeDistance > endPoint.Y && pathRegion.maxZ + safeDistance > startPoint.Z && pathRegion.maxZ + safeDistance > endPoint.Z && pathRegion.minX - safeDistance < startPoint.X && pathRegion.minX - safeDistance < endPoint.X && pathRegion.minY - safeDistance < startPoint.Y && pathRegion.minY - safeDistance < endPoint.Y && pathRegion.minZ - safeDistance < startPoint.Z && pathRegion.minZ - safeDistance < endPoint.Z) { return(new Tuple <bool, bool>(true, true)); } else if (pathRegion.maxX + safeDistance > startPoint.X && pathRegion.maxY + safeDistance > startPoint.Y && pathRegion.maxZ + safeDistance > startPoint.Z && pathRegion.minX - safeDistance < startPoint.X && pathRegion.minY - safeDistance < startPoint.Y && pathRegion.minZ - safeDistance < startPoint.Z) { return(new Tuple <bool, bool>(true, false)); } else if (pathRegion.maxX + safeDistance > endPoint.X && pathRegion.maxY + safeDistance > endPoint.Y && pathRegion.maxZ + safeDistance > endPoint.Z && pathRegion.minX - safeDistance < endPoint.X && pathRegion.minY - safeDistance < endPoint.Y && pathRegion.minZ - safeDistance < endPoint.Z) { return(new Tuple <bool, bool>(false, true)); } return(new Tuple <bool, bool>(false, false)); }
public static PathRegion GetNewPathRegionUsingDbRP(PathRegion oldRegion, RegionType regionType, Point3D vertex, int safeDistance) { PathRegion partionRegion = new PathRegion(); switch (regionType) { case RegionType.One: { partionRegion.minX = (int)(vertex.X - safeDistance); partionRegion.maxX = oldRegion.minX; partionRegion.minY = oldRegion.minY; partionRegion.maxY = oldRegion.maxY; partionRegion.minZ = oldRegion.minZ; partionRegion.maxZ = oldRegion.maxZ; break; } case RegionType.Two: { partionRegion.minX = oldRegion.minX; partionRegion.maxX = oldRegion.maxX; partionRegion.minY = (int)(vertex.Y - safeDistance); partionRegion.maxY = oldRegion.minY; partionRegion.minZ = oldRegion.minZ; partionRegion.maxZ = oldRegion.maxZ; break; } case RegionType.Three: { partionRegion.minX = oldRegion.maxX; partionRegion.maxX = (int)(vertex.X + safeDistance); partionRegion.minY = oldRegion.minY; partionRegion.maxY = oldRegion.maxX; partionRegion.minZ = oldRegion.minZ; partionRegion.maxZ = oldRegion.maxZ; break; } case RegionType.Four: { partionRegion.minX = oldRegion.minX; partionRegion.maxX = oldRegion.maxX; partionRegion.minY = oldRegion.minY; partionRegion.maxY = oldRegion.maxY; partionRegion.minZ = oldRegion.maxZ; partionRegion.maxZ = (int)(vertex.Z + safeDistance); break; } case RegionType.Five: { partionRegion.minX = oldRegion.minX; partionRegion.maxX = oldRegion.maxX; partionRegion.minY = oldRegion.minY; partionRegion.maxY = oldRegion.maxY; partionRegion.minZ = (int)(vertex.Z - safeDistance); partionRegion.maxZ = oldRegion.minZ; break; } case RegionType.Six: { partionRegion.minX = oldRegion.minX; partionRegion.maxX = oldRegion.maxX; partionRegion.minY = oldRegion.maxY; partionRegion.maxY = (int)(vertex.Y + safeDistance); partionRegion.minZ = oldRegion.minZ; partionRegion.maxZ = oldRegion.maxZ; break; } } return(partionRegion); }
private void SolutionFinderFromDb(Point3D vertex, PathRegion pathRegion, ref PathRegion finalPathRegion, ref bool isSolutionFind, int[,] adjencyMatrix, List <GraphVertex> graphVertices, int pEnd, int pStart) { rbLog.AppendText("Определение подходящего региона..\n"); rbLog.ScrollToCaret(); rbLog.Refresh(); RegionType regionType = PathRegion.GetRegionType(vertex, pathRegion); if (!regionType.Equals(RegionType.None)) { rbLog.AppendText("Регион определен. Обновление региона..\n"); rbLog.ScrollToCaret(); rbLog.Refresh(); PathRegion newExtraRegion = PathRegion.GetNewPathRegionUsingDbRP(pathRegion, regionType, vertex, _safeDistance); Stack <int> stackFirstManipulator = new Stack <int>(); int iteration = 0; int pathStackCapacity = -1; do { if (iteration.Equals(0)) { rbLog.AppendText("Достраиваем граф на первой итерации..\n"); rbLog.ScrollToCaret(); rbLog.Refresh(); GetGraphPath(newExtraRegion, pEnd, pStart); } else { rbLog.AppendText("Добавляем вершины по общему региону..\n"); rbLog.ScrollToCaret(); rbLog.Refresh(); GetGraphPath(finalPathRegion, pEnd, pStart); } rbLog.AppendText("Обрабатываем карту алгоритмом Дейкстры..\n"); rbLog.ScrollToCaret(); rbLog.Refresh(); pStart = GetIndexVertex(_startPoint); pEnd = GetIndexVertex(_endPoint); Deikstra(ref _graphVertices, _adjencyMatrixManipulator, pStart); rbLog.AppendText("Пытаемся найти путь..\n"); rbLog.ScrollToCaret(); rbLog.Refresh(); stackFirstManipulator = GetPathAfterDeikstra(_graphVertices, _adjencyMatrixManipulator, pEnd); pathStackCapacity = stackFirstManipulator.Count; rbLog.AppendText("Проверка, что путь найден..\n"); rbLog.ScrollToCaret(); rbLog.Refresh(); if (stackFirstManipulator.Count > 1) { rbLog.AppendText("Путь найден..\n"); rbLog.ScrollToCaret(); rbLog.Refresh(); GetFinalPathAndDraw(stackFirstManipulator, Brushes.Purple, _graphVertices, _startPoint, _endPoint); } else { rbLog.AppendText("Путь не найден..\n"); rbLog.ScrollToCaret(); rbLog.Refresh(); } rbLog.AppendText("Обновляем регион..\n"); rbLog.ScrollToCaret(); rbLog.Refresh(); finalPathRegion = PathRegion.ConnectTwoPathRegions(pathRegion, newExtraRegion); iteration++; }while (pathStackCapacity <= 1); } else { isSolutionFind = false; } }
private void btnStartAlgoritm_Click(object sender, EventArgs e) { _startPoint = new Point3D((double)nudStartXVertex.Value, (double)nudStartYVertex.Value, (double)nudStartZVertex.Value); _endPoint = new Point3D((double)nudEndXVertex.Value, (double)nudEndYVertex.Value, (double)nudEndZVertex.Value); _boxSize = new Size3D((int)nudWidthObject.Value, (int)nudLengthObject.Value, (int)nudHeightObject.Value); _percentageComparer = (int)nudPercentagePointCloud.Value; _iterations = (int)nudIterationCount.Value; _distanceBetweenPoints = (int)nudDistanceBetweenVertexes.Value; _safeDistance = GetSafeDistance(_boxSize); rbLog.AppendText("Обработка введенной информации..\n"); rbLog.ScrollToCaret(); rbLog.Refresh(); DateTimeOffset start = DateTimeOffset.Now; PathRegion finalPathRegion = new PathRegion(); Stack <int> stackFirstManipulator = new Stack <int>(); PathResultInformation pathResultInformation = new PathResultInformation(); DatabaseInformationInputer dbGet = new DatabaseInformationInputer(); rbLog.AppendText("Получение информации из базы данных..\n"); rbLog.ScrollToCaret(); rbLog.Refresh(); var getAllDb = (from p in dbGet.info join c in dbGet.pathRegions on p.regionGraph.id equals c.id where (c.maxX >= _startPoint.X && c.minX <= _startPoint.X && c.maxY >= _startPoint.Y && c.minY <= _startPoint.Y && c.maxZ >= _startPoint.Z && c.minZ <= _startPoint.Z) || (c.maxX >= _endPoint.X && c.minX <= _endPoint.X && c.maxY >= _endPoint.Y && c.minY <= _endPoint.Y && c.maxZ >= _endPoint.Z && c.minZ <= _endPoint.Z) select new { graphVertex = p.graphVertexes, adjencyMatrix = p.adjencyMatrix, obstaclesPoint = p.obstaclesPoint, pathRegion = p.regionGraph, safeDistance = p.safeDistance }).ToList(); rbLog.AppendText("Проверка подходящих вариантов из базы данных..\n"); rbLog.ScrollToCaret(); rbLog.Refresh(); if (!getAllDb.Count.Equals(0)) { for (int i = 0; i < getAllDb.Count; i++) { PathRegion pathRegion = getAllDb[i].pathRegion; Tuple <bool, bool> comparerResult = PathParametersComparer.RegionCompare(_startPoint, _endPoint, pathRegion, _safeDistance); bool isSolutionFind = false; if (!comparerResult.Equals(new Tuple <bool, bool>(false, false)) && ComparerItemsInalizer.ComparerPointCloudComparerInPercenrage(_obstaclesPoints, (List <Point3D>)CompleteSolutionAnalizer.GetObstaclesVertexes(getAllDb[i].obstaclesPoint), pathRegion) >= _percentageComparer && PathParametersComparer.AnalizeSafeDistance(_safeDistance, getAllDb[i].safeDistance)) { switch (comparerResult) { case var tuple when tuple.Item1.Equals(true) && tuple.Item2.Equals(false): { rbLog.AppendText("Стартовая точка входит в регион. Идет достроение веротяностной карты (графа)..\n"); rbLog.ScrollToCaret(); rbLog.Refresh(); isSolutionFind = true; _adjencyMatrixManipulator = CompleteSolutionAnalizer.GetAdjencyMatrix(getAllDb[i].adjencyMatrix); _graphVertices = CompleteSolutionAnalizer.GetGraphVertexes(getAllDb[i].graphVertex); _graphVertices.Add(new GraphVertex(_graphVertices.Count, _startPoint, true)); _graphVertices.Add(new GraphVertex(_graphVertices.Count, _endPoint, true)); SolutionFinderFromDb(_endPoint, pathRegion, ref finalPathRegion, ref isSolutionFind, _adjencyMatrixManipulator, _graphVertices, _graphVertices.Count - 1, _graphVertices.Count - 2); break; } case var tuple when tuple.Item1.Equals(false) && tuple.Item2.Equals(true): { rbLog.AppendText("Конечная точка входит в регион. Идет достроение вероятностной карты (графа)..\n"); rbLog.ScrollToCaret(); rbLog.Refresh(); isSolutionFind = true; _adjencyMatrixManipulator = CompleteSolutionAnalizer.GetAdjencyMatrix(getAllDb[i].adjencyMatrix); _graphVertices = CompleteSolutionAnalizer.GetGraphVertexes(getAllDb[i].graphVertex); _graphVertices.Add(new GraphVertex(_graphVertices.Count, _startPoint, true)); _graphVertices.Add(new GraphVertex(_graphVertices.Count, _endPoint, true)); SolutionFinderFromDb(_startPoint, pathRegion, ref finalPathRegion, ref isSolutionFind, _adjencyMatrixManipulator, _graphVertices, _graphVertices.Count - 1, _graphVertices.Count - 2); break; } case var tuple when tuple.Item1.Equals(true) && tuple.Item2.Equals(true): { rbLog.AppendText("Обе точки входят в регион..\n"); rbLog.AppendText("Добавляем точки в карту..\n"); rbLog.ScrollToCaret(); rbLog.Refresh(); _adjencyMatrixManipulator = CompleteSolutionAnalizer.GetAdjencyMatrix(getAllDb[i].adjencyMatrix); _graphVertices = CompleteSolutionAnalizer.GetGraphVertexes(getAllDb[i].graphVertex); List <GraphVertex> startEndVertexes = new List <GraphVertex>(); startEndVertexes.AddRange(new List <GraphVertex> { new GraphVertex(_graphVertices.Count, _startPoint, true), new GraphVertex(_graphVertices.Count + 1, _endPoint, true) }); _adjencyMatrixManipulator = ResizeAdjencyMatrixManipulator(_adjencyMatrixManipulator, startEndVertexes.Count); TryToConnect(_graphVertices, _distanceBetweenPoints, _adjencyMatrixManipulator, AlgoritmTryType.DatabaseDataWork, startEndVertexes); /////// _graphVertices.AddRange(startEndVertexes); Deikstra(ref _graphVertices, _adjencyMatrixManipulator, _graphVertices.Count - 2); stackFirstManipulator = GetPathAfterDeikstra(_graphVertices, _adjencyMatrixManipulator, _graphVertices.Count - 1); GetFinalPathAndDraw(stackFirstManipulator, Brushes.Purple, _graphVertices, _startPoint, _endPoint); finalPathRegion = pathRegion; isSolutionFind = true; break; } } if (isSolutionFind) { rbLog.AppendText("Решение найдено, происходит сохранение в базу данных..\n"); rbLog.ScrollToCaret(); rbLog.Refresh(); pathResultInformation = new PathResultInformation(finalPathRegion, _obstaclesPoints, _adjencyMatrixManipulator, _graphVertices, _safeDistance); break; } } if (!isSolutionFind) { _adjencyMatrixManipulator = new int[0, 0]; _graphVertices.Clear(); rbLog.AppendText("Не было найдено подходящих вариантов из базы данных..\n"); rbLog.ScrollToCaret(); rbLog.Refresh(); } } } else { rbLog.AppendText("Подходящих решений не найдено. Поиск нового решения..\n"); rbLog.ScrollToCaret(); rbLog.Refresh(); WorkAtOverAgain(ref stackFirstManipulator, ref pathResultInformation); } try { SavePathInfoInDatabase(pathResultInformation); } catch (Exception ex) { WorkAtOverAgain(ref stackFirstManipulator, ref pathResultInformation); SavePathInfoInDatabase(pathResultInformation); } MessageBox.Show($"Путь найден!\n{start.ToString()}\n{DateTimeOffset.Now.ToString()}\nВсего вершин: {_graphVertices.Count}", "Оповещение", MessageBoxButtons.OK, MessageBoxIcon.Information); }