Я недавно сделал маленькую библиотеку для решения задачи поиска кратчайшего пути на 2D карте с выпуклыми препятствиями. В процессе реализации я придумал пару алгоритмов и трюков, описания которых я нигде не встречал. Поэтому делюсь этими "изобретениями" с общественностью.
Горжусь тем, что мое решение работает очень быстро. Для внушительного количества полигонов все операции можно выполнять каждый кадр. Т.е. не надо ничего запекать и вся геометрия карты может меняться в каждом кадре.
Касательные
Некоторые читатели могут возмутиться, что к полигонам, как не дифференцируемым кривым, касательных нет. Но тут я использую термин не из матанализа с таким же названием. В моем случае касательная к полигону — это прямая, пересекающая его ровно в одной точке, или проходящая через сторону.
Откуда вообще эти касательные берутся? Несложными рассуждениями можно доказать, что все кратчайшие пути на карте с выпуклыми полигонами-препятствиями состоят из отрезков прямых, которые или совпадают со стороной одного из препятствия, или обоими концами являются или концом пути или касаются какого-то полигона.
Доказательство от противного. Допустим в кратчайшем пути какая-то часть не является прямым отрезком, или является прямым отрезком, не совпадающим со стороной полигона и один из его концов не является концом пути или касательной. У кривой всегда можно чуть-чуть срезать "угол". Ведь эта кривая не может идти по границе полигона, который состоит из прямых отрезков. Противоречие с тем, что путь кратчайший. Далее будем считать, что путь состоит из прямых отрезков.
Если же отрезок просто висит концом в пространстве или лежит на границе полигона, но не является касательной к нему, то можно этот конец чуть чуть подвинуть чтобы выпрямить угол со следующим отрезком, сократив тем самым путь. Опять противоречие.
Единственные случаи, когда мы не можем сократить путь слегка подвинув границу отрезка пути — это если отрезок уже совпадает со стороной полигона или касается его.
Все случаи выше показаны на картинке. Красным показан какой-то путь, пунктиром — сокращенный путь.
Из наблюдения выше получается следующий алгоритм построения кратчайшего пути по карте: Для всех вершин полигонов и внешних точек (возможные начала и концы путей) нужно построить касательные ко всем полигонам. Оставить только те, которые касаются обоих полигонов. Добавить к этому все границы полигонов и отрезки между внешними точками. Потом выкинуть все отрезки, которые пересекаются с каким-то препятствием. Построить граф где эти отрезки — это ребра. Найти в нем кратчайший путь.
Наивный алгоритм
Самый простой метод построить касательные из точки — это перебрать все вершины полигона и найти те, где обе стороны лежат по одну сторону от прямой точка-вершина. Понять. что стороны лежат по одну сторону можно проверив знаки векторных произведений векторов сторон (v1, v2) и вектора из точки (v):
Получается что-то вроде такого:
// |p| хранит вершины полигона в порядке обхода против часовой стрелки.
// Класс Point используется для хранения точек или векторов.
// Оператор ^ переопределен для векторного произведения.
// Оператор - переопределен для векторной разности
std::pair<int, int> Polygon::GetTangentIds(const Point &a) {
int n = p.size();
std::pair<int, int> res = { -1, -1 };
for (int i = 0; i < n; ++i) {
if (p[i] == a) {
res = { i, i };
break;
}
Point v1 = p[i] - p[(i + n - 1) % n];
Point v = p[i] - a;
Point v2 = p[(i + 1) % n] - p[i];
double dir1 = v1 ^ v;
double dir2 = v ^ v2;
if (dir1 < eps && dir2 < -eps) {
res.first = i;
}
if (dir1 > eps && dir2 > -eps) {
res.second = i;
}
}
return res;
}Обратите внимание на сравнения с eps — так правильно сравнивать вещественные числа, чтобы ошибки округления не ломали алгоритм. Плюс или минус выбираются в зависимости от строгости сравнения. < -eps — это значит строго меньше 0, < eps — значит меньше равно. Знаки расставлены в коде выше так, чтобы выбрать самую удаленную вершину, если точка a лежит на продолжении стороны.
Этот метод работает за O(n). Можно отдельно рассмотреть случай вершины номер 0 и номер n-1, чтобы избавиться от модуля.
Быстрый логарифмический алгоритм
Раз уж точки уже упорядочены в порядке обхода, то тут явно напрашивается применение бинарного или хотя бы троичного поиска. Искать сразу две далекие точки одним алгоритмом не получится, поэтому будем запускать наш логарифмический поиск два раза — для левой и правой касательной.
Еще одна проблема — очень сложно даже думать о каком-то упорядоченном поиске в зацикленном контейнере. Поэтому разорвем наш полигон в первой вершине. Просто проверим, вдруг она является искомым концом касательной. Если нет, то можно забыть про сторону между последней и первой вершиной и не думать о зацикленности полигона.
Если взять произвольную сторону полигона, то можно понять, лежит ли она на "лицевой" или обратной стороне полигона. Похожим методом считается освещенность граней в компьютерной графике — достаточно посмотреть на скалярное произведение нормали к стороне и вектора из вершины. Но можно не считать нормали, а брать векторное произведение с вектором-стороной:
Если вектор-сторона идет слева направо относительно вектора на начало стороны, то это — лицевая сторона. В противном случае — это обратная сторона. Концы касательных — это вершины между "лицевыми" и "обратными" сторонами.
Уже можно было бы запустить бинарный поиск, если бы мы точно знали, что одна граница отрезка лежит на лицевой стороне, а другая — на обратной. Но их такие еще найти надо. Тут можно воспользоваться еще одним наблюдением — вершины внутри одной стороны строго упорядочены: лицевая сторона идет слева направо, а обратная сторона — справа налево. Если у нас есть две вершины на лицевой стороне (или обе на обратной), то мы можем легко понять, лежат ли концы касательных между них: Надо только посмотреть, лежит ли последняя вершина раньше первой в порядке вершин:
Если обе вершины на обратной стороне, то переключение между обратной и лицевой стороной происходит между ними (в порядке обхода) только если вторая вершина "перескочила" лицевую сторону и оказалась раньше первой вершины отрезка. Аналогичное правило действует для двух вершин на лицевой стороне.
Давайте для конкретики введем "знаки" у вершин. Все вершины, сторона из которых идет справа налево относительно вектора из точки начала касательных, обозначим как "+" вершины — в них значение векторного произведения будет положительно. Все точки, сторона из которых лежит на лицевой стороне или идет слева направо относительно начала касательных, обозначим "-" — в них значение векторного произведения отрицательно.
Есть еще один неприятный случай: некоторые вершины могут иметь нулевое векторное произведение, если касательная совпадает со стороной (обозначим такие вершины "0"-вершинами). Еще более странные случаи, если точка-начало касательных лежит на стороне полигона, или вообще совпадает с вершиной. Ниже приведены все возможные случаи расположения начала касательных и "знаки" вершин:
- Касательные не совпадают ни с одной стороной.
- Левая касательная совпадает со стороной.
- Точка лежит на пересечении двух сторон.
- Правая касательная совпадает со стороной.
- Точка лежит на стороне.
- Точка совпадает с вершиной.
- Точка внутри полигона.
Нули могут быть с обоих концов от отрезка из плюсов, минусов может и не быть. Одно точно видно — плюсы есть всегда. Поэтому давайте в качестве левой касательной брать "0/-" вершину, перед которой идет "+" вершина, а в качестве правой касательной — "+" вершину, перед которой идет "0/-" вершина. Этот подход из нескольких возможных концов (при совпадении стороны и касательной) найдет самую далекую. Если же нужен самый близкий конец, то можно после выполнения логарифмического алгоритма попытаться подвинуть левый конец вперед вдоль полигона, а правый — назад. Еще, последний случай (точка внутри полигона) немного все портит. В полигоне ничего кроме "+" нет. И мы никогда не найдем границу между двумя областями. Надо аккуратно проверить, что наш поиск не повисает в этом случае и не выдает какие-то неправильные ответы.
Итак, алгоритм для левой касательной:
1. Проверить, вдруг самая первая вершина удовлетворяет условию касательной. Если это не так, то мы знаем, что переключение между "+" и "0/-" происходит (если оно вообще есть) где-то между вершинами с 0 по n-1.
2. l=0, r=n-1.
4. Пока r-l > 1:
4.1. Подсчитать "знак" вершины l.
4.2. Взять m: ceil((l+r)/2). Обратите внимание, условие 4. означает, что l < m < r.
4.3. Подсчитать знак вершины m.
4.4. Присвоить r = m, если выполняется один из трех случаев:
l - "+" и m - "-/0"
l - "+" и m - "+" и l лежит левее m
l - "-/0" и m - "/0" и l лежит правее m (или на одной прямой с ней)
4.5. Иначе присвоить l = m.Есть один нетривиальный момент, что делать, если l и m имеют одни и те же знаки лежат на одной прямой с точкой начала касательных? Т.е. вершина l не лежит ни строго левее ни строго правее m?
Это возможно только в случаях 2, 3, 6 — когда левая касательная совпадает со стороной. Если правая касательная совпадает со стороной, то знаки у вершин будут разные (дальняя — всегда +). При этом l и m указывают на 2 соседние вершины! Но, поскольку у нас поддерживается l < m, мы же разорвали цикл и ищем внутри не зацикленного массива, то это значит, что l — тот самый искомый конец касательной. Т.е. в случае, если обе вершины имеют знаки "-/0" и лежат на одной прямой, мы должны сдвинуть правый конец. Поэтому в пункте 4.4 сравнения надо делать именно так, если знаки совпадают во втором случае.
Для поиска правой касательной строгость сравнения должна быть в обратную сторону. Если обе вершины "-/0" и они лежат на одной прямой, то надо сдвинуть левую границу.
Итак, весь код алгоритма:
std::pair<int, int> Polygon::GetTangentIdsLogarithmic(const Point& a) const
{
std::pair<int, int> res = { -1, -1 };
const int n = points_.size();
Point vl = points_[1] - points_[0];
Point to_l = points_[0] - a;
Point v_prev = points_[0] - points_[n - 1];
Point to_prev = points_[n - 1] - a;
double pointing_l = to_l ^ vl;
double pointing_prev = to_prev ^ v_prev;
int l = 0;
int r = n - 1;
// Проверка, что вершина 0 - левая касательная.
if (pointing_prev > eps && pointing_l < eps) {
res.first = 0;
// Случай совпадения касательной и стороны
if (pointing_l > -eps) {
Point v_next = points_[2] - points_[1];
Point to_next = points_[1] - a;
double pointing_next = to_next ^ v_next;
// точка начала еще может лежать на стороне
if (pointing_next < -eps) {
res.first = 1;
}
else if (pointing_next < eps) {
// Или совпадать с вершиной 1.
return { 1, 1 };
}
}
}
else {
// Бинарный поиск.
while (l < r - 1) {
int m = (r + l + 1) / 2;
Point vm = points_[m + 1] - points_[m];
Point to_m = points_[m] - a;
double pointing_m = to_m ^ vm;
double lm_dir = to_l ^ to_m;
if (((pointing_m < eps) || (lm_dir < -eps)) &&
((pointing_l > eps) || (lm_dir > -eps))) {
r = m;
}
else {
l = m;
to_l = to_m;
pointing_l = pointing_m;
}
}
// На случай, если |a| лежит внутри полигона, надо проверить, что бинпоиск
// остановился на ответе, а не просто на случайной вершине.
int next = (r + 1) % n;
Point to_r = points_[r] - a;
Point vr = points_[next] - points_[r];
double pointing_r = to_r ^ vr;
if (pointing_r < eps && pointing_l > eps) {
res.first = r;
// Проверка, что |a| лежит на продолжении стороны.
Point v_next = points_[(next + 1) % n] - points_[next];
Point to_next = points_[next] - a;
double pointing_next = to_next ^ v_next;
if (pointing_r > -eps) {
// Проверка, что |a| не лежит на стороне.
if (pointing_next < -eps) {
res.first = next;
}
else if (pointing_next < eps) {
// Или совпадает с вершиной.
return { next, next };
}
}
}
else return { -1, -1 };
}
// Теперь работаем с правой касательной.
vl = points_[1] - points_[0];
to_l = points_[0] - a;
v_prev = points_[0] - points_[n - 1];
to_prev = points_[n - 1] - a;
pointing_l = to_l ^ vl;
pointing_prev = to_prev ^ v_prev;
l = 0;
r = n - 1;
// Проверка первой вершины
if (pointing_prev < eps && pointing_l > eps) {
res.second = 0;
// Может ее надо подвинуть назад.
if (res.first != n - 1 && pointing_prev > -eps) {
res.second = n - 1;
}
}
else {
while (l < r - 1) {
int m = (r + l + 1) / 2;
Point vm = points_[m + 1] - points_[m];
Point to_m = points_[m] - a;
double pointing_m = to_m ^ vm;
double lm_dir = to_l ^ to_m;
if (((pointing_l < eps) || (lm_dir < eps)) &&
((pointing_m > eps) || (lm_dir > eps))) {
r = m;
}
else {
l = m;
to_l = to_m;
pointing_l = pointing_m;
}
}
// Проверка, что бинпоиск остановился на ответе.
Point to_r = points_[r] - a;
Point vr = points_[(r + 1) % n] - points_[r];
double pointing_r = to_r ^ vr;
if (pointing_r > eps && pointing_l < eps) {
res.second = r;
// Проверка, что ответ нужно подвинуть назад.
if ((res.first != l) && pointing_l > -eps) {
res.second = l;
}
}
}
return res;
}
Несмотря на длинный и, возможно, пугающий код, этот алгоритм работает быстрее наивного уже при n=4.
Проверка пересечения отрезка и полигона за O(1)
*terms and conditions apply.
Следующий трюк, которым я хотел поделиться — это как пересекать отрезок и полигон за константу, при выполнении некоторых допущений. Во-первых, ни один конец отрезка не лежит внутри полигона, и, во-вторых, касательные уже должны быть известны. В противном случае, можно за логарифм построить касательные и проверить, что концы отрезка не лежат внутри полигона. Это дало бы логарифмический алогритм проверки на пересечение.
Оба этих допущения выполняются в моей библиотеке поиска кратчайшего пути вокруг препятствий. Возможных отрезков, которые нужно проверять на пересечение — O(k*n^2), если у нас n полигонов с k вершинами. Когда как точек, из которых они торчат — всего O(k*n). Поэтому гораздо быстрее сначала проверить все точки-концы отрезков по одному разу и вычеркнуть все отрезки из плохих точек. Второе допущение выполняется, потому что все касательные на все полигоны мы и так строим — это же и есть кандидаты на ребра в графе кратчайших путей. Для проверки на пересечения они достаются бесплатно.
Итак, нам даны полигон, отрезок с концами вне полигона и касательные из одного конца отрезка. Если отрезок пересекает полигон, то он точно должен пересекать и хорду между двумя концами касательных (иначе один из его концов должен был бы лежать внутри полигона). Т.е. получается, что нужно проверить пересечение лишь с одним отрезком, а это можно сделать за O(1).
