На данный момент самый распространенный алгоритм для определения столкновений между многогранниками. Реализован в Havok, Dtecta, bullet. Имеет много различных реализаций и модификаций. В данной статье будут описаны базовые понятия и реализованы GJK(алгоритм для нахождения ближайших точек) для 2d случая.
Попытаюсь объяснить с примерами и кодом.
Теория
Определение столкновений в реальном времени.
Задача: определить ближайшие точки между двумя многогранниками, расстояние между ними (если многогранники пересекаются - глубину проникновения).
Вход: вершины многогранника A и B.
Выход: контакт - ближайшие точки, расстояние.
GJK
Пару важных понятий, на которых основан алгоритм:
Сума Минковского
Сума Минковского двух множеств A и B в Евклидовом пространстве это результат сложения каждого элемента A с каждым элементом B:
A+B={a+b | a принадлежит A, b принадлежит B}.
A-B={a+(-b) | a принадлежит A, b принадлежит B}.
Пример:
A={(-3,1), (-3,2), (-1,1)} и B={(0,0), (2,0), (0,2),(2,2)},
тогда A+B ={(-3,1),(-3,4),(-1,4),(1,3),(1,1)}
A={(0,2), (0,3), (2,2)} и B={(1,1), (3,1), (3,4),(1,4)},
тогда A-B ={(-1,-1),(-1,2),(3,2),(3,-2),(1,-2)}
Важной особенностью является то, что разница Минковского будет содержать всегда точку O(0,0), если множества пересекаются (на рисунке внизу случай, когда многогранники пересекаются, справа результат разницы Минковского).
В основе алгоритм GJK лежит тот факт, что два многогранника пересекаются только в том случае, если их разница Минковского C = A-B содержит начало системы координат (точку O(0,0), если какие – то два многогранника A и B разделяют одну точку (ai = bi), то их разница Минковского будет равна O, т.е. pi = ai – bi = O по определению). Поэтому проблема сокращается до нахождения ближайшей точки на C (выпуклом многограннике, полученном в результате разницы Минковского A и B) к O(началу системы координат). Сложность нахождения разницы Минковского равна O(m+n), но особенностью GJK является как раз то, что GJK не вычисляет в явном виде разницу Минковского, а только находит точку pi = ai-bi как краевую (экстремальную, максимальную) в заданном направлении, что совпадает с точкой из разницы Минковского посчитанной в явном виде.
Например, для круга с центром в C и радиусом R, в направлении dir краевая точка находится как p = C + R * dir
Для многогранника эта функция задается как
p = max{dot(v[i],dir)}
, где vi - вершины многогранника,i = 1,2,…N.
В общем виде для всех фигур:
p = support( dir );
Тогда краевая вершина на многограннике C будет находиться как A.support( dir ) - B.support( -dir )
Имеет смысл для каждой фигуры задавать свою support функцию.
Выделим абстрактный базовый класс Convex.
class Convex
{
public:
virtual ~Convex(){};
virtual vec2f support(const vec2f& dir ) = 0;
};
Для круга:
class Circle : public Convex
{
public:
vec2f C;
float R;
Circle(const vec2f& _C, float _R )
: C( _C)
, R( _R)
{}
vec2f support(const vec2f& normal )
{
return C + dir * R;
}
};
Для многогранника:
class Polygon : public Convex
{
public:
Polygon()
{
}
void addVert(float x, float y)
{
verts.push_back( vec2f(x,y));
}
vec2f support(const vec2f& normal )
{
float max = dot( verts[0], normal );
int index = 0;
for( size_t i = 1; i < verts.size(); i++ )
{
float dot = dot( verts[i], normal );
if( dot > max )
{
max = dot;
index = i;
}
}
return verts[index];
}
private:
std::vector<vec2f> verts;
};
Чтобы найти разницу Минковского C для точки ближайшей к O, GJK использует следующую теорему: Для выпуклого многогранника H, каждая точка из H может быть выражена как выпуклая комбинация не более чем d+1 точкой из H. Это позволяет GJK алгоритму искать объем разницы Минковского обновлением множества Q до d+1 точки из C на каждой итерации. Выпуклая оболочка Q формирует симплекс внутри C. Если O содержится в текущем симплексе, то алгоритм прекращает свою работу, иначе Q обновляется, формируя новый симплекс, который гарантированно будет содержать точки ближе к O, чем предыдущий симплекс. В итоге процесс остановится с Q, содержащим ближайшие точки к O.
Общая схема алгоритма:
1. Инициализируем симплекс Q d+1 точками из разницы Минковского A и B.
2. Находим ближайшую точку P от симплекса Q к O. Симплекс Q может быть точкой, прямой, треугольником или тетраэдром. Как в данном случае искать ближайшую точку до d-симплекса, будет рассказано далее.
3. Если P и есть начало координат, то начало координат содержится в разнице Минковского A и B. Останавливаемся и возвращаем, что A и B пересекаются.
4. Уменьшаем Q до подмножества Q', такого, что P принадлежит Q'. Это делается для того, что бы ни учитывать точки, которые заведомо дальше.
5. Пусть V = A.support( P ) - B.support( -P ) будет экстремальной точкой в направлении P.
6. Если нет точек "более экстремальных" в направлении P, тогда возвращаем, что A и B не пересекаются. Длина вектора от начала координат к P это разделяющее расстояние от A до B.
7. Добавляем V в Q и переходим на шаг 2.
Пример:
1. Инициализируем симплекс Q d+1 точками из разницы Минковского A и B. Поскольку рассматриваем 2d случай, то d = 2, т.е. добавляем 3 точки – Q1, Q2, Q3.
2. Находим ближайшую точку P от симплекса Q к O. Симплекс Q может быть точкой, прямой, треугольником или тетраэдром. Как в данном случае искать ближайшую точку до d-симплекса, будет рассказано далее.
3. Если P и есть начало координат, то начало координат содержится в разнице Минковского A и B. Останавливаемся и возвращаем, что A и B пересекаются.
4. Уменьшаем Q до подмножества Q', такого, что P принадлежит Q'. Это делается для того, что бы ни обрабатывать точки, которые заведомо дальше от O. На рисунке это точка Q0.
5. Пусть V = A.support( P ) - B.support( -P ) будет экстремальной точкой в направлении P.
6. Если V не является самой экстремальной точкой в направлении P, тогда возвращаем, что A и B не пересекаются. Длина вектора от начала координат к P это разделяющее расстояние от A до B.
7. Добавляем V в Q и переходим на шаг 2.
Шаг 2. Находим ближайшую точку от O к треугольнику (Q1, V, Q2).
Шаг 3, 4. Поскольку только через Q2 и V можно выразить P как выпуклую комбинацию вершин в Q, то Q = {Q2,V}. Или другими словами, проекция P на VQ1 будет за пределами VQ1, поэтому Q1 и убираем.
Шаг 5. Экстремальная вершина в направлении P даст Q2
Шаг 6. Поскольку не существует больше экстремальных вершин в данном направлении, то P есть ближайшая точка к O, а вершины многогранников A, B являются ближайшими точками.
Осталось описать, как находить ближайшую точку к d-симплексу. Тут может быть 3 случая:
1. точка O и линия.
x нормален к p0p1, если x(p1-p0) = 0. Подставляем x = lambda0p0 + lambda1p1, причем lambda0+lambda1 = 1, и больше 0.
Получаем систему: решая которую получаем точку P.
2. точка 0 и треугольник.
Аналогично. Только каждое ребро выражаем через случай 1. Необходимо рассмотреть 2 линии.
3. точка 0 и тетраэдр.
В этом случае выражаем через случай 2. Необходимо рассмотреть 4 треугольника.
Псевдокод:
void gjk(A,B)
{
V = any point in A – B;
W = empty;
while (v == 0)
{
w = A.support(dir) – B.support(-dir);
V = closest_point(conv(W U {w}));
}
return ||v||;
}