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

Материал из Викиконспекты
Перейти к: навигация, поиск
м (Построение visibility графа)
м (rollbackEdits.php mass rollback)
 
(не показаны 92 промежуточные версии 9 участников)
Строка 1: Строка 1:
{{ptready}}
+
== Нахождение любого пути между точками с препятствиями ==
<div style="background-color: #ABCDEF; font-size: 16px; font-weight: bold; color: #000000; text-align: center; padding: 4px; border-style: solid; border-width: 1px;">Эта статья находится в разработке!</div>
+
{|align="right"
<includeonly>[[Категория: В разработке]]</includeonly>
+
|-valign="top"
 +
|[[Файл:trap.png|200px|thumb|right|Путь с препятствиями через трапецоидную карту]]
 +
|[[Файл:notShort.png|300px|thumb|right|Такой путь не самый короткий]]
 +
|}
 +
Для начала рассмотрим движение материальной точки. Случай, когда размером и формой движимого объекта пренебречь нельзя, будет рассмотрен [[Visibility graph и motion planning#Motion planning|позднее]].
  
== Visibility graph ==
+
Эту задачу можно решить с помощью [[Трапецоидная карта | трапецоидной карты]]. По ней строится граф, ребра которого соединяют центры трапедоидов, а также начальную и конечную вершины с серединами вертикальных сторон трапецоидов. В таком графе ищется путь между начальной и конечной вершинами.
[[Файл:trap.png|200px|thumb|left|Путь с препятствиями через трапецоидную карту]]
 
[[Файл:notShort.png|300px|thumb|right|Такой путь не самый короткий]]
 
  
В общем, когда мы ищем путь от точки <tex> S </tex> до <tex> T </tex> с препятствиями (надо уточнить, что двигаем мы точку, а не какой-то полигон), можно построить [[Трапецоидная карта | трапецоидную карту]], соединить ребрами середины вертикальных сторон с центрами трапецоидов и в этом графе [[Алгоритм Дейкстры | Дейкстрой]] найти путь от <tex> S </tex> до <tex> T </tex>. Но этот путь не будет кратчайшим(кэп).
+
Если точки лежат внутри одного трапецоида {{---}} ответ найден. Иначе идём из стартовой точки в центр её трапецоида, далее по построенным рёбрам ищем трапецоид содержащий финальную точку. Для этого можно использовать поиск в ширину или другой алгоритм нахождения кратчайшего пути в графе. В конечном итоге, соединяем середину последнего трапецоида с конечной вершиной.
 +
 
 +
Данный алгоритм работает за <tex> O(n \log n) </tex> и за линейное количество памяти и хорошо подходит для нахождения какого-нибудь пути между парой данных вершин. Но если нужно найти кратчайший путь, этот алгоритм не подходит, хоть и работает быстро. Однако, решения нахождения кратчайшего пути в лучшем случае работают за <tex> O(n^2) </tex> времени и памяти (здесь и далее <tex> n </tex> {{---}} количество всех вершин).
 +
 
 +
== Нахождение кратчайшего пути между точками с препятствиями ==
 +
=== Visibility graph ===
 +
Рассмотрим точное решение нахождения кратчайшего пути на плоскости между двумя точками с полигональными препятствиями с помощью построения графа видимости. После его построения, как и в случае с трапецоидной картой, кратчайший путь ищется любым стандартным алгоритмом поиска (например, алгоритмом [[Алгоритм Дейкстры|Дейкстры]] или [[Алгоритм A*|A*]]).
 +
 
 +
Для простоты рассуждений начальную и конечную вершины будем считать вершинами полигонов.
 +
 
 +
{{Лемма
 +
|about=О кратчайшем пути
 +
|statement=
 +
Любой кратчайший путь между двумя вершинами с полигональными препятствиями представляет собой ломаную, вершины которой {{---}} вершины полигонов.
 +
|proof=
 +
[[Файл:short.png|150px|thumb|right|Short cut]]
 +
Пусть кратчайший путь {{---}} не ломаная. В таком случае, на пути существует такая точка <tex> p </tex>, которая не принадлежит ни одному прямому отрезку. Это означает, что существует <tex>\epsilon</tex>-окрестность точки <tex> p </tex>, в которую не попадает ни одно препятствие (случай, когда точка попала на ребро рассматривается аналогично). В таком случае, подпуть, который находится внутри <tex>\epsilon</tex>-окрестности, по неравенству треугольника может быть сокращён по хорде, соединяющий точки пересечения границы <tex>\epsilon</tex>-окрестности с путем. Раз часть пути может быть уменьшена, значит и весь путь может быть уменьшен, а значит исходное предположение некорректно.
 +
}}
 +
{{Определение
 +
|definition =
 +
Говорят, что вершина <tex> u </tex> ''видна'' (англ. mutually visible) из <tex> v </tex>, если отрезок <tex> uv </tex> не пересекает ни одного препятствия.
 +
}}
 +
{{Определение
 +
|definition =
 +
''Граф видимости'' (англ. visibility graph) {{---}} граф, вершины которого {{---}} вершины полигонов. Между вершинами <tex> u </tex> и <tex> v </tex> существует ребро, если из <tex> u </tex> видна <tex> v </tex>.
 +
}}
 +
 
 +
В худшем случае в таком графе может быть <tex> O(n^2) </tex> ребер. Однако по некоторым ребрам кратчайший путь точно не пройдет, и такие ребра из графа можно удалить.
  
 
{{Лемма
 
{{Лемма
 +
|about=О неиспользуемых вершинах
 
|statement=
 
|statement=
Любой кратчайший путь от <tex> S </tex> до <tex> T </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> не принадлежит кратчайшему пути и его можно удалить из графа. (См. поясняющую картинку справа)
 +
# Все внутренние вершины, кроме вырожденного случая, (начальная/конечная точка лежит внутри выпуклой оболочки фигуры) можно игнорировать.
 
|proof=
 
|proof=
[[Файл:short.png|150px|thumb|right|Ну в общем тут все очевидно]]
+
[[Файл:edgeNotToDelete.png|200px|thumb|right|Не удаляем <tex> BS </tex>]]
Пусть путь проходит(в смысле вершины) через какую-то другую точку. Рассмотрим окрестность этой точки. По неравенству треугольника мы сможем немножко, да срезать. Значит этот путь не кратчайший. Противоречие, значит лемма доказана и все офигенно.
+
# Путь проходящий через ребро <tex> BD </tex> будет длиннее, чем через соседей точки <tex> B </tex>, так как по неравенству треугольника <tex> AB + BD > AD </tex>
 +
# Если случай не вырожденный, значит заход внутрь фигуры только увеличит суммарный путь, так как по неравенству треугольника расстояние между соседними выпуклыми вершинами всегда меньше суммы расстояний с учётом внутренней.
 
}}
 
}}
  
По этой лемме запилим visibility graph. Его вершины {{---}} вершины полигонов. Между вершинами <tex> u </tex> и <tex> v </tex> существует ребро, если из <tex> u </tex> <i> видна </i>(mutually visible) <tex> v </tex> (ребра полигонов тоже входят в этот граф). Теперь, если мы добавим к множеству вершин <tex> S </tex> и <tex> T </tex> (и ребра в видимые вершины), у нас получится граф, в котором опять же Дейкстрой находим кратчайший путь. По лемме любое ребро кратчайшего пути {{---}} ребро visibility графа, так что мы нашли то, что нужно.  
+
По доказанным леммам любое ребро кратчайшего пути содержится в графе. Таким образом, для нахождения кратчайшего пути осталось найти кратчайший путь в этом графе от начальной до конечной вершины.
  
 
=== Построение visibility графа ===
 
=== Построение visibility графа ===
==== <tex> O(n ^ 3) </tex> ====
+
==== Наивный алгоритм. <tex> O(n ^ 3) </tex> ====
Если делать <s> в тупую</s> наивно, т. е. для каждой пары вершин проверять можно ли добавить ли такое ребро(нет ли пересечений с полигонами), будет <tex> O(n^3) </tex> (зато просто пилится).
+
Для каждой пары вершин проверяем, можно ли добавить ребро между ними, то есть нет ли пересечений с полигонами. <tex> O(n^2) </tex> пар вершин и <tex> O(n) </tex> ребер, то есть <tex> O(n^3) </tex>.
[[Файл:search.png|400px|thumb|right|Дерево поиска пересекаемых ребер]]
 
==== <tex> O(n ^ 2 \log n) </tex> ====
 
Однако можно это сделать за <tex> O(n ^ 2 \log  n) </tex>. Пусть мы хотим из вершины <tex> v </tex> найти все видимые из нее вершины. Теперь мы будем перебирать ребра не в случайном порядке, а так чтобы можно было проверять за логарифм.
 
  
Для этого посортим все вершины по полярному углу (только те, которые справа нашей, ибо очевидно, что назад можно уже не смотреть) и запилим сбалансированное двоичное дерево поиска.  
+
==== Lee’s Algorithm. <tex> O(n ^ 2 \log n) </tex> ====
 +
{|
 +
|
 +
[[Файл:Zam.png|300px|thumb|left|Заметание плоскости вращающимся лучом]]
 +
Однако можно это сделать за <tex> O(n ^ 2 \log  n) </tex>. Идея алгоритма проста: для каждой вершины найдем видимые из нее вершины. Если научиться делать это за <tex> O(n \log  n) </tex>, задача решена, так как всего точек <tex> n </tex>.
  
Вершин у нас <tex> O(n) </tex>, сортим за <tex> O(n \log n) </tex> плюс запросы в дереве за <tex> O(n) * O(\log n) </tex>. Итого что хотели.
+
Для каждой вершины будем рассматривать только правую половину плоскости, так как ребра, которые должны идти в левую половину, будут исходить из вершин, для которых текущая вершина будет справа.
  
==== <tex> O(n ^ 2) </tex> ====
+
Переформулируем задачу. Дано: точка <tex> v </tex> и множество отрезков {{---}} ребер препятствий.
Каким-то магическим образом, можно избавиться и от логарифма в асимптотике. Это делается с помощью [http://bit.ly/1eEqTzk rotation tree]. Про него рассказывал Антон Ков., но как-то мутно и не очень понятно. Суть такова, что мы обходим вершины в таком хитром порядке, что почти не просматриваем лишнее и получаем асимптотику {{---}} квадрат.
+
Найти: множество концов отрезков, видимых из <tex> v </tex>.
 +
 
 +
Для решения этой задачи будем использовать заметающий луч с началом в точке <tex> v </tex>. Его статусом будут отрезки, которые его пересекают, упорядоченные по возрастанию расстояния от точки <tex> v </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>).
 +
 
 +
===== Псевдокод =====
 +
graph buildVisibilityGraph(Set<Segment> segments)
 +
    vertices = getVertices(segments) <tex> \cup\ \{s,\ t\} </tex>
 +
    graph = visibilityGraph(vertices)                
 +
    '''for''' Vertex <tex>v</tex> '''in''' vertices
 +
      '''for''' Vertex <tex>w</tex> '''in''' getVisibleVertices(<tex>v</tex>, segments)
 +
          visibilityGraph.addEdge(<tex>v</tex>, <tex>w</tex>)
 +
    '''return''' visibilityGraph
 +
Здесь функция getVisibleVertices(<tex> v </tex>) возвращает все видимые из <tex> v </tex> вершины и выглядит так:
 +
Set<Vertex> getVisibleVertices(Vertex <tex>v</tex>, Set<Segment> segments)
 +
    Set<Vertex> answer
 +
    '''for''' Segment <tex>s</tex> '''in''' segments
 +
      '''if''' intersect(<tex> s </tex>, <tex> l </tex>)
 +
          status.add(<tex>s</tex>)
 +
    '''for''' Point <tex>w</tex> '''in''' segments
 +
      '''if''' <tex>v.x \leqslant w.x</tex>
 +
          currentVertices.add(<tex>w</tex>)
 +
    sort(currentVertices) by angle
 +
    '''for''' Point <tex>w</tex> '''in''' currentVertices
 +
      '''if''' '''not''' intersect(<tex>vw</tex>, status.closest)
 +
          answer.add(<tex>w</tex>)
 +
      '''for''' Segment <tex>s</tex> ending in <tex>w</tex>
 +
          status.delete(<tex>s</tex>)
 +
      '''for''' Segment <tex>s</tex> beginning in <tex>w</tex>
 +
          status.add(<tex>s</tex>)
 +
    '''return''' answer
 +
В качестве статуса нужно использовать структуру данных, позволяющую добавлять и удалять из нее отрезки за <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|Обновление статуса заметающего луча: добавляем ребра <tex> w_1 w_5 </tex> и <tex> w_1 w_2 </tex> в статус]]
 +
[[Файл:Zamrefr2.png|250px|thumb|right|Добавляем ребра <tex> w_3 w_2 </tex> и <tex> w_3 w_4 </tex> в статус]]
 +
[[Файл:Zamrefr3.png|250px|thumb|right|Удаляем ребра <tex> w_3 w_2 </tex> и <tex> w_1 w_2 </tex> из статуса]]
 +
|}
  
 
== Motion planning ==
 
== Motion planning ==
В общем тут все очевидно. Тут мы просто двигаем не точку, а произвольный выпуклый полигон. Если мы его не можем вращать, просто "обводим"  препятствия нашим полигоном (запиливаем [[Сумма Минковского (определение, вычисление)|сумму Минковского]] препятствий и полигона) и получаем другие препятствия, но зато теперь мы двигаем точку. А это мы уже научились делать выше.
+
[[Файл:mink.png|200px|thumb|left|Изменяем препятствия]]
 +
[[Файл:mink2.png|400px|thumb|right|Ищем путь для точки]]
 +
Рассмотрим задачу нахождения кратчайшего пути, когда движимый объект {{---}} это выпуклый полигон. Например, робот, которого надо доставить из начальной в конечную точку.
  
Если же этот полигон можно вращать, то делаем примерно то же самое, только как-то по-хитрому. Нам про это, кажется, не рассказывали(или рассказывали так же:))
+
Если полигон вращать нельзя, задачу сводится к движению точки так: выбирается точка на полигоне, которая принимается за начало координат. В такой системе координат для каждого препятствия считается [[Сумма Минковского (определение, вычисление)|сумма Минковского]] с полигоном. Получаются бОльшие препятствия, но теперь достаточно двигать выбранную точку, что было описано выше.
  
== Источники ==
+
Если полигон можно вращать, задача нахождения ''кратчайшего'' пути становится достаточно ресурсоёмка, поэтому обычно рассматривают задачу нахождения какого-нибудь пути между конечными точками.
* 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
 
  
 +
Первый шаг решения этой задачи совпадает с предыдущим случаем: выберем точку и построим [[Сумма Минковского (определение, вычисление)|сумму Минковского]] препятствий с полигоном. Рассмотрим малый угол <tex> \epsilon </tex>. Представим, что поворот полигона на этот угол {{---}} это движение вверх-вниз между слоями, на каждом из которых посчитана сумма Минковского с полигоном, повернутым на этот угол.
 +
 +
На каждом слое построим трапецоидную карту и граф, как описано в [[Visibility graph и motion planning#Нахождение пути между точками с препятствиями|начале]]. Если [[Пересечение многоугольников (PSLG overlaying)|пересечь]] соседние слои и добавить между их графами ребра, получится один большой граф, в котором ищется кратчайший путь.
 +
 +
При таком подходе может возникнуть ошибка при пересечении слоев: на каждом слое состояния будут допустимые, а осуществить поворот физически будет невозможно. Обычно, эту проблему решают двумя способами: измельчением угла поворота и изначальным сглаживанием углов полигона. Первый способ повышает не только точность решения, но и вычислительную сложность задачи. Второй подход практически исключает возможность нахождения пути, когда его нет, но повышает вероятность "ненахождения" пути, когда он есть.
 +
 +
== Источники информации ==
 +
* 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 Academia.edu] {{---}} 3D Visibility Graph
 +
* [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 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]