Visibility graph и motion planning — различия между версиями

Материал из Викиконспекты
Перейти к: навигация, поиск
(Visibility graph)
м (rollbackEdits.php mass rollback)
 
(не показано 39 промежуточных версий 5 участников)
Строка 1: Строка 1:
== Нахождение пути между точками с препятствиями ==
+
== Нахождение любого пути между точками с препятствиями ==
 
{|align="right"
 
{|align="right"
 
|-valign="top"
 
|-valign="top"
Строка 5: Строка 5:
 
|[[Файл:notShort.png|300px|thumb|right|Такой путь не самый короткий]]
 
|[[Файл:notShort.png|300px|thumb|right|Такой путь не самый короткий]]
 
|}
 
|}
Рассмотрим задачу нахождения пути от точки <tex> S </tex> до <tex> T </tex> с препятствиями. Для начала рассмотрим движение материальной точки, случай, когда размером и формой движимого объекта пренебречь нельзя, будет [[Visibility graph и motion planning#Motion planning|позднее]].  
+
Для начала рассмотрим движение материальной точки. Случай, когда размером и формой движимого объекта пренебречь нельзя, будет рассмотрен [[Visibility graph и motion planning#Motion planning|позднее]].  
  
Обычно эта задача решается с помощью [[Трапецоидная карта | трапецоидной карты]], по которой строится граф, ребра которого соединяют центры трапедоидов и вершины <tex> S </tex> и <tex> T </tex> с серединами вертикальных сторон трапецоидов. В этом графе любым алгоритмом поиска кратчайших путей (например, алгоритмом [[Алгоритм Дейкстры|Дейкстры]] или [[Алгоритм A*|A*]]) находится путь от <tex> S </tex> до <tex> T </tex>.
+
Эту задачу можно решить с помощью [[Трапецоидная карта | трапецоидной карты]]. По ней строится граф, ребра которого соединяют центры трапедоидов, а также начальную и конечную вершины с серединами вертикальных сторон трапецоидов. В таком графе ищется путь между начальной и конечной вершинами.
  
Данный алгоритм работает за <tex> O(n \log n) </tex> и за линейное количество памяти и идеально подходит для нахождения какого-нибудь пути между конечными вершинами. Но иногда нужно найти кратчайший путь, и этот алгоритм не подходит, хоть и дает хорошее приближение. Однако решения нахождения кратчайшего пути в лучшем случае работают за <tex> O(n^2) </tex> времени и памяти (здесь и далее <tex> n </tex> {{---}} количество всех вершин).
+
Если точки лежат внутри одного трапецоида {{---}} ответ найден. Иначе идём из стартовой точки в центр её трапецоида, далее по построенным рёбрам ищем трапецоид содержащий финальную точку. Для этого можно использовать поиск в ширину или другой алгоритм нахождения кратчайшего пути в графе. В конечном итоге, соединяем середину последнего трапецоида с конечной вершиной.
  
== Visibility graph ==
+
Данный алгоритм работает за <tex> O(n \log n) </tex> и за линейное количество памяти и хорошо подходит для нахождения какого-нибудь пути между парой данных вершин. Но если нужно найти кратчайший путь, этот алгоритм не подходит, хоть и работает быстро. Однако, решения нахождения кратчайшего пути в лучшем случае работают за <tex> O(n^2) </tex> времени и памяти (здесь и далее <tex> n </tex> {{---}} количество всех вершин).
Рассмотрим точное решение нахождения кратчайшего пути на плоскости от точки <tex> S </tex> до <tex> T </tex> с полигональными препятствиями с помощью построения графа видимости. После его построения, как и в случае с трапецоидной картой, путь ищется стандартными алгоритмами.
 
  
Для простоты рассуждений вершины <tex> S </tex> и <tex> T </tex> будем считать вершинами полигонов.
+
== Нахождение кратчайшего пути между точками с препятствиями ==
 +
=== Visibility graph ===
 +
Рассмотрим точное решение нахождения кратчайшего пути на плоскости между двумя точками с полигональными препятствиями с помощью построения графа видимости. После его построения, как и в случае с трапецоидной картой, кратчайший путь ищется любым стандартным алгоритмом поиска (например, алгоритмом [[Алгоритм Дейкстры|Дейкстры]] или [[Алгоритм A*|A*]]).
 +
 
 +
Для простоты рассуждений начальную и конечную вершины будем считать вершинами полигонов.
  
 
{{Лемма
 
{{Лемма
Строка 26: Строка 29:
 
{{Определение
 
{{Определение
 
|definition =
 
|definition =
Говорят, что вершина <tex> u </tex> <i> видна </i>(англ. mutually visible) из <tex> v </tex>, если отрезок <tex> uv </tex> не пересекает ни одного препятствия.
+
Говорят, что вершина <tex> u </tex> ''видна'' (англ. mutually visible) из <tex> v </tex>, если отрезок <tex> uv </tex> не пересекает ни одного препятствия.
 
}}
 
}}
 
{{Определение
 
{{Определение
 
|definition =
 
|definition =
'''visibility graph''' {{---}} граф, вершины которого {{---}} вершины полигонов. Между вершинами <tex> u </tex> и <tex> v </tex> существует ребро, если из <tex> u </tex> видна <tex> v </tex>.
+
''Граф видимости'' (англ. visibility graph) {{---}} граф, вершины которого {{---}} вершины полигонов. Между вершинами <tex> u </tex> и <tex> v </tex> существует ребро, если из <tex> u </tex> видна <tex> v </tex>.
 
}}
 
}}
  
Строка 36: Строка 39:
  
 
{{Лемма
 
{{Лемма
 +
|about=О неиспользуемых вершинах
 
|statement=
 
|statement=
 
[[Файл:edgeToDelete.png|150px|thumb|right|Удаляем <tex> BD </tex>]]
 
[[Файл:edgeToDelete.png|150px|thumb|right|Удаляем <tex> BD </tex>]]
Если существуют вершины <tex> A, B, C </tex> одного препятствия и вершина <tex> D </tex> такая, что поворот <tex> DBA </tex> не совпадает с поворотом <tex> DBC </tex>, то ребро <tex> DB </tex> не принадлежит кратчайшему пути и его можно удалить из графа. (См. поясняющую картинку справа)
+
# Если существуют вершины <tex> A, B, C </tex> одного препятствия и вершина <tex> D </tex> такая, что поворот <tex> DBA </tex> не совпадает с поворотом <tex> DBC </tex>, то ребро <tex> DB </tex> не принадлежит кратчайшему пути и его можно удалить из графа. (См. поясняющую картинку справа)
 +
# Все внутренние вершины, кроме вырожденного случая, (начальная/конечная точка лежит внутри выпуклой оболочки фигуры) можно игнорировать.
 
|proof=
 
|proof=
 
[[Файл:edgeNotToDelete.png|200px|thumb|right|Не удаляем <tex> BS </tex>]]
 
[[Файл:edgeNotToDelete.png|200px|thumb|right|Не удаляем <tex> BS </tex>]]
Путь проходящий через ребро <tex> BD </tex> будет длиннее, чем через соседей точки <tex> B </tex>, так как по неравенству треугольника <tex> AB + BD < AD </tex>
+
# Путь проходящий через ребро <tex> BD </tex> будет длиннее, чем через соседей точки <tex> B </tex>, так как по неравенству треугольника <tex> AB + BD > AD </tex>
 +
# Если случай не вырожденный, значит заход внутрь фигуры только увеличит суммарный путь, так как по неравенству треугольника расстояние между соседними выпуклыми вершинами всегда меньше суммы расстояний с учётом внутренней.
 
}}
 
}}
  
По доказанным леммам любое ребро кратчайшего пути содержится в графе, таким образом для нахождения кратчайшего пути осталось найти кратчайший путь в этом графе от <tex> S </tex> до <tex> T </tex>.
+
По доказанным леммам любое ребро кратчайшего пути содержится в графе. Таким образом, для нахождения кратчайшего пути осталось найти кратчайший путь в этом графе от начальной до конечной вершины.
  
 
=== Построение visibility графа ===
 
=== Построение visibility графа ===
 
==== Наивный алгоритм. <tex> O(n ^ 3) </tex> ====
 
==== Наивный алгоритм. <tex> O(n ^ 3) </tex> ====
Если делать наивно, то есть для каждой пары вершин проверять можно ли добавить ли такое ребро(нет ли пересечений с полигонами), будет <tex> O(n^3) </tex>.
+
Для каждой пары вершин проверяем, можно ли добавить ребро между ними, то есть нет ли пересечений с полигонами. <tex> O(n^2) </tex> пар вершин и <tex> O(n) </tex> ребер, то есть <tex> O(n^3) </tex>.
  
 
==== Lee’s Algorithm. <tex> O(n ^ 2 \log n) </tex> ====
 
==== Lee’s Algorithm. <tex> O(n ^ 2 \log n) </tex> ====
Строка 54: Строка 60:
 
|
 
|
 
[[Файл:Zam.png|300px|thumb|left|Заметание плоскости вращающимся лучом]]
 
[[Файл:Zam.png|300px|thumb|left|Заметание плоскости вращающимся лучом]]
Однако можно это сделать за <tex> O(n ^ 2 \log  n) </tex>. Идея алгоритма проста : для каждой вершины найдем видимые из нее вершины независимо. Если научиться делать это за <tex> O(n \log  n) </tex>, задача решена, так как всего точек <tex> n </tex>.
+
Однако можно это сделать за <tex> O(n ^ 2 \log  n) </tex>. Идея алгоритма проста: для каждой вершины найдем видимые из нее вершины. Если научиться делать это за <tex> O(n \log  n) </tex>, задача решена, так как всего точек <tex> n </tex>.
  
Будем рассматривать точки слева направо, таким образом потребуется рассматривать только правую половину плоскости, так как ребра, которые должны идти в левую будут уже добавлены ранее.
+
Для каждой вершины будем рассматривать только правую половину плоскости, так как ребра, которые должны идти в левую половину, будут исходить из вершин, для которых текущая вершина будет справа.
  
Переформулируем задачу : дана точка <tex> v </tex> и множество отрезков {{---}} ребер препятствий.  
+
Переформулируем задачу. Дано: точка <tex> v </tex> и множество отрезков {{---}} ребер препятствий.  
Найти : множество концов отрезков, видимых из <tex> v </tex>.
+
Найти: множество концов отрезков, видимых из <tex> v </tex>.
  
 
Для решения этой задачи будем использовать заметающий луч с началом в точке <tex> v </tex>. Его статусом будут отрезки, которые его пересекают, упорядоченные по возрастанию расстояния от точки <tex> v </tex> до точки пересечения. Точками событий будут концы отрезков.  
 
Для решения этой задачи будем использовать заметающий луч с началом в точке <tex> v </tex>. Его статусом будут отрезки, которые его пересекают, упорядоченные по возрастанию расстояния от точки <tex> v </tex> до точки пересечения. Точками событий будут концы отрезков.  
  
Изначально, пустим луч из рассматриваемой вершины вертикально вверх и добавим в статус все отрезки, которые он пересекает, по увеличению расстояния до них. Теперь будем рассматривать точки <tex> w \in V </tex> в порядке сортировки по углу между <tex> v </tex> и вертикальной полуосью <tex> l </tex>. При таком обходе проверка видимости вершины будет выполняться за <tex> O(1) </tex>, так как достаточно проверить пересечение с отрезком, первым в статусе. Действительно, если вершина не видна, то отрезок <tex> vw </tex> пересекает несколько отрезков, лежащих перед <tex> w </tex>, а значит и ближайший, то есть первый в статусе. В противном случае все пересекаемые лучом отрезки лежат за вершиной <tex> w </tex> и пересечения отрезка <tex> uw </tex> с ближайшим отрезком в статусе не будет. Вне зависимости от видимости вершины, необходимо изменить статус заметающего луча. Для этого необходимо удалить из статуса все отрезки, которые заканчиваются вершине <tex> w </tex> (лежат слева от прямой <tex> vw </tex>) и добавить все отрезки, которые в ней начинаются (лежат справа от прямой <tex> vw </tex>).
+
Пустим луч из рассматриваемой вершины <tex> v </tex> вертикально вверх и добавим в статус все отрезки, которые он пересекает, по увеличению расстояния до них. Теперь будем рассматривать точки <tex> w \in V </tex> в порядке сортировки по углу между <tex> v </tex> и вертикальной полуосью <tex> l </tex>. При таком обходе для проверки видимости вершины достаточно проверить пересечение с ближайшим к <tex> v </tex> отрезком, то есть первым в статусе(так как отрезки отсортированы по расстоянию до них). Действительно, если вершина <tex> w </tex> не видна, то отрезок <tex> vw </tex> пересекает несколько отрезков, лежащих перед <tex> w </tex>, а значит и ближайший. В противном случае все пересекаемые лучом отрезки лежат за вершиной <tex> w </tex> и пересечения отрезка <tex> vw </tex> с ближайшим отрезком не будет. Вне зависимости от видимости вершины, необходимо изменить статус заметающего луча. Для этого необходимо удалить из статуса все отрезки, которые заканчиваются вершине <tex> w </tex> (лежат слева от прямой <tex> vw </tex>) и добавить все отрезки, которые в ней начинаются (лежат справа от прямой <tex> vw </tex>).
  
 
===== Псевдокод =====
 
===== Псевдокод =====
<pre>
+
graph buildVisibilityGraph(Set<Segment> segments)
graph buildVisibilityGraph(Set<Segment> segments)
+
    vertices = getVertices(segments) <tex> \cup\ \{s,\ t\} </tex>
  Set<Vertex> vertices = getVertices(segments)       //получаем все вершины препятствий
+
    graph = visibilityGraph(vertices)                
  graph visibilityGraph(vertices)                   //изначально в графе только вершины
+
    '''for''' Vertex <tex>v</tex> '''in''' vertices
  for Vertex v in vertices                           //для каждой вершины
+
      '''for''' Vertex <tex>w</tex> '''in''' getVisibleVertices(<tex>v</tex>, segments)
      for Vertex w in getVisibleVertices(v, segments) //добавляем в граф все видимые из нее вершины
+
          visibilityGraph.addEdge(<tex>v</tex>, <tex>w</tex>)
        visibilityGraph.addEdge(v, w)
+
    '''return''' visibilityGraph
  return visibilityGraph
+
Здесь функция getVisibleVertices(<tex> v </tex>) возвращает все видимые из <tex> v </tex> вершины и выглядит так:
</pre>
+
Set<Vertex> getVisibleVertices(Vertex <tex>v</tex>, Set<Segment> segments)
Здесь функция getVisibleVertices(<tex> v </tex>) возвращет все видимые из <tex> v </tex> вершины и выглядит так:
+
    Set<Vertex> answer
<pre>
+
    '''for''' Segment <tex>s</tex> '''in''' segments
vector<Vertex> getVisibleVertices(vertex v, set<segment> segments)
+
      '''if''' intersect(<tex> s </tex>, <tex> l </tex>)
// Инициализируем статус
+
          status.add(<tex>s</tex>)
  for segment s in segments
+
    '''for''' Point <tex>w</tex> '''in''' segments
      if intersection s and ray from v to up exists
+
      '''if''' <tex>v.x \leqslant w.x</tex>
        status.add(s)
+
          currentVertices.add(<tex>w</tex>)
// Инициализируем множество вершин, которые нужно рассматривать
+
    sort(currentVertices) by angle
  for point w in segments
+
    '''for''' Point <tex>w</tex> '''in''' currentVertices
      if w.x >= v.x
+
      '''if''' '''not''' intersect(<tex>vw</tex>, status.closest)
        currentVertices.add(w)
+
          answer.add(<tex>w</tex>)
  sort(currentVertices) by angle
+
      '''for''' Segment <tex>s</tex> ending in <tex>w</tex>
// Для каждой вершины проверяем, видима ли она и обновляем статус
+
          status.delete(<tex>s</tex>)
  for w in currentVertices
+
      '''for''' Segment <tex>s</tex> beginning in <tex>w</tex>
      if intersection vw and status.first not exists
+
          status.add(<tex>s</tex>)
        answer.add(w)
+
    '''return''' answer
      delete from status all edges ending in w
+
В качестве статуса нужно использовать структуру данных, позволяющую добавлять и удалять из нее отрезки за <tex> O(\log n) </tex> и извлекать минимум за <tex> O(1) </tex> или <tex> O(\log n) </tex>. В этом случае достигается асимптотика <tex> O(n^2 \log n) </tex>, так как для каждой из <tex> n </tex> точек выполняется сортировка за <tex> O(n \log n) </tex>, обновление статуса (суммарно <tex> O(n \log n) </tex>, так как каждый отрезок добавляется и удаляется из статуса не более одного раза) и запросы ближайшего отрезка (<tex> O(\log n) </tex> или <tex> O(1) </tex> на точку, то есть <tex> O(n \log n) </tex> или <tex> O(n) </tex>).  
      add in status all edges beginning in w
 
  return answer
 
</pre>
 
В качестве статуса нужно использовать структуру данных, позволяющую добавлять и удалять из нее отрезки за <tex> O(\log n) </tex> и извлекать минимум за <tex> O(1) </tex> или <tex> O(\log n) </tex>. В этом случае достигается асимптотика <tex> O(n^2 \log n) </tex>, так как для каждой из <tex> n </tex> точек выполняется сортировка(<tex> O(n \log n) </tex>), обновление статуса (суммарно <tex> O(n \log n) </tex>, так как каждый отрезок добавляется и удаляется из статуса только один раз) и запросы ближайшего отрезка (<tex> O(\log n) </tex> или <tex> O(1) </tex> на точку, то есть <tex> O(n \log n) </tex> или <tex> O(n) </tex>).  
 
 
|
 
|
[[Файл:Zamrefr1.png|250px|thumb|right|Обновление статуса заметающего луча]]
+
[[Файл:Zamrefr1.png|250px|thumb|right|Обновление статуса заметающего луча: добавляем ребра <tex> w_1 w_5 </tex> и <tex> w_1 w_2 </tex> в статус]]
[[Файл:Zamrefr2.png|250px|thumb|right|Обновление статуса заметающего луча]]
+
[[Файл:Zamrefr2.png|250px|thumb|right|Добавляем ребра <tex> w_3 w_2 </tex> и <tex> w_3 w_4 </tex> в статус]]
[[Файл:Zamrefr3.png|250px|thumb|right|Обновление статуса заметающего луча]]
+
[[Файл:Zamrefr3.png|250px|thumb|right|Удаляем ребра <tex> w_3 w_2 </tex> и <tex> w_1 w_2 </tex> из статуса]]
 
|}
 
|}
  
==== Overmars and Welzl’s Algorithm <tex> O(n ^ 2) </tex> ====
+
== Motion planning ==
[http://igitur-archive.library.uu.nl/math/2006-1214-201604/overmars_88_new_methods.pdf visibility graph при помощи rotation tree]
+
[[Файл:mink.png|200px|thumb|left|Изменяем препятствия]]
 +
[[Файл:mink2.png|400px|thumb|right|Ищем путь для точки]]
 +
Рассмотрим задачу нахождения кратчайшего пути, когда движимый объект {{---}} это выпуклый полигон. Например, робот, которого надо доставить из начальной в конечную точку.
  
C помощью [http://bit.ly/1eEqTzk rotation tree] можно достичь асимптотики <tex> O(n^2) </tex>.
+
Если полигон вращать нельзя, задачу сводится к движению точки так: выбирается точка на полигоне, которая принимается за начало координат. В такой системе координат для каждого препятствия считается [[Сумма Минковского (определение, вычисление)|сумма Минковского]] с полигоном. Получаются бОльшие препятствия, но теперь достаточно двигать выбранную точку, что было описано выше.
  
== Motion planning ==
+
Если полигон можно вращать, задача нахождения ''кратчайшего'' пути становится достаточно ресурсоёмка, поэтому обычно рассматривают задачу нахождения какого-нибудь пути между конечными точками.
[[Файл:mink.png|200px|thumb|left|Раздуваем препятствия]]
 
[[Файл:mink2.png|400px|thumb|right|Ищем путь для точки]]
 
Тут мы двигаем не точку, а произвольный выпуклый полигон. Если мы его не можем вращать, просто считаем configuration space, т. е. "обводим" препятствия нашим полигоном (делаем [[Сумма Минковского (определение, вычисление)|сумму Минковского]] препятствий и полигона, сдвинутого в начало координат какой-нибудь точкой) и получаем другие препятствия, но зато теперь мы двигаем точку. А это мы уже научились делать выше.
 
  
Теперь рассмотрим случай, когда мы можем вращать полигон. Для начала построим [[Трапецоидная карта|трапецоидную карту]], как будто мы не можем вращать полигон. Теперь будем вращать полигон на малый угол, пока он не сделает полный оборот вокруг своей оси, и для каждого угла сделаем трапецоидную карту. Теперь разместим(мысленно) все карты друг над другом. Таким образом получится, что поворот на малый угол {{---}} это движение вверх/вниз между слоями. Осталось [[Пересечение многоугольников (PSLG overlaying)|попересекать]] соседние слои и добавить между ними ребра (помним что первый и последний слои одинаковы) и уже в этом графе найти путь.
+
Первый шаг решения этой задачи совпадает с предыдущим случаем: выберем точку и построим [[Сумма Минковского (определение, вычисление)|сумму Минковского]] препятствий с полигоном. Рассмотрим малый угол <tex> \epsilon </tex>. Представим, что поворот полигона на этот угол {{---}} это движение вверх-вниз между слоями, на каждом из которых посчитана сумма Минковского с полигоном, повернутым на этот угол.
  
При такой реализации в некоторых случаях у нас может возникнуть ошибка в повороте, так как в одной плоскости мы все можем делать точно: положения на соседних слоях могут допускаться, а повернуть мы не сможем. Это лечится в основном двумя способами: измельчением угла поворота и изначальным сглаживанием углов полигона {{---}} повращать полигон на <tex> +\epsilon </tex> и <tex> -\epsilon </tex> и объединить с исходным, получив новый полигон.
+
На каждом слое построим трапецоидную карту и граф, как описано в [[Visibility graph и motion planning#Нахождение пути между точками с препятствиями|начале]]. Если [[Пересечение многоугольников (PSLG overlaying)|пересечь]] соседние слои и добавить между их графами ребра, получится один большой граф, в котором ищется кратчайший путь.
  
Так как эта задача достаточно ресурсоемка, мы рассматриваем только наличие пути, а не нахождение кратчайшего.
+
При таком подходе может возникнуть ошибка при пересечении слоев: на каждом слое состояния будут допустимые, а осуществить поворот физически будет невозможно. Обычно, эту проблему решают двумя способами: измельчением угла поворота и изначальным сглаживанием углов полигона. Первый способ повышает не только точность решения, но и вычислительную сложность задачи. Второй подход практически исключает возможность нахождения пути, когда его нет, но повышает вероятность "ненахождения" пути, когда он есть.
  
== Источники ==
+
== Источники информации ==
* Mark de Berg, Otfried Cheong, Marc van Kreveld, Mark Overmars (2008), Computational Geometry: Algorithms and Applications (3rd edition), Springer-Verlag, ISBN 978-3-540-77973-5 Chapter 15 page 324-331
+
* Mark de Berg, Otfried Cheong, Marc van Kreveld, Mark Overmars, Computational Geometry: Algorithms and Applications {{---}} Third edition {{---}} Springer, 2008. {{---}} Chapter 15. {{---}} ISBN 978-3-540-77973-5
* [http://www.academia.edu/2845047/3D_Visibility_Graph статья] про visibility graphs на academia.edu
+
* [http://www.academia.edu/2845047/3D_Visibility_Graph Academia.edu] {{---}} 3D Visibility Graph
* [http://habrahabr.ru/post/199256/ Хабр]
+
* [http://habrahabr.ru/post/199256/ Хабрахабр] {{---}} Motion planning: граф видимости, дорожные карты
 +
* [http://igitur-archive.library.uu.nl/math/2006-1214-201604/overmars_88_new_methods.pdf igitur-archive.library.uu.nl] {{---}} Visibility graph при помощи rotation tree за <tex>O(n^2)</tex>.
 
[[Категория: Вычислительная геометрия]]
 
[[Категория: Вычислительная геометрия]]
  
 
== Ссылки ==
 
== Ссылки ==
* Моя [https://github.com/Igorjan94/cg/blob/master/include/cg/algorithms/visibilityGraph.h реализация] алгоритма за <tex> O(n^2 \log n) </tex>. Далеко от идеального, но работает
+
* [https://github.com/Igorjan94/cg/blob/master/include/cg/algorithms/visibilityGraph.h Github] {{---}} Реализация алгоритма за <tex> O(n^2 \log n) </tex>

Текущая версия на 19:39, 4 сентября 2022

Нахождение любого пути между точками с препятствиями

Путь с препятствиями через трапецоидную карту
Такой путь не самый короткий

Для начала рассмотрим движение материальной точки. Случай, когда размером и формой движимого объекта пренебречь нельзя, будет рассмотрен позднее.

Эту задачу можно решить с помощью трапецоидной карты. По ней строится граф, ребра которого соединяют центры трапедоидов, а также начальную и конечную вершины с серединами вертикальных сторон трапецоидов. В таком графе ищется путь между начальной и конечной вершинами.

Если точки лежат внутри одного трапецоида — ответ найден. Иначе идём из стартовой точки в центр её трапецоида, далее по построенным рёбрам ищем трапецоид содержащий финальную точку. Для этого можно использовать поиск в ширину или другой алгоритм нахождения кратчайшего пути в графе. В конечном итоге, соединяем середину последнего трапецоида с конечной вершиной.

Данный алгоритм работает за [math] O(n \log n) [/math] и за линейное количество памяти и хорошо подходит для нахождения какого-нибудь пути между парой данных вершин. Но если нужно найти кратчайший путь, этот алгоритм не подходит, хоть и работает быстро. Однако, решения нахождения кратчайшего пути в лучшем случае работают за [math] O(n^2) [/math] времени и памяти (здесь и далее [math] n [/math] — количество всех вершин).

Нахождение кратчайшего пути между точками с препятствиями

Visibility graph

Рассмотрим точное решение нахождения кратчайшего пути на плоскости между двумя точками с полигональными препятствиями с помощью построения графа видимости. После его построения, как и в случае с трапецоидной картой, кратчайший путь ищется любым стандартным алгоритмом поиска (например, алгоритмом Дейкстры или A*).

Для простоты рассуждений начальную и конечную вершины будем считать вершинами полигонов.

Лемма (О кратчайшем пути):
Любой кратчайший путь между двумя вершинами с полигональными препятствиями представляет собой ломаную, вершины которой — вершины полигонов.
Доказательство:
[math]\triangleright[/math]
Short cut
Пусть кратчайший путь — не ломаная. В таком случае, на пути существует такая точка [math] p [/math], которая не принадлежит ни одному прямому отрезку. Это означает, что существует [math]\epsilon[/math]-окрестность точки [math] p [/math], в которую не попадает ни одно препятствие (случай, когда точка попала на ребро рассматривается аналогично). В таком случае, подпуть, который находится внутри [math]\epsilon[/math]-окрестности, по неравенству треугольника может быть сокращён по хорде, соединяющий точки пересечения границы [math]\epsilon[/math]-окрестности с путем. Раз часть пути может быть уменьшена, значит и весь путь может быть уменьшен, а значит исходное предположение некорректно.
[math]\triangleleft[/math]
Определение:
Говорят, что вершина [math] u [/math] видна (англ. mutually visible) из [math] v [/math], если отрезок [math] uv [/math] не пересекает ни одного препятствия.


Определение:
Граф видимости (англ. visibility graph) — граф, вершины которого — вершины полигонов. Между вершинами [math] u [/math] и [math] v [/math] существует ребро, если из [math] u [/math] видна [math] v [/math].


В худшем случае в таком графе может быть [math] O(n^2) [/math] ребер. Однако по некоторым ребрам кратчайший путь точно не пройдет, и такие ребра из графа можно удалить.

Лемма (О неиспользуемых вершинах):
Удаляем [math] BD [/math]
  1. Если существуют вершины [math] A, B, C [/math] одного препятствия и вершина [math] D [/math] такая, что поворот [math] DBA [/math] не совпадает с поворотом [math] DBC [/math], то ребро [math] DB [/math] не принадлежит кратчайшему пути и его можно удалить из графа. (См. поясняющую картинку справа)
  2. Все внутренние вершины, кроме вырожденного случая, (начальная/конечная точка лежит внутри выпуклой оболочки фигуры) можно игнорировать.
Доказательство:
[math]\triangleright[/math]
Не удаляем [math] BS [/math]
  1. Путь проходящий через ребро [math] BD [/math] будет длиннее, чем через соседей точки [math] B [/math], так как по неравенству треугольника [math] AB + BD \gt AD [/math]
  2. Если случай не вырожденный, значит заход внутрь фигуры только увеличит суммарный путь, так как по неравенству треугольника расстояние между соседними выпуклыми вершинами всегда меньше суммы расстояний с учётом внутренней.
[math]\triangleleft[/math]

По доказанным леммам любое ребро кратчайшего пути содержится в графе. Таким образом, для нахождения кратчайшего пути осталось найти кратчайший путь в этом графе от начальной до конечной вершины.

Построение visibility графа

Наивный алгоритм. [math] O(n ^ 3) [/math]

Для каждой пары вершин проверяем, можно ли добавить ребро между ними, то есть нет ли пересечений с полигонами. [math] O(n^2) [/math] пар вершин и [math] O(n) [/math] ребер, то есть [math] O(n^3) [/math].

Lee’s Algorithm. [math] O(n ^ 2 \log n) [/math]

Заметание плоскости вращающимся лучом

Однако можно это сделать за [math] O(n ^ 2 \log n) [/math]. Идея алгоритма проста: для каждой вершины найдем видимые из нее вершины. Если научиться делать это за [math] O(n \log n) [/math], задача решена, так как всего точек [math] n [/math].

Для каждой вершины будем рассматривать только правую половину плоскости, так как ребра, которые должны идти в левую половину, будут исходить из вершин, для которых текущая вершина будет справа.

Переформулируем задачу. Дано: точка [math] v [/math] и множество отрезков — ребер препятствий. Найти: множество концов отрезков, видимых из [math] v [/math].

Для решения этой задачи будем использовать заметающий луч с началом в точке [math] v [/math]. Его статусом будут отрезки, которые его пересекают, упорядоченные по возрастанию расстояния от точки [math] v [/math] до точки пересечения. Точками событий будут концы отрезков.

Пустим луч из рассматриваемой вершины [math] v [/math] вертикально вверх и добавим в статус все отрезки, которые он пересекает, по увеличению расстояния до них. Теперь будем рассматривать точки [math] w \in V [/math] в порядке сортировки по углу между [math] v [/math] и вертикальной полуосью [math] l [/math]. При таком обходе для проверки видимости вершины достаточно проверить пересечение с ближайшим к [math] v [/math] отрезком, то есть первым в статусе(так как отрезки отсортированы по расстоянию до них). Действительно, если вершина [math] w [/math] не видна, то отрезок [math] vw [/math] пересекает несколько отрезков, лежащих перед [math] w [/math], а значит и ближайший. В противном случае все пересекаемые лучом отрезки лежат за вершиной [math] w [/math] и пересечения отрезка [math] vw [/math] с ближайшим отрезком не будет. Вне зависимости от видимости вершины, необходимо изменить статус заметающего луча. Для этого необходимо удалить из статуса все отрезки, которые заканчиваются вершине [math] w [/math] (лежат слева от прямой [math] vw [/math]) и добавить все отрезки, которые в ней начинаются (лежат справа от прямой [math] vw [/math]).

Псевдокод
graph buildVisibilityGraph(Set<Segment> segments)
   vertices = getVertices(segments) [math] \cup\ \{s,\ t\} [/math]
   graph = visibilityGraph(vertices)                  
   for Vertex [math]v[/math] in vertices
      for Vertex [math]w[/math] in getVisibleVertices([math]v[/math], segments)
         visibilityGraph.addEdge([math]v[/math], [math]w[/math])
   return visibilityGraph

Здесь функция getVisibleVertices([math] v [/math]) возвращает все видимые из [math] v [/math] вершины и выглядит так:

Set<Vertex> getVisibleVertices(Vertex [math]v[/math], Set<Segment> segments)
   Set<Vertex> answer
   for Segment [math]s[/math] in segments
      if intersect([math] s [/math], [math] l [/math])
         status.add([math]s[/math])
   for Point [math]w[/math] in segments
      if [math]v.x \leqslant w.x[/math]
         currentVertices.add([math]w[/math])
   sort(currentVertices) by angle
   for Point [math]w[/math] in currentVertices
      if not intersect([math]vw[/math], status.closest)
         answer.add([math]w[/math])
      for Segment [math]s[/math] ending in [math]w[/math]
         status.delete([math]s[/math])
      for Segment [math]s[/math] beginning in [math]w[/math]
         status.add([math]s[/math])
   return answer

В качестве статуса нужно использовать структуру данных, позволяющую добавлять и удалять из нее отрезки за [math] O(\log n) [/math] и извлекать минимум за [math] O(1) [/math] или [math] O(\log n) [/math]. В этом случае достигается асимптотика [math] O(n^2 \log n) [/math], так как для каждой из [math] n [/math] точек выполняется сортировка за [math] O(n \log n) [/math], обновление статуса (суммарно [math] O(n \log n) [/math], так как каждый отрезок добавляется и удаляется из статуса не более одного раза) и запросы ближайшего отрезка ([math] O(\log n) [/math] или [math] O(1) [/math] на точку, то есть [math] O(n \log n) [/math] или [math] O(n) [/math]).

Обновление статуса заметающего луча: добавляем ребра [math] w_1 w_5 [/math] и [math] w_1 w_2 [/math] в статус
Добавляем ребра [math] w_3 w_2 [/math] и [math] w_3 w_4 [/math] в статус
Удаляем ребра [math] w_3 w_2 [/math] и [math] w_1 w_2 [/math] из статуса

Motion planning

Изменяем препятствия
Ищем путь для точки

Рассмотрим задачу нахождения кратчайшего пути, когда движимый объект — это выпуклый полигон. Например, робот, которого надо доставить из начальной в конечную точку.

Если полигон вращать нельзя, задачу сводится к движению точки так: выбирается точка на полигоне, которая принимается за начало координат. В такой системе координат для каждого препятствия считается сумма Минковского с полигоном. Получаются бОльшие препятствия, но теперь достаточно двигать выбранную точку, что было описано выше.

Если полигон можно вращать, задача нахождения кратчайшего пути становится достаточно ресурсоёмка, поэтому обычно рассматривают задачу нахождения какого-нибудь пути между конечными точками.

Первый шаг решения этой задачи совпадает с предыдущим случаем: выберем точку и построим сумму Минковского препятствий с полигоном. Рассмотрим малый угол [math] \epsilon [/math]. Представим, что поворот полигона на этот угол — это движение вверх-вниз между слоями, на каждом из которых посчитана сумма Минковского с полигоном, повернутым на этот угол.

На каждом слое построим трапецоидную карту и граф, как описано в начале. Если пересечь соседние слои и добавить между их графами ребра, получится один большой граф, в котором ищется кратчайший путь.

При таком подходе может возникнуть ошибка при пересечении слоев: на каждом слое состояния будут допустимые, а осуществить поворот физически будет невозможно. Обычно, эту проблему решают двумя способами: измельчением угла поворота и изначальным сглаживанием углов полигона. Первый способ повышает не только точность решения, но и вычислительную сложность задачи. Второй подход практически исключает возможность нахождения пути, когда его нет, но повышает вероятность "ненахождения" пути, когда он есть.

Источники информации

  • Mark de Berg, Otfried Cheong, Marc van Kreveld, Mark Overmars, Computational Geometry: Algorithms and Applications — Third edition — Springer, 2008. — Chapter 15. — ISBN 978-3-540-77973-5
  • Academia.edu — 3D Visibility Graph
  • Хабрахабр — Motion planning: граф видимости, дорожные карты
  • igitur-archive.library.uu.nl — Visibility graph при помощи rotation tree за [math]O(n^2)[/math].

Ссылки

  • Github — Реализация алгоритма за [math] O(n^2 \log n) [/math]