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

Поиск контактных точек коллизии

Введение

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

Ожидая получить ответ на свой вопрос в интернете я начал искать его. Однако, что меня удивило, нет ни одного внятного объяснения решения данной проблемы. В лучшем случае вы найдёте небольшой комментарий с кратким описанием того, что должны сделать.

Дабы закрыть эту проблему я решил написать собственную статью о нахождении точек контакта и помочь будущим поколениям.

Необходимые знания

Наивный подход

Прежде давайте вспомним как работает EPA.

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

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

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

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

Теория 

Для начала кратко опишу список того, что предстоит сделать:

1. Найти плоскости контакта.

2. Найти вершины, принадлежащие этой плоскости.

3. Определить реальную область контакта.

Простой случай

Для наглядного примера представим что мы столкнули 2 куба:

Алгоритм EPA вернёт нам нормаль контакта (зеленая стрелка). Дальше нашей задачей будет собрать плоскость контакта (красная плоскость). Как именно ее получить будет рассказано ниже. Затем находим вершины, которые принадлежат этой плоскости. Как видно из картинки их 5.

После составляем 2д фигуры из этих вершин и ищем где они пересекаются. В нашем упрощенном случае можно сразу сказать где именно произошло пересечение - в точке B. 

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

Сложный случай

И так, перейдём к более сложному случаю, когда пересечение выглядит примерно так:

Теперь же опорных точек пересечения больше, чем точек на фигуре B. К пересечению vertex by face прибавились ещё edge by edge (на картинке отмечены синими точками). Но и не стоит забывать про вершины фигуры B. Если в прошлом случае мы могли сразу сказать, что эта вершина и была точкой пересечения, то теперь мы должны это ещё доказать.

Кратко составим список того, что необходимо выполнить:

1. Выполнить намотку вершин.

2. Проверить, принадлежит ли вершина n и n + 1 фигуры B фигуре A.

   1. Если принадлежат обе вершины - добавляем их в список контактных точек.

   2. Если принадлежит только 1 или 0, то ищем где именно проходит пересечение.

3. Проверяем контактный буфер. Если его размер равен 0 - возвращаемся к пункту 2, но уже проверяем фигуры наоборот.

Теперь же стоит более подробно рассказать про каждый из пунктов.

Пункт 1: намотка вершин

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

Как видно, пересечений между ними нет, соответственно наш алгоритм даст сбой.

Пункт 2: проверка пересечений

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

Теперь разберём все возможные случаи при проверке пересечений.

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

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

Пункт 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();
	~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);
        }
    }
}

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

Заключение

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

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