public IEnumerable <Vector2> GetPath(Vector2 start, Vector2 end) { var result = new List <Vector2>(); movedSegments = new List <Segment>(); // 1. Старт result.Add(start); Vector2 obstacleEnd = start; while (obstacleEnd != Vector2.zero) { // 2. Сразу проверяем можем ли построить путь без пересечений List <Vector2> intersectPoints = findIntersectPoints3(obstacleEnd, end, segments); if (intersectPoints.Count == 0) { result.Add(end); return(result); } // Вычисляем два пути: по часовой и против часовой стрелки PathDistance byEnd = calcPathByEnd(obstacleEnd, intersectPoints, start, end); PathDistance byStart = calcPathByStart(obstacleEnd, intersectPoints, start, end); PathDistance shortestDistance = byEnd.distance < byStart.distance ? byEnd : byStart; result.AddRange(shortestDistance.points); obstacleEnd = shortestDistance.obstacleEnd; } result.Add(end); return(result); }
public PathDistance calcPathByStart(Vector2 obstacleEnd, List <Vector2> intersectPoints, Vector2 start, Vector2 end) { // Находим ближайшую точку пересечения и ее сегмент, запоминаем Vector2 intersectNearestPoint = findNearestPoint(obstacleEnd, intersectPoints); Segment nearestSegment = findNearestSegment(intersectNearestPoint); // Проверяем можем ли с вершины обстакла дойти до финиша if (findIntersectPoints2(nearestSegment.start, end, segments) == 0) { PathDistance path1 = new PathDistance(); path1.addPoint(intersectNearestPoint); path1.accDistance(Vector2.Distance(start, intersectNearestPoint)); // Выбираем Начало отрезка и запоминаем path1.addPoint(nearestSegment.start); path1.accDistance(Vector2.Distance(intersectNearestPoint, nearestSegment.start)); path1.obstacleEnd = Vector2.zero; return(path1); } PathDistance path = new PathDistance(); path.addPoint(intersectNearestPoint); path.accDistance(Vector2.Distance(start, intersectNearestPoint)); // Выбираем Начало отрезка и запоминаем path.addPoint(nearestSegment.start); path.accDistance(Vector2.Distance(intersectNearestPoint, nearestSegment.start)); // Если не можем, ищем обстакл на котором располагается ближайший сегмент Obstacle obstacle = findObstacleBySegment(nearestSegment); // Идем по сегментам обстакла, пока не будем пересекать сами себя и не будем пересекать уже пройденные обстаклы, // или не построем путь из вершина до финиша while (!(findIntersectPoints2(nearestSegment.start, end, obstacle.segments) == 0 && findIntersectPoints2(nearestSegment.start, end, movedSegments.ToArray()) == 0)) { nearestSegment = obstacle.getNextSegmentByCounterClockWise(nearestSegment); // Нашли следующий сегмент обстакла, проверяем можем ли дойти до финиша без пересечений if (findIntersectPoints2(nearestSegment.start, end, segments) == 0) { path.addPoint(nearestSegment.start); path.accDistance(Vector2.Distance(nearestSegment.end, nearestSegment.start)); path.obstacleEnd = Vector2.zero; return(path); } // Не можем - запоминаем конец обстакла path.addPoint(nearestSegment.start); path.accDistance(Vector2.Distance(nearestSegment.end, nearestSegment.start)); } path.obstacleEnd = nearestSegment.start; movedSegments.AddRange(obstacle.segments.ToList()); return(path); }