Векторы и ориентация

Навигатор говорит: «Через двести метров поверните направо». Откуда он знает, что направо? У него нет глаз, есть только координаты: маршрут — это ломаная из точек. Ниже настоящий маршрут по улицам Алматы, от площади Республики до Зелёного базара, тот же, что в статье о Дейкстре. Все подсказки справа программа получила из координат двумя умножениями на каждый перекрёсток. Нажмите «Поехали» и включите голос.

На маршруте 3,8 км и 39 участков улиц между перекрёстками, а поворотов всего десять: на остальных перекрёстках дорога идёт прямо или почти прямо. Самый длинный путь без поворотов — 1837 метров. Чтобы отличить поворот от плавного изгиба, программе нужно знать угол и его сторону. И то и другое дают два произведения векторов.

Идея

Для векторов u = (u_x, u_y) и w = (w_x, w_y) число u_xw_y − u_yw_x положительно, если w повёрнут относительно u налево (против часовой стрелки), отрицательно, если направо, и равно нулю, если векторы лежат на одной прямой. Это векторное (или псевдоскалярное, косое) произведение. Число u_xw_x + u_yw_y — скалярное произведение — положительно, если угол между векторами острый, и отрицательно, если тупой.

Для трёх точек O, A и B берём векторы OA и OB:

Если путь идёт O → A → B, знак cross(O, A, B) отвечает на вопрос, в какую сторону он поворачивает в точке A. Подвигайте точки и посмотрите, как меняются оба числа.

У векторного произведения есть и наглядный смысл: это площадь параллелограмма, натянутого на векторы OA и OB, взятая со знаком. Когда B переходит через прямую OA, площадь проходит через ноль и меняет знак — вот и вся «магия». Половина модуля — площадь треугольника OAB, и на этом стоит формула площади многоугольника.

Почему это работает

Обозначим через θ угол, на который нужно повернуть u против часовой стрелки, чтобы он смотрел туда же, куда w. Тогда

Синус положителен для поворота налево на угол от 0 до 180°, отрицателен для поворота направо. Косинус положителен, пока угол острый. Длины векторов только масштабируют ответ, поэтому для знака их не нужно знать — и не нужно извлекать корни.

Отсюда же формула угла поворота, которой пользуется навигатор: θ = atan2(u × w, u · w). Функция atan2 по двум числам, пропорциональным синусу и косинусу, возвращает угол от −180° до 180° сразу с правильным знаком. Это лучше, чем знакомое из школы arccos(u · w / (|u||w|)): арккосинус не знает стороны поворота и теряет точность у 0° и 180°, где его график почти вертикален.

В навигаторе u — направление въезда в перекрёсток, w — направление выезда. Если |θ| меньше 25°, программа молчит: это изгиб улицы, а не поворот. Если больше 120° — говорит «круто». Пороги выбраны на глаз; в настоящих навигаторах ещё учитывают, сколько дорог сходится на перекрёстке и как они называются.

Пересекаются ли отрезки

Самое частое применение ориентации — проверить, пересекаются ли отрезки AB и CD. Пуля пересекает стену? Новая дорога пересекает реку? Курсор попал в линию на чертеже?

Отрезки пересекаются, если C и D лежат по разные стороны прямой AB, а A и B — по разные стороны прямой CD. «По разные стороны» — это разные знаки у cross(A, B, C) и cross(A, B, D). Одной проверки мало: C и D могут быть по разные стороны прямой AB, но сам отрезок AB может кончиться раньше, чем до него дойдёт CD.

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

Код

Всё вычислительное ядро — две функции по одной строке. Остальное — их применения: поворот, площадь треугольника, острый угол, точка на отрезке и пересечение отрезков.

Координаты здесь целые, и это не случайность. Векторное произведение — это умножения и вычитания, без деления. Для целых координат ответ точный, и знак никогда не врёт. В Go для координат до 10⁹ хватает int64: каждая разность не больше 2·10⁹, произведение — 4·10¹⁸, разность двух произведений — 8·10¹⁸, а предел int64 около 9,2·10¹⁸. Python об этом не думает: его целые числа не переполняются.

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

Ловушка: дроби и плавающая точка

Карты хранят координаты в градусах — дробных числах. Возьмём три точки на прямой y = x: p = (0,5; 0,5), q = (12; 12), r = (24; 24). Сдвинем p на крошечный шаг 2⁻⁵³ — это единица последнего разряда числа 0,5 в формате double — и спросим, по какую сторону прямой qr она оказалась. Сделаем так для сетки 256 × 256 сдвигов и сравним с точным ответом.

Точно на прямой лежат только 256 точек — диагональ сетки. Вычисление в double ошибается для 11 972 точек из 65 536, почти для каждой пятой. Большинство ошибок — «на прямой» там, где точка чуть в стороне. Но для 672 точек ответ хуже: левая сторона названа правой или наоборот. Сам ответ не случайный, он воспроизводится, — но верить ему у прямой нельзя.

В Go есть ещё одна тонкость. Спецификация разрешает компилятору склеивать умножение со сложением в одну инструкцию FMA, которая округляет один раз вместо двух. На процессорах ARM компилятор так и делает: то же выражение без обёрток float64(…) вокруг произведений даёт на ARM другую картину — 4911 ошибок, из них 4592 с перепутанной стороной. На x86 с настройками сборки по умолчанию — те же 11 972, что в Python. Явное приведение float64(…) запрещает склейку, и программа отвечает одинаково везде.

Что с этим делают на практике:

• Переходят к целым. Градусы умножают на 10⁷ и округляют — шаг получается около сантиметра на местности, точнее любого GPS. Так хранит координаты OpenStreetMap. Дальше вся геометрия целочисленная и точная.
• Сравнивают с допуском. Считают «на прямой», если |cross| меньше ε, умноженного на длины векторов. Это честно говорит «не знаю» вместо неверного знака, но ε приходится подбирать под задачу.
• Точные предикаты. Сначала считают в double и оценивают погрешность. Если ответ далеко от нуля, ему можно верить; если близко — пересчитывают точно. Так устроены предикаты Джонатана Шевчука (1997) и их потомки в геометрических библиотеках: почти всегда быстро, всегда верно.

Сколько это стоит

Одна проверка ориентации — два умножения и несколько вычитаний, O(1). Проверка пересечения двух отрезков — четыре таких проверки. Навигатору на маршрут из n отрезков нужно O(n) действий.

Дорого становится, когда отрезков много. Проверить все пары из n отрезков — O(n²): для ста тысяч улиц это пять миллиардов пар. Алгоритм заметающей прямой Бентли — Оттмана находит все k пересечений за O((n + k) log n), а внутри у него всё та же проверка ориентации.

Где ещё работает ориентация

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

Задачи

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