Определение области коллизии

1. Обе точки внутри фигуры. Самый простой случай, просто добавляем их в контактный буфер.

2. Только одна точка внутри фигуры. Вот это уже сложный случай. Нам необходимо проверить с каким именно ребром произошло пересечение, а после найти точку, в которой произошло пересечение. Когда речь пойдет о коде, я расскажу как это сделать.

3. Ни одна точка не лежит внутри. Опять проверяем на пересечения рёбра, ведь оно могло произойти где-то между вершин наших фигур. https://habr.com/ru/articles/753776/#comment_25845428

Пункт 3: проверка контактного буфера

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

Подготовка

Наконец, после долгих приготовлений, перейдём к коду.

Прежде чем описывать сам алгоритм стоит упомянуть все типы данных, которые будут в нем использоваться.

Первым делом поговорим о самом часто используемом типе — Vector3. Данный тип будет хранить координаты точки. Также внутри него определены некоторые математические операции, такие как DotProduct(), CrossProduct() и другие. Ближе поговорим о них по ходу алгоритма.

class Vector3 { public:      float X;      float Y;    float Z;  Vector3(float x = 0, float y = 0, float z = 0) { Vector3::X = x; Vector3::Y = y; Vector3::Z = z; } }

Следующим идёт тип Geometry. Представляет собой хранилище геометрии коллайдера. Объявить его можно следующим образом:

class Geometry : public Module { protected: Object* _parentObject;  float* _vertex; unsigned int _vertexCount = 0;  friend class Collision;  public: Geometry(); virtual ~Geometry();  Object* GetParentObject() override; void SetParentObject(const Object& parent) override;  ModulesList GetType() override;  virtual bool Create(std::string linkToFBX); virtual Vector3 FindFurthestPoint(Vector3 direction); };

Для удобного хранения результатов работы алгоритмов я использую следующий класс:

class CollisionInfo { public: Vector3 Normal; float PenetrationDepth; std::vector<Vector3> collisionPoints; };

Однако, глубина проникновения сегодня нам не понадобится.

Последним же классом будет класс плоскости. Ничего необычного, просто удобно хранить данные:

class Plane { public:     Vector3 P1;     Vector3 Normal;  public:     inline Plane(Vector3 p1, Vector3 normal) {         P1 = p1;         Normal = normal;     } };

Алгоритм

Наконец, объявим нашу невероятную функцию. Принимать она будет ссылки на геометрию коллайдеров, а также буфер информации о коллизии:

void Collision::CalculateContactPoints(                      Geometry& contactObject1,                       Geometry& contactObject2,                       CollisionInfo& colInfo);

Начнём по порядку. Первым делом найдем плоскость контакта. Для этого нам нужны 2 вещи — нормаль и одна точка, лежащая в этой плоскости. Нормаль у нас уже есть, её для нас нашёл EPA. Как же тогда найти точку?

На самом деле всё просто. Так как обе коллизии представляют собой именно выпуклые объекты, следовательно точка всегда будет самой дальней по направлению нормали.

От алгоритма EPA у вас наверняка осталась функция FindFurthestPoint(), цель которой помочь вам собрать опорную функцию. Используем её и здесь. В моём случае определение плоскости выглядит так:

Plane contactPlane{ contactObject1.FindFurthestPoint(colInfo.Normal), colInfo.Normal };

Переходим ко второму пункту — поиск вершин, принадлежащих контактной плоскости. Для этого сначала объявим массив для них:

std::vector<std::pair<Vector3, float>> contactPointsA; contactPointsA.reserve(4); std::vector<std::pair<Vector3, float>> contactPointsB; contactPointsB.reserve(4);

Каждая ячейка массива будет хранить 2 значения:

1. Позицию точки в мировых координатах.

2. Относительное вращение этой точки.

К тому что такое вращение точки и зачем нам это нужно мы вернемся позже.

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

Далее нам нужно будет найти эти точки. Создадим для этого анонимную функцию:

auto findContactPoints = [](Geometry& contactObject, Plane contactPlane, std::vector<std::pair<Vector3, float>>& contactBuf) {      for (size_t i = 0; i < contactObject._vertexCount * 3; i += 3)     {         Vector3 point{                    contactObject._vertex[i + 0],                   contactObject._vertex[i + 1],                   contactObject._vertex[i + 2] };         point += contactObject.GetParentObject()->GetPosition();          float distance = Vector3::GetVertexToPlaneDistance(                                                  point,                                                   contactPlane.P1,                                                   contactPlane.Normal);          if (distance > 0.005f)             continue;          contactBuf.push_back({ point,0 });     } }; 

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

Тут стоит сказать почему я не проверяю равна ли дистанция 0, ведь только в этом случае точка действительно принадлежит плоскости. Для объяснения этого давайте вспомним что именно возвращает нам алгоритм EPA:

colInfo.PenetrationDepth = minDistance + 0.001f;

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

Вывод: при проверки принадлежности точке к контактной плоскости добавьте некоторый эпсилон.

Следующим пунктом мы должны убедиться в типе коллизии. Есть это vertex by face, тогда всё просто. Возвращаем единственную точку и выходим из алгоритма:

if (contactPointsA.size() == 1) {         colInfo.collisionPoints.push_back(contactPointsA[0].first);         return;     } else if (contactPointsB.size() == 1) {         colInfo.collisionPoints.push_back(contactPointsB[0].first);         return;     }

Если мы убедились, что это не vertex by face контакт, то для начала придется выполнить намотку объекта. Вычислим ориджин объекта:

auto findOrigin = [](const std::vector<std::pair<Vector3, float>>& points) {     Vector3 origin{ 0,0,0 };     for (size_t i = 0; i < points.size(); i++) {         origin += points[i].first;     }     origin /= points.size();     return origin; };

Ориджином я называю точку внутри нашей 2д проекции столкновения. Она не обязательно должна находиться в центре и может зависеть от плотности сетки. В нашем случае она просто должна быть внутри.

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

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

Однако этого угла нам недостаточно, так как он не показывает расположение этой точки (по часовой стрелки или против). Соответственно, значение больше чем 180 градусов мы получить не сможем. Благо узнать где находиться точка относительно вектора не сложно, просто найдем нормаль нашей 2д фигуры.

Из-за некоммутативности векторного произведения мы получим разные вектора при их умножении в разном порядке. Поэтому, имея нормаль, мы можем сравнить её с нормалью контакта и с уверенностью сказать относительное положение точки — слева или справа.

auto calculateAngles = [findOrigin](std::vector<std::pair<Vector3, float>>& contactPoints, Vector3 normal) {     Vector3 originA = findOrigin(contactPoints);     Vector3 refVector = contactPoints[0].first - originA;     for (size_t i = 1; i < contactPoints.size(); i++) {         Vector3 originToPoint = contactPoints[i].first - originA;         float u = Vector3::DotProduct(normal, Vector3::CrossProduct(refVector, originToPoint));         float angle = Vector3::GetAngle(refVector, originToPoint) * 180 / M_PI;          //if u <= 0 than point is left         if (u <= 0.001f) {             contactPoints[i].second = angle;         }         else         {             contactPoints[i].second = angle + 180;         }     } };

Стоит также отметить, что проверку стоит сделать именно так: if (u <= 0.001f), а не так: if (u <= 0). Причиной тому является банальная машинная ошибка. Существует вероятность получить на выходе значение очень близкое к 0, из-за чего вместо 180 градусов, точка получит все 360.

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

auto compareAngle = [](const std::pair<Vector3, float>& pA, const std::pair<Vector3, float>& pB) {     if (pA.second < pB.second) {         return true;     }     return false; };  std::sort(contactPointsA.begin(), contactPointsA.end(), compareAngle); std::sort(contactPointsB.begin(), contactPointsB.end(), compareAngle);

В данном случае порядок сортировки (по часовой стрелки или против) абсолютно не важен. Наматывать можно в любом порядке.

После успешной намотки переходим к самому интересному — проверка пересечений. Создаём буфер для хранения реальных опорных точек контакта и определяем функцию для поиска пересечений:

std::vector<Vector3> realContactPoints;

static void CheckIntersection( std::vector<std::pair<Vector3, float>>& contactPointsA, std::vector<std::pair<Vector3, float>>& contactPointsB, Vector3 normal, std::vector<Vector3>& contactPointsBuf);

Создаём цикл для обработки пересечений, не забываем про последнее ребро:

for (size_t it = 0; it < contactPointsB.size(); it++) {     size_t itSecondP = it + 1;     if (itSecondP == contactPointsB.size()) {         itSecondP = 0;     }

После по очереди проверяем каждую из точек ребра на принадлежность фигуре:

//We assume that the first point is inside unsigned int passedCount = 1;  //Check if first point inside shape for (size_t jt = 0; jt < contactPointsA.size(); jt++) {     size_t jtSecondP = jt + 1;     if (jtSecondP == contactPointsA.size()) {         jtSecondP = 0;     }      Vector3 vct = contactPointsB[it].first - contactPointsA[jt].first;     Vector3 edgeVector = contactPointsA[jtSecondP].first - contactPointsA[jt].first;      Vector3 norm = Vector3::CrossProduct(Vector3::GetNormalize(edgeVector), normal);      if (Vector3::DotProduct(norm, vct) < 0) {         passedCount--;         break;     } }

Для начала делаем предположение, что первая точка находится внутри и устанавливаем passedCount в значение 1. После нам нужно доказать, что точка находится внутри фигуры. Я решил исходить из того, что если точка находиться вне фигуры, то существует такая нормаль у ребра, которая смотрит от неё. Выглядит это примерно так:

Стоит отметить, что в данном случае очень важно правильно определить нормаль ребра, дабы она не смотрела от центра фигуры.

Если после выхода из цикла passedCount всё ещё равен 1, значит точка располагается внутри. Добавляем её в наш массив опорных точек контакта:

if (passedCount == 1)     contactPointsBuf.push_back(contactPointsB[it].first);

Точно так-же проверяем вторую точку ребра из фигуры B. Единственно отличие — её мы НЕ добавляем в буфер. Делается это для того, чтобы не плодить дубликаты, ведь мы её добавим при следующем проходе цикла.

Далее проверяем какой у нас тип контакта и нужно ли исследовать ребро на пересечение:

//all points of current line are inside shape if (passedCount == 2)     continue;

Переходим к заключительному пункту нашей проверки на пересечение. Для начала определим цикл проверки и найдём ближайшую точку между рёбрами. Ближайшую нужно находить по той причине, что рёбра эти вряд-ли вообще пересекаются (вспоминаем про добавленный эпсилон к минимальной дистанции выталкивания). Также не забываем проверить параллельность рёбер. Если они параллельны — переходим к следующему ребру.

for (size_t jt = 0; jt < contactPointsA.size(); jt++) {     size_t jtSecondP = jt + 1;     if (jtSecondP == contactPointsA.size()) {         jtSecondP = 0;     }      Vector3 projectPoint;      bool notParallel = Vector3::ClosetPointBetweenAxis(         { contactPointsB[it].first, contactPointsB[itSecondP].first },         { contactPointsA[jt].first, contactPointsA[jtSecondP].first }, projectPoint);      if (!notParallel)         continue;

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

float vct1 = Vector3::GetNonSqrtMagnitude(contactPointsB[it].first - projectPoint); float vct2 = Vector3::GetNonSqrtMagnitude(contactPointsB[itSecondP].first - projectPoint);  float lengthAxisB = Vector3::GetNonSqrtMagnitude(contactPointsB[itSecondP].first - contactPointsB[it].first); float lengthAxisA = Vector3::GetNonSqrtMagnitude(contactPointsA[jtSecondP].first - contactPointsA[jt].first);  if (vct1 > lengthAxisA || vct2 > lengthAxisA)     continue;  if (vct1 > lengthAxisB || vct2 > lengthAxisB)     continue;  contactPointsBuf.push_back(projectPoint); 

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

В конце добавляем точку внутрь контактного буфера.

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

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

//All point shape B outside shape A //So All points shape A inside shapeB if (realContactPoints.size() == 0)     CheckIntersection(contactPointsB, contactPointsA, colInfo.Normal, realContactPoints);

На этом всё, наш алгоритм написан, осталось только куда-то вернуть эти точки. Я передаю их в буфер, после сохраняя их внутрь физической модели объекта:

colInfo.collisionPoints = { realContactPoints.begin(), realContactPoints.end() };

Проверка алгоритма

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

Полная версия кода

void Collision::CalculateContactPoints(Geometry& contactObject1, Geometry& contactObject2, CollisionInfo& colInfo) {     auto findOrigin = [](const std::vector<std::pair<Vector3, float>>& points) {         Vector3 origin{ 0,0,0 };         for (size_t i = 0; i < points.size(); i++) {             origin += points[i].first;         }         origin /= points.size();         return origin;     };     auto compareAngle = [](const std::pair<Vector3, float>& pA, const std::pair<Vector3, float>& pB) {         if (pA.second < pB.second) {             return true;         }         return false;     };     auto findContactPoints = [](Geometry& contactObject, Plane contactPlane, std::vector<std::pair<Vector3, float>>& contactBuf) {           for (size_t i = 0; i < contactObject._vertexCount * 3; i += 3)         {             Vector3 point{ contactObject._vertex[i + 0],contactObject._vertex[i + 1],contactObject._vertex[i + 2] };             point += contactObject.GetParentObject()->GetPosition();              float distance = Vector3::GetVertexToPlaneDistance(point, contactPlane.P1, contactPlane.Normal);             if (distance > 0.005f)                 continue;              contactBuf.push_back({ point,0 });         }     };     auto calculateAngles = [findOrigin](std::vector<std::pair<Vector3, float>>& contactPoints, Vector3 normal) {         Vector3 originA = findOrigin(contactPoints);         Vector3 refVector = contactPoints[0].first - originA;         for (size_t i = 1; i < contactPoints.size(); i++) {             Vector3 originToPoint = contactPoints[i].first - originA;             float u = Vector3::DotProduct(normal, Vector3::CrossProduct(refVector, originToPoint));             float angle = Vector3::GetAngle(refVector, originToPoint) * 180 / M_PI;              //if u <= 0 than point is left             if (u <= 0.001f) {                 contactPoints[i].second = angle;             }             else             {                 contactPoints[i].second = angle + 180;             }         }     };      //Point and angle     std::vector<std::pair<Vector3, float>> contactPointsA; contactPointsA.reserve(4);     std::vector<std::pair<Vector3, float>> contactPointsB; contactPointsB.reserve(4);      colInfo.Normal.NormilizeSelf();     Plane contactPlane{ contactObject1.FindFurthestPoint(colInfo.Normal), colInfo.Normal };      //Finding contact points from shape A     findContactPoints(contactObject1, contactPlane, contactPointsA);      //Finding contact points from shape B     findContactPoints(contactObject2, contactPlane, contactPointsB);      if (contactPointsA.size() == 0) {         for (size_t i = 0; i < contactPointsB.size(); i++)         {             colInfo.collisionPoints.push_back(contactPointsB[i].first);         }         return;     }     if (contactPointsB.size() == 0) {         for (size_t i = 0; i < contactPointsA.size(); i++)         {             colInfo.collisionPoints.push_back(contactPointsA[i].first);         }         return;     }      //Vertex to face contact     if (contactPointsA.size() == 1) {         colInfo.collisionPoints.push_back(contactPointsA[0].first);         return;     }     else if (contactPointsB.size() == 1) {         colInfo.collisionPoints.push_back(contactPointsB[0].first);         return;     }      //calculate angle     calculateAngles(contactPointsA, colInfo.Normal);     calculateAngles(contactPointsB, colInfo.Normal);      //Rebuild shapes correctly(by clock)     std::sort(contactPointsA.begin(), contactPointsA.end(), compareAngle);     std::sort(contactPointsB.begin(), contactPointsB.end(), compareAngle);      std::vector<Vector3> realContactPoints;     CheckIntersection(contactPointsA,                        contactPointsB,                        colInfo.Normal,                        realContactPoints);      //All point shape B outside shape A     //So All points shape A inside shapeB     if (realContactPoints.size() == 0) {         CheckIntersection(contactPointsB,                            contactPointsA,                            colInfo.Normal,                            realContactPoints);     }      colInfo.collisionPoints = { realContactPoints.begin(), realContactPoints.end() }; }   void Collision::CheckIntersection(     std::vector<std::pair<Vector3, float>>& contactPointsA,     std::vector<std::pair<Vector3, float>>& contactPointsB,     Vector3 normal,     std::vector<Vector3>& contactPointsBuf) {      for (size_t it = 0; it < contactPointsB.size(); it++)     {         size_t itSecondP = it + 1;         if (itSecondP == contactPointsB.size()) {             itSecondP = 0;         }          //We assume that the first point is inside         unsigned int passedCount = 1;          //Check if first point inside shape         for (size_t jt = 0; jt < contactPointsA.size(); jt++) {             size_t jtSecondP = jt + 1;             if (jtSecondP == contactPointsA.size()) {                 jtSecondP = 0;             }              Vector3 vct = contactPointsB[it].first - contactPointsA[jt].first;             Vector3 edgeVector = contactPointsA[jtSecondP].first - contactPointsA[jt].first;              Vector3 norm = Vector3::CrossProduct(Vector3::GetNormalize(edgeVector), normal);              if (Vector3::DotProduct(norm, vct) < 0) {                 passedCount--;                 break;             }         }         if (passedCount == 1)             contactPointsBuf.push_back(contactPointsB[it].first);           //Check if second point inside shape         //We assume that the second point is inside         passedCount++;         for (size_t jt = 0; jt < contactPointsA.size(); jt++) {             size_t jtSecondP = jt + 1;             if (jtSecondP == contactPointsA.size()) {                 jtSecondP = 0;             }             Vector3 vct = contactPointsB[itSecondP].first - contactPointsA[jt].first;             Vector3 edgeVector = contactPointsA[jtSecondP].first - contactPointsA[jt].first;              Vector3 norm = Vector3::GetNormalize(Vector3::CrossProduct(Vector3::GetNormalize(edgeVector), normal));              if (Vector3::DotProduct(norm, vct) < 0) {                 passedCount--;                 break;             }         }          //all points of current line is inside shape         if (passedCount == 2)             continue;          //Check if line separate shape         for (size_t jt = 0; jt < contactPointsA.size(); jt++) {             size_t jtSecondP = jt + 1;             if (jtSecondP == contactPointsA.size()) {                 jtSecondP = 0;             }              Vector3 projectPoint;              bool notParallel = Vector3::ClosetPointBetweenAxis(                 { contactPointsB[it].first, contactPointsB[itSecondP].first },                 { contactPointsA[jt].first, contactPointsA[jtSecondP].first }, projectPoint);              if (!notParallel)                 continue;              float vct1 = Vector3::GetNonSqrtMagnitude(contactPointsB[it].first - projectPoint);             float vct2 = Vector3::GetNonSqrtMagnitude(contactPointsB[itSecondP].first - projectPoint);              float lengthAxisB = Vector3::GetNonSqrtMagnitude(contactPointsB[itSecondP].first - contactPointsB[it].first);             float lengthAxisA = Vector3::GetNonSqrtMagnitude(contactPointsA[jtSecondP].first - contactPointsA[jt].first);              if (vct1 > lengthAxisA || vct2 > lengthAxisA)                 continue;              if (vct1 > lengthAxisB || vct2 > lengthAxisB)                 continue;              contactPointsBuf.push_back(projectPoint);         }     } }

Также его можно найти тут.

Заключение

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

Надеюсь эта статья поможет вам в будущих начинаниях.

ссылка на оригинал статьи https://habr.com/ru/articles/753776/