Граф видимости: путь вокруг препятствий

Если механизм уже знаком - листай до «Теперь сам».

Как думаешь: что нужно сохранить

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

На что это похоже

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

Механизм: граф видимости для планирования движения

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

Любой лишний излом в открытом свободном пространстве можно распрямить по неравенству треугольника. На прямом участке границы поворот тоже не нужен. Поэтому существует кратчайший путь, чьи внутренние повороты приходятся на вершины препятствий. Построй граф видимости: его вершины - все углы, старт и финиш; ребро есть для каждой видимой пары, вес равен длине. Неотрицательные веса позволяют применить Дейкстру.

Полный граф может иметь квадратично много рёбер. Простой перебор пар и проверка всех препятствий стоит до кубического времени. Угловое заметание вокруг каждой вершины, описанное в книге, строит граф за O(n² log n) при n рёбрах препятствий. Это стоимость геометрического этапа, которую нельзя забывать при оценке всей задачи.

Если робот - фиксированный многоугольник R, который только переносится, выбери опорную точку, например начало его локальных координат. Запрещённые положения этой точки для препятствия P образуют сумму Минковского P+(-R): множество всех сумм векторов p-r. Отражение -R необходимо: столкновение означает c+r=p, то есть c=p-r. Объедини расширенные препятствия, затем решай задачу для точки в дополнении к объединению. Если разрешено вращение, одной плоскости положений уже недостаточно: появляется координата угла. Простое двумерное расширение сохраняет только модель фиксированной ориентации.

Правило

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

Разбор: проследим вычисление

Для прямоугольника [2,4]×[-1,1] проверим все пары шести вершин, включая старт (0,0) и финиш (6,0). Параметрический интервал пересечения позволяет отличить проход через внутренность от касания границы. Это специальный точный по модели тест для одного осевого прямоугольника, не универсальная проверка многоугольников.

import heapq
import math

def blocked(a, b):
    lo, hi = 0.0, 1.0
    for av, bv, low, high in zip(a, b, (2, -1), (4, 1)):
        delta = bv - av
        if delta == 0:
            if not low < av < high:
                return False
        else:
            t1, t2 = (low-av)/delta, (high-av)/delta
            lo = max(lo, min(t1, t2))
            hi = min(hi, max(t1, t2))
    return lo < hi

p = [(0, 0), (6, 0), (2, -1), (4, -1), (4, 1), (2, 1)]
graph = [[] for _ in p]
for i in range(len(p)):
    for j in range(i):
        if not blocked(p[i], p[j]):
            w = math.dist(p[i], p[j])
            graph[i].append((j, w))
            graph[j].append((i, w))
distance = [float('inf')] * len(p)
distance[0] = 0
queue = [(0, 0)]
while queue:
    value, v = heapq.heappop(queue)
    if value != distance[v]:
        continue
    for u, weight in graph[v]:
        new = value + weight
        if new < distance[u]:
            distance[u] = new
            heapq.heappush(queue, (new, u))
assert blocked(p[0], p[1])
assert not blocked(p[2], p[3])
print("прямая запрещена", blocked(p[0], p[1]))
print("длина обхода", round(distance[1], 6))
print("проверка", round(2 + 2 * math.sqrt(5), 6))

прямая запрещена True
длина обхода 6.472136
проверка 6.472136

Разбор: где проходит граница

Пусть робот - квадрат с полуразмером 0,5, центр служит опорной точкой, вращения нет. Прямоугольное препятствие расширяется на 0,5 по обеим осям. Путь центра вдоль старой границы приведёт к столкновению: корпус заходит внутрь. Для положительного зазора увеличь расширение ещё на выбранный допуск. Сначала зафиксируй, считается ли касание допустимым: это влияет на открытость границ и возможность существования кратчайшего пути.

Что ломается без этого

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

Зачем это в работе

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

Теперь сам

Препятствие занимает [3,5]×[2,4], робот - осевой квадрат полуразмера 1. Какой прямоугольник запрещён его центру при фиксированной ориентации?