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