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);
}
