Nowadays, computers are powerful computers,
capable of performing millions of operations per second. And of course you can’t do without a simulation of the real or game world. One of the tasks of computer modeling and simulation is to determine the collision of two objects, one of the solutions of which is realized by the separation axis theorem.

ITKarma picture

Note: The article will give an example with 2 parallelepipeds (hereinafter referred to as cubes), but the idea for other convex objects will be preserved.
Note: All implementation will be done in Unity.

Act 0. General theory


First you need to get acquainted with the " dividing hyperplane theorem ". It will be the basis of the algorithm.

Theorem. Two convex geometries do not intersect, if and only if there is a hyperplane between them that separates them. Axis orthogonal separating
hyperplanes are called the dividing axis, and the projections of the figures on it do not intersect.

ITKarma picture
Separating axis (two-dimensional case)

ITKarma picture
Separating axis (three-dimensional case)
You may notice that the projections on the dividing axis do not intersect.

Property. The potential dividing axis will be in the following sets:
  • The norms of the planes of each cube (red)
  • Vector product of cube edges $ \ {[\ vec {x}, \ vec {y}]: x∈X, y∈Y \} $ ,

where X is the edges of the first cube (green) and Y is the second (blue).

ITKarma picture

We can describe each cube with the following input data:
  • Cube center coordinates
  • Cube dimensions (height, width, depth)
  • Quaternion of the cube

Let's create an additional class for this, instances of which will provide information about the cube.
public class Box { public Vector3 Center; public Vector3 Size; public Quaternion Quaternion; public Box(Vector3 center, Vector3 size, Quaternion quaternion) { this.Center=center; this.Size=size; this.Quaternion=quaternion; }//дополнительный конструктор, который//позволяет сгенерировать объект на основе GameObject public Box(GameObject obj) { Center=obj.transform.position; Size=obj.transform.lossyScale; Quaternion=obj.transform.rotation; } } 

Act 1. Quaternions


As often happens, an object can rotate in space. In order to find the coordinates of the vertices, taking into account the rotation of the cube, you need to understand what a quaternion is.

Quaternion is a hypercomplex number that determines the rotation of an object in space.

$ w + xi + yj + zk $


The imaginary part (x, y, z) represents a vector that determines the direction of rotation
The real part (w) determines the angle by which rotation will take place.

Its main difference from all the usual Euler angles in that it’s enough for us to have a one vector that will determine the direction of rotation than three linearly independent vectors that rotate an object in 3 subspaces.

I recommend two articles detailing quaternions:

Times
Two

Now that we have a minimal understanding of quaternions, let's understand how to rotate a vector and describe the rotation function of a vector by a quaternion.

Vector rotation formula

$ \ vec {v} '= q * \ vec {v} * \ overline {q} $


$ \ vec {v} '$- Search Vector
$ \ vec {v} $- source vector
$ q $- quaternion
$ \ overline {q} $- reverse quaternion

To begin with, we give the concept of the inverse quaternion in an orthonormal basis — it is a quaternion with an imaginary part opposite in sign.

$ q=w + xi + yj + zk $
$ \ overline {q}=w-xi-yj-zk $

Let's calculate $ \ vec {v} * \ overline {q} $

$ M=\ vec {v} * \ overline {q}=(0 + v_xi + v_yj + v_zk) (q_w - q_xi - q_yj - q_zk)=$
$=\ color {red} {v_xq_wi} + \ color {purple} {v_xq_x } - \ color {blue} {v_xq_yk} + \ color {green} {v_xq_zj} + $
$ + \ color {green} {v_yq_wj} + \ color {blue} {v_yq_xk } + \ color {purple} {v_yq_y} - \ color {red} {v_yq_zi} + $
$ + \ color {blue} {v_zq_wk} - \ color {green} {v_zq_x } + \ color {red} {v_zq_yi} + \ color {purple} {v_zq_z} $

Now we will write out the individual components and from this work we will collect a new quaternion
$ M=u_w + u_xi + u_yj + u_zk $
$ u_w=\ color {purple} {v_xq_x + v_yq_y + v_zq_z} -tex=
$ u_xi=\ color {red} {(v_xq_w - v_yq_z + v_z} $
$ u_yj=\ color {green} {(v_xq_z + v_yq_w) j_z_v_z $
$ u_zk=\ color {blue} {(- v_xq_y + v_yq_x + v_z_ + +___ } $

We calculate the remainder, i.e. $ q * M $and get the desired vector.

Note: In order not to clutter up the calculations, we give only the imaginary (vector) part of this work. After all, it is she who characterizes the desired vector.

$\vec{v}'=q*M=(q_w + q_xi + q_yj + q_zk)(u_w + u_xi + u_yj + u_zk) =$
$= \color{red}{q_wu_xi} + \color{green}{q_wu_yj} + \color{blue}{q_wu_zk} + $
$ +\color{red}{q_xu_wi} + \color{blue}{q_xu_yk} - \color{green}{q_xu_zj} +$
$+\color{green}{q_yu_wj} - \color{blue}{q_yu_xk} + \color{red}{q_yu_zi} +$
$+\color{blue}{q_zu_wk} +\color{green}{q_zu_xj} -\color{red}{q_zu_yi}$

Соберем компоненты вектора

$v_x'=\color{red}{q_wu_x + q_xu_w + q_yu_z - q_zu_y}$
$v_y'=\color{green}{q_wu_y - q_xu_z + q_yu_w + q_zu_x}$
$v_z'=\color{blue}{q_wu_z + q_xu_y - q_yu_x + q_zu_w}$

$v'=(v_x', v_y', v_z')$
Таким образом необходимый вектор получен

Напишем код:

private static Vector3 QuanRotation(Vector3 v,Quaternion q) { float u0=v.x * q.x + v.y * q.y + v.z * q.z; float u1=v.x * q.w - v.y * q.z + v.z * q.y; float u2=v.x * q.z + v.y * q.w - v.z * q.x; float u3=-v.x * q.y + v.y * q.x + v.z * q.w; Quaternion M=new Quaternion(u1,u2,u3,u0); Vector3 resultVector; resultVector.x=q.w * M.x + q.x * M.w + q.y * M.z - q.z * M.y; resultVector.y=q.w * M.y - q.x * M.z + q.y * M.w + q.z * M.x; resultVector.z=q.w * M.z + q.x * M.y - q.y * M.x + q.z * M.w; return resultVector; } 

Акт 2. Нахождение вершин куба


Зная как вращать вектор кватернионом, не составит труда найти все вершины куба.

Перейдем к функцию нахождении вершин куба. Определим базовые переменные.

private static Vector3[] GetPoint(Box box) {//Тут будут храниться координаты вершин Vector3[] point=new Vector3[8];//получаем координаты//.... return point; } 

Далее необходимо найти такую точку(опорную точку), от которой будет легче всего найти другие вершины.

Из центра покоординатно вычитаем половину размерности куба.Потом к опорной точке прибавляем по одной размерности куба.

//...//первые четыре вершины point[0]=box.Center - box.Size/2; point[1]=point[0] + new Vector3(box.Size.x, 0, 0); point[2]=point[0] + new Vector3(0, box.Size.y, 0); point[3]=point[0] + new Vector3(0, 0, box.Size.z);//таким же образом находим оставшееся точки point[4]=box.Center + box.Size/2; point[5]=point[4] - new Vector3(box.Size.x, 0, 0); point[6]=point[4] - new Vector3(0, box.Size.y, 0); point[7]=point[4] - new Vector3(0, 0, box.Size.z);//... 

ITKarma picture
Можем видеть, как сформированы точки

После нахождения координат вершин, необходимо повернуть каждый вектор на соответствующий кватернион.

//... for (int i=0; i < 8; i++) { point[i] -= box.Center;//перенос центра в начало координат point[i]=QuanRotation(point[i], box.Quaternion);//поворот point[i] += box.Center;//обратный перенос }//... 

полный код получения вершин
private static Vector3[] GetPoint(Box box) { Vector3[] point=new Vector3[8];//получаем координаты вершин point[0]=box.Center - box.Size/2; point[1]=point[0] + new Vector3(box.Size.x, 0, 0); point[2]=point[0] + new Vector3(0, box.Size.y, 0); point[3]=point[0] + new Vector3(0, 0, box.Size.z);//таким же образом находим оставшееся точки point[4]=box.Center + box.Size/2; point[5]=point[4] - new Vector3(box.Size.x, 0, 0); point[6]=point[4] - new Vector3(0, box.Size.y, 0); point[7]=point[4] - new Vector3(0, 0, box.Size.z);//поворачиваем вершины кватернионом for (int i=0; i < 8; i++) { point[i] -= box.Center;//перенос центра в начало координат point[i]=QuanRotation(point[i], box.Quaternion);//поворот point[i] += box.Center;//обратный перенос } return point; } 


Перейдем к проекциям.

Акт 3. Поиск разделяющих осей


Следующим шагом необходимо найти множество осей, претендующих на разделяющую.
Вспомним, что ее можно найти в следующих множествах:

  • Нормали плоскостей каждого куба(красные)
  • Векторное произведение ребер кубов $\{[\vec{x},\vec{y}[ : x∈X, y∈Y\}$, где X — ребра первого куба (зеленые), а Y — второго (синие).

ITKarma picture

Для того, чтобы получить необходимые оси, достаточно иметь четыре вершины куба, которые формируют ортогональную систему векторов. Эти вершины находятся в первых четырех ячейках массива точек, которые мы сформировали во втором акте.

ITKarma picture

Необходимо найти нормали плоскостей, порожденные векторами:

  • $\vec{a}$и $\vec{b}$
  • $\vec{b}$и $\vec{c}$
  • $\vec{a}$и $\vec{c}$

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

такой код позволяет получить эти вектора и найти нормали к плоскостям для двух кубов(понятный вариант)
private static List<Vector3> GetAxis(Vector3[] a, Vector3[] b) {//ребра Vector3 A; Vector3 B;//потенциальные разделяющие оси List<Vector3> Axis=new List<Vector3>();//нормали плоскостей первого куба A=a[1] - a[0]; B=a[2] - a[0]; Axis.Add(Vector3.Cross(A,B).normalized); A=a[2] - a[0]; B=a[3] - a[0]; Axis.Add(Vector3.Cross(A,B).normalized); A=a[1] - a[0]; B=a[3] - a[0]; Axis.Add(Vector3.Cross(A,B).normalized);//нормали второго куба A=b[1] - b[0]; B=b[2] - b[0]; Axis.Add(Vector3.Cross(A,B).normalized); A=b[1] - b[0]; B=b[3] - b[0]; Axis.Add(Vector3.Cross(A,B).normalized); A=b[2] - b[0]; B=b[3] - b[0]; Axis.Add(Vector3.Cross(A,B).normalized);//... } 


Но можно сделать проще:
private static List<Vector3> GetAxis(Vector3[] a, Vector3[] b) {//ребра Vector3 A; Vector3 B;//потенциальные разделяющие оси List<Vector3> Axis=new List<Vector3>();//нормали плоскостей первого куба for (int i=1; i < 4; i++) { A=a[i] - a[0]; B=a[(i+1)%3+1] - a[0]; Axis.Add(Vector3.Cross(A,B).normalized); }//нормали второго куба for (int i=1; i < 4; i++) { A=b[i] - b[0]; B=b[(i+1)%3+1] - b[0]; Axis.Add(Vector3.Cross(A,B).normalized); }//... } 

Еще мы должны найти все векторные произведения ребер кубов. Это можно организовать простым перебором:

private static List<Vector3> GetAxis(Vector3[] a, Vector3[] b) {//...//получение нормалей//...//Теперь добавляем все векторные произведения for (int i=1; i < 4; i++) { A=a[i] - a[0]; for (int j=1; j < 4; j++) { B=b[j] - b[0]; if (Vector3.Cross(A,B).magnitude != 0) { Axis.Add(Vector3.Cross(A,B).normalized); } } } return Axis; } 

Полный код
private static List<Vector3> GetAxis(Vector3[] a, Vector3[] b) {//ребра Vector3 A; Vector3 B;//потенциальные разделяющие оси List<Vector3> Axis=new List<Vector3>();//нормали плоскостей первого куба for (int i=1; i < 4; i++) { A=a[i] - a[0]; B=a[(i+1)%3+1] - a[0]; Axis.Add(Vector3.Cross(A,B).normalized); }//нормали второго куба for (int i=1; i < 4; i++) { A=b[i] - b[0]; B=b[(i+1)%3+1] - b[0]; Axis.Add(Vector3.Cross(A,B).normalized); }//Теперь добавляем все векторные произведения for (int i=1; i < 4; i++) { A=a[i] - a[0]; for (int j=1; j < 4; j++) { B=b[j] - b[0]; if (Vector3.Cross(A,B).magnitude != 0) { Axis.Add(Vector3.Cross(A,B).normalized); } } } return Axis; } 


Акт 4. Проекции на оси


Мы подошли к самому главному моменту. Здесь мы должны найти проекции кубов на все потенциальные разделяющие оси. У теоремы есть одно важное следствие: если объекты пересекаются, то ось на которую величины пересечения проекции кубов минимальна — является направлением(нормалью) коллизии, а длинна отрезка пересечения — глубиной проникновения.

Но для начала напомним формулу скалярной проекции вектора v на единичный вектор a :

$proj_\left\langle \vec{a} \right\rangle \vec{v}=\frac{(\vec{v},\vec{a})}{| \vec{a} |}$



private static float ProjVector3(Vector3 v, Vector3 a) { a=a.normalized; return Vector3.Dot(v, a)/a.magnitude; } 

Теперь опишем функцию, которая будет определять пересечение проекций на оси-кандидаты.

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

private static Vector3 IntersectionOfProj(Vector3[] a, Vector3[] b, List<Vector3> Axis) { for (int j=0; j < Axis.Count; j++) {//в этом цикле проверяем каждую ось//будем определять пересечение проекций на разделяющие оси из списка кандидатов }//Если мы в цикле не нашли разделяющие оси, то кубы пересекаются, и нам нужно//определить глубину и нормаль пересечения. } 

Проекция на ось задается двумя точками, которые имеют максимальные и минимальные значения на самой оси:

ITKarma picture

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

private static void ProjAxis(out float min, out float max, Vector3[] points, Vector3 Axis) { max=ProjVector3(points[0], Axis); min=ProjVector3(points[0], Axis); for (int i=1; i < points.Length; i++) { float tmp=ProjVector3(points[i], Axis); if (tmp > max) { max=tmp; } if (tmp < min) { min= tmp; } } } 

Итого, применив данную функцию( ProjAxis ), получим проекционные точки каждого куба.

private static Vector3 IntersectionOfProj(Vector3[] a, Vector3[] b, List<Vector3> Axis) { for (int j=0; j < Axis.Count; j++) {//проекции куба a float max_a; float min_a; ProjAxis(out min_a,out max_a,a,Axis[j]);//проекции куба b float max_b; float min_b; ProjAxis(out min_b,out max_b,b,Axis[j]);//... }//... } 

Далее, на основе проекционных вершин определяем пересечение проекций:

ITKarma picture

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

float[] points={min_a, max_a, min_b, max_b}; Array.Sort(points); 

Note the following property:

1) If the segments do not intersect , then the sum of the segments will be less than the segment formed by the extreme points:

ITKarma picture

2) If the segments intersect , then the sum of the segments will be greater than the segment formed by the extreme points:

ITKarma picture

With this simple condition, we checked the intersection and non-intersection of the segments.

If there is no intersection, then the depth of intersection will be zero:

//...//Сумма отрезков float sum=(max_b - min_b) + (max_a - min_a);//Длина крайних точек float len=Math.Abs(p[3] - p[0]); if (sum <= len) {//разделяющая ось существует//объекты непересекаются return Vector3.zero; }//Предполагаем, что кубы пересекаются и рассматриваем текущую ось далее//.... 

Thus, it is enough for us to have at least one vector on which the projections of the cubes do not intersect, then the cubes themselves do not intersect. Therefore, when we find the dividing axis, we can not check the remaining vectors, and complete the algorithm.

In the case of intersection of cubes, everything is a little more interesting: the projection of cubes on all of the vector will intersect, and we must define a vector with minimal intersection.

We create this vector before the loop, and we will store the vector with the minimum length in it. Thus, at the end of the loop we get the desired vector.

private static Vector3 IntersectionOfProj(Vector3[] a, Vector3[] b, List<Vector3> Axis) { Vector3 norm=new Vector3(10000,10000,10000); for (int j=0; j < Axis.Count; j++) {//... }//в случае, когда нашелся вектор с минимальным пересечением, возвращаем его return norm; { 

And every time we find the axis on which the projections intersect, we check whether it is the smallest in length among all. multiply such an axis by the length of the intersection, and the result will be the desired normal (direction) of the intersection of the cubes.

I also added a definition of the normal orientation with respect to the first cube.

//... if (sum <= len) {//разделяющая ось существует//объекты не пересекаются return new Vector3(0,0,0); }//Предполагаем, что кубы пересекаются и рассматриваем текущий вектор далее//пересечение проекций - это расстояние между 2 и 1 элементом в нашем массиве//(см. рисунок на котором изображен случай пересечения отрезков) float dl=Math.Abs(points[2] - points[1]); if (dl < norm.magnitude) { norm=Axis[j] * dl;//ориентация нормали if(points[0] != min_a) norm=-norm; }//... 

All code
private static Vector3 IntersectionOfProj(Vector3[] a, Vector3[] b, List<Vector3> Axis) { Vector3 norm=new Vector3(10000,10000,10000); for (int j=0; j < Axis.Count; j++) {//проекции куба a float max_a; float min_a; ProjAxis(out min_a,out max_a,a,Axis[j]);//проекции куба b float max_b; float min_b; ProjAxis(out min_b,out max_b,b,Axis[j]); float[] points={min_a, max_a, min_b, max_b}; Array.Sort(points); float sum=(max_b - min_b) + (max_a - min_a); float len=Math.Abs(points[3] - points[0]); if (sum <= len) {//разделяющая ось существует//объекты не пересекаются return new Vector3(0,0,0); } float dl=Math.Abs(points[2] - points[1]); if (dl < norm.magnitude) { norm=Axis[j] * dl;//ориентация нормы if(points[0] != min_a) norm=-norm; } } return norm; } 


Conclusion


The project with the implementation and the example is uploaded to GitHub, and you can find it here .

My goal was to share my experience in solving problems associated with determining the intersection of two convex objects. And it is also accessible and clear to talk about this theorem.

Source