OpenGL: Коллизия линии и полигона

#include <float.h>  // Чтобы использовать _isnan() для acos()#define PI 3.1415926535897932       // Наш знаменитый ПИ %)

// Изменим описание функции IntersectedPlane(), ниже опишу зачем:
bool IntersectedPlane(CVector3 vPoly[], CVector3 vLine[], CVector3 &vNormal, float &originDistance);

// Возвращяет dot product между двумя векторами
float Dot(CVector3 vVector1, CVector3 vVector2);

// Возвращает угол между двумя векторами
double AngleBetweenVectors(CVector3 Vector1, CVector3 Vector2);

// Возвращает точку пересечения полигона и линии (подразумевается пересечение плоскости)
CVector3 IntersectionPoint(CVector3 vNormal, CVector3 vLine[], double distance);

// Возвращает true если точка пересечения находится внутри полигона
bool InsidePolygon(CVector3 vIntersection, CVector3 Poly[], long verticeCount);

// Используйте эту функцию для теста пересечения линии и полигона
bool IntersectedPolygon(CVector3 vPoly[], CVector3 vLine[], int verticeCount);

distance2 = ((vNormal.x * vLine[1].x)  +                    // Ax +
(vNormal.y * vLine[1].y)  +                    // Bx +
(vNormal.z * vLine[1].z)) + originDistance;    // Cz + D

// Проверим на пересечение

if(distance1 * distance2 >= 0)
return false;

return true;
}

// Следующая функция производит «рассчет оператора точки» (Dot product):
float Dot(CVector3 vVector1, CVector3 vVector2)
{
// Вот формула Dot product: V1.V2 = (V1.x * V2.x  +  V1.y * V2.y  +  V1.z * V2.z)
// В математическом представлении она выглядит так: V1.V2 = ||V1|| ||V2|| cos(theta)
// ‘.’ называется DOT. || || — величина, она всегда положительна. То есть величина V1
// умножить на величину V2 умножить на косинус угла. Выглядит устрашающе, но позже станет
// яснее. Эта функция используются во множестве ситуаций, которые будут описаны в других
// уроках. В этом уроке с её помощью мы будем получать угол между двумя векторами.
// Если векторы нормализованы, dot product вернет косинус угла между 2мя векторами.
// Что это значит? Это значит, что на самом деле возвращается не сам угол, а cos(angle).
// Ну а что если мы хотим получить сам угол? Тогда мы используем аркосинус.
// Больше об этом будет написано в функции AngleBetweenVectors(). Давайте рассмотрим
// пример использования dot product. Как вычислить угол между перпендикулярными векторами?
// Если мы нормализуем вектор, мы можем получить результат ||V1|| * ||V2||, останется только
// найти cos(theta). Если вектор нормализован, его величина — 1, так что получится 1*1*cos(theta),
// что бессмысленно, так что мы отбрасываем эту часть формулы. Итак, что такое косинус 90?
// Если вы возьмете калькулятор, то узнает, что это 0. И получается, что если результат Dot()
// равен нулю, векторы перпендикулярны. Всё что мы делали — получили аркосинус нуля, который
// равен 90.

//    (V1.x * V2.x        +        V1.y * V2.y        +        V1.z * V2.z)
return ( (vVector1.x * vVector2.x) + (vVector1.y * vVector2.y) + (vVector1.z * vVector2.z) );
}

////////////////////////////////////////////////////////////////////////////////
//
// Следуюшая функция возвращает угол между векторами
//
////////////////////////////////////////////////////////////////////////////////

double AngleBetweenVectors(CVector3 Vector1, CVector3 Vector2)
{
// Помните, выше мы говорили, что Dot product возвращает косинус угла между
// двумя векторами? Подразумевается, что векторы нормализованы. И, если у нас нет
// нормализованного вектора, то просто делаем arcCos(DotProduct(A, B))
// Нам нужно разделить dot product на величину двух умноженных друг на друга
// векторов. Вот формула: arc cosine of (V . W / || V || * || W || )
// ||V|| — это величина V. Это «отменит» величины dot product.
// Но если вы уже нормализовали векторы, вы можете забыть о величинах.

// Получаем Dot от обоих векторов
float dotProduct = Dot(Vector1, Vector2);

// Получаем умножение величин обоих векторов
float vectorsMagnitude = Magnitude(Vector1) * Magnitude(Vector2) ;

// Получаем аркосинус от (dotProduct / vectorsMagnitude), что есть угол в градусах.
double angle = acos( dotProduct / vectorsMagnitude );

// Теперь убедимся, что угол не -1.#IND0000000, что означает «недостижим». acos() видимо
// считает прикольным возвращать -1.#IND0000000. Если мы не сделаем этой проверки, результат
// проверки пересечения будет иногда показывать true, когда на самом деле пересечения нет.
// я выяснил эту фичу тяжким трудом после МНОГИХ часов и уже написанных неверных уроков 😉
// Обычно это значение возвращается, когда dot product и величина имеют одинаковое значение.
// Мы вернём 0 если это случается.

if(_isnan(angle))
return 0;

// Вернем угол в градусах
return( angle );
}

/////////////////////////////////// INTERSECTION POINT \\\\\\\\\\\\\\\\\\\*
/////
/////   Возвращает точку пересечения линии и плоскости
/////
/////////////////////////////////// INTERSECTION POINT \\\\\\\\\\\\\\\\\\\*

CVector3 IntersectionPoint(CVector3 vNormal, CVector3 vLine[], double distance)
{
CVector3 vPoint = {0}, vLineDir = {0};      // Переменные для точки пересечения и направления линии
double Numerator = 0.0, Denominator = 0.0, dist = 0.0;

// Здесь немного сложная часть. Нам нужно найти 3д точку, находящуюся на плоскости.
// Вот шаги для реализации этого:

// 1) Сначала нам нужно получить вектор нашей линии, затем нормализовать его, чтобы длинна была 1
vLineDir = Vector(vLine[1], vLine[0]);      // Получим вектор линии
vLineDir = Normalize(vLineDir);         // Нормализуем его

// 2) Используем формулу плоскости (дистанция = Ax + By + Cz + D) чтобы найти дистанцию от одной из
// точек до плоскости. Делаем дистанцию отрицательной, т.к. нам нужно идти НАЗАД от нашей точки
// к плоскости. Это действие просто возвращает назад к плоскости, чтобы найти точку пересечения.

Numerator = — (vNormal.x * vLine[0].x +     // Используем формулу плоскости с нормалью и линией
vNormal.y * vLine[0].y +
vNormal.z * vLine[0].z + distance);

// 3) Если мы получим Dot product между вектором нашей линии и нормалью полигона,
// это даст нам косинус угла между 2мя (т.к. они обе нормализованы — длинна 1).
// Затем мы разделим Numerator на это значение чтобы найти отстояние плоскости от начальной точки.

Denominator = Dot(vNormal, vLineDir);   // Получаем Dot pruduct между линией и нормалью

// Так как мы используем деление, нужно уберечься от ошибки деления на ноль. Если мы получим 0,
// это значит, это НЕДОСТИЖИМАЯ точка, т.к. линая находится на плоскости (нормаль перпендикулярна
// к линии — (Normal.Vector = 0)).
// В этом случае просто вернем любую точку на линии.

if( Denominator == 0.0)     // Проверим, не делим ли мы на ноль
return vLine[0];    // Вернем любую точку линии

// Мы делим (дистанция от отчки до плоскости) на (dot product), чтобы получить дистанцию
// (dist), которая нужна нам для движения от начальной точки линии. Нам нужно умножить эту дистанцию (dist)
// на вектор линии (направление). Когда вы умножаете scalar на ветор, вы двигаетесь вдоль
// этого вектора. Это и есть то, что мы делаем. Мы двигаемся от нашей начальной точки, выбранной
// на линии, НАЗАД к плоскости вдоль вектора линии. Логично было бы просто получить Numerator,
// который является дистанцией от точки до линии, а потом просто двигатся назад вдоль вектора линии.
// Дистанция от плоскости — имеется в виду САМАЯ КОРОТКАЯ дистанция. Что если линия почти параллельна
// полигону, и не пересекается с ним на протяжении своей длинны? Расстояние до плоскости мало, но
// расстояние до точки пересечения вектора линии с плоскостью очень велико. Если мы разделим
// дистанцию на dot product из вектора линии и нормали плоскости, то получим правильную длинну.

dist = Numerator / Denominator;

// Теперь, как и говорилось выше, делим дистанцию на вектор, потом добавляем точку линии.
// Это переместит точку вдоль вектора на некую дистанцию. Это в свою очередь даст
// нам точку пересечения.

vPoint.x = (float)(vLine[0].x + (vLineDir.x * dist));
vPoint.y = (float)(vLine[0].y + (vLineDir.y * dist));
vPoint.z = (float)(vLine[0].z + (vLineDir.z * dist));

return vPoint;          // Вернем точку пересечения.
}

/////////////////////////////////// INSIDE POLYGON \\\\\\\\\\\\\\\\\\\*
/////
/////   Проверяет, находится ли точка внутри полигона
/////
/////////////////////////////////// INSIDE POLYGON \\\\\\\\\\\\\\\\\\\*

bool InsidePolygon(CVector3 vIntersection, CVector3 Poly[], long verticeCount)
{
const double MATCH_FACTOR = 0.9999;     // Исп. для покрытия ошибки плавающей точки
double Angle = 0.0;             // Инициализируем угол
CVector3 vA, vB;                // Временные векторы

// Одно то, что линия пересекает плоскость, ещё не значит, что она пересекает полигон в
// этой плоскости. Эта функция проверяет точку пересечения на предмет того, находится ли
// она внутри полигона.
// На самом деле мы используем замечательный метод. Он создает треугольники внутри
// полигона от точки пересечения, проводя линии к каждой вершине полигона. Потом все углы
// созданных треугольников складываются. И если сумма углов равна 360, то мы внутри!
// Если же значение меньше 360, мы снаружи полигона.
// Чтобы лучше понять, как это работает, возьмите карандаш и нарисуйте
// равносторонний треугольник. Нарисуйте точку в его центре. Теперь от этой точки проведите линии
// к каждой вершине треугольника. Таким образом у нас получилось 3 треугольника внутри главного, верно?
// Теперь. Мы знаем, что если сложим все углы треугольника, то получим 180, верно?
// Почти в точности это мы и делаем.

for (int i = 0; i < verticeCount; i++)      // Проходим циклом по каждой вершине и складываем их углы
{
vA = Vector(Poly[i], vIntersection);    // Вычитаем точку пересечения из текущей вершины

// Вычитаем точку пересечения из следующей вершины:
vB = Vector(Poly[(i + 1) % verticeCount], vIntersection);

// Находим угол между 2мя векторами и складываем их все
Angle += AngleBetweenVectors(vA, vB);
}

// Теперь имея сумму углов, нам нужно проверить, равны ли они 360. Так как мы используем
// Dot product, мы работаем в радианах, так что проверим, равны ли углы 2*PI. PI мы обьявили в 3dmath.h.
// Вы заметите, что мы используем MATH_FACTOR. Мы используем его из-за неточности в рассчетах
// с плавающей точкой. Обычно результат не будет ровно 2*PI, так что нужно учесть маленькую
// погрешность. Я использовал .9999, но вы можете изменить это на ту погрешность, которая вас
// устроит.

if(Angle >= (MATCH_FACTOR * (2.0 * PI)) )   // Если угол >= 2PI (360 градусов)
return TRUE;                // Точка находится внутри полигона

return FALSE;       // Иначе — снаружи
}

/////////////////////////////////// INTERSECTED POLYGON \\\\\\\\\\\\\\\\\\\*
/////
/////   Проверяет, пересекается ли линия с полигоном
/////
/////////////////////////////////// INTERSECTED POLYGON \\\\\\\\\\\\\\\\\\\*

bool IntersectedPolygon(CVector3 vPoly[], CVector3 vLine[], int verticeCount)
{
CVector3 vNormal = {0};
float originDistance = 0;

// Сначала проверяем, пересекает ли наша линия плоскость. Если нет — то не нужно
// продолжать, полигон на плоскости она тоже не пересекает.
// Передаем в функцию адрес vNormal и originDistance, IntersectedPlane вычислит их для нас.

// Указатель   // Указатель
if(!IntersectedPlane(vPoly, vLine,   vNormal,   originDistance))
return false;

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

CVector3 vIntersection = IntersectionPoint(vNormal, vLine, originDistance);

// Теперь, имея точку пересечения, нужно проверить, лежит ли она внутри полигона.
// Чтобы сделать это, передаём:
// (точка пересечения, полигон, количество вершин полигона).

if(InsidePolygon(vIntersection, vPoly, verticeCount))
return true;            // Есть пересечение! Вернём true

// Если мы дошли досюда, пересечения нет, вернём false

return false;
}

case VK_RIGHT:
vTriangle[0].z += 0.01f;
vTriangle[1].z += 0.01f;
vTriangle[2].z += 0.01f;
break;
}

break;

///////////////////////////////////////////////////////////////////
//
// А в функции RenderScene() просто заменим одну функцию другой.

// Вместо
bool bCollided = IntersectedPlane(vTriangle, vLine);

// напишем
bool bCollided = IntersectedPolygon(vTriangle, vLine, 3);