GJK 3d

There are numerous resources regarding GJK on the internet. This site is mostly for the ones interested in programming their own algorithm rather than taking a finished product.

What is GJK?
With the GJK (Gilbert–Johnson–Keerthi) algorithm, one can determine whether two convex shapes are intersecting or not. The algorith itself can be used in 2d or 3d.

Some useful information can be found at:

http://mollyrocket.com/849 - great overview of the functionality of the algorithm
http://dyn4j.org/2010/04/gjk-gilbert-johnson-keerthi - 2d implementation of GJK
http://in2gpu.com/2014/05/18/gjk-algorithm-3d/ - tetrahedral case

I will not go into great detail since this is mainly covered in the video which can be found at mollyrocket (first link in the list above). I will focus on my implementation and emphasize on the difficulties i encountered (esp. the tetrahedral case).

Disclaimer: I am not a professional programmer. My implementation may not be the fastest or most beautiful. The path of improvement is long and wide. All the info provided on this site is not meant to be a 'zero to GJK'. For a general point of entry i recommend the links above (especially the first one)!

Header - files:

First we define the two classes (shape & simplex) for the headerfile gjk.h:

shape:

001
002
003
004
005
006
007
008
009
010
011
012
013
014
015
016

class shape
{
   public:
      float x;
      float y;
      float z;

      shape negate(); // x -> -x ; y -> -y ; z -> -z
      float dot(shape d); // dot product
      shape cross(shape cpt); // cross product of two vectors(shapes)
      void set(shape set); // set a vector(shape) to a given x/y/z value(s)

      shape rot_x(float rot_x); // rotates a shape around the x-axis
      shape rot_y(float rot_y); // rotates a shape around the y-axis
      shape rot_z(float rot_z); // rotates a shape around the z-axis
};

Mark Code

A shape consists of three values x/y/z. Later we will create arrays of shape-class objects. When talking of 'shape(s)' i will either refer to a point defined by three cartesian coordinates (x/y/z) or a quantity of points. The context should make it clear, which one im talking about.

The (header)functions defined obove are:

negate() - change the algebraic sign of the elements x/y/z
dot(shape d) - dot product of two shapes
cross(shape cpt) - cross product of two shapes
set(shape set) - change the values of a shape
rot_x/y/z(float rot_x/y/z) - rotate a shape (mainly for visualization)

simplex:

001
002
003
004
005
006
007
008
009
010
011
012
013
014
015

class simplex
{
   public:
      void add(shape simplex_add); // add a new point to the simplex (list)
      void set_zero(void); // (re)sets all points in the simplex-list to zero
      void del(int id);    // delete vector from the simplex with given id (id = 0, 1, 2)
      shape getLast(void); // returns the last added point
      shape get(int id); // returns the x/y/z - values of a given index of the simples with id (= 0 ... 3)
      int size(void); // returns the amount of points in the simplex list (0 ... 4)

   private:
      float x[4];
      float y[4];
      float z[4];
};

Mark Code

The class simplex and its functions are used to store the points of the simplex and manipulate the list. There can be a total maximum of four points in the simplex.

GJK function(s):

001

bool gjk(shape *A, shape *B, int dim_a, int dim_b);

Mark Code

This will be our main GJK-function into which we will feed the arrays of x/y/z-shapes into. dim_a and dim_b are the number of shapes (each one defined by a x/y/z-value) we feed into the function. It will return true when there is an intersection between the two convex shapes and false if there is no intersection. Further down is an example on how to call the function in detail.

The functions of the class shape are:

001
002
003
004
005
006

shape shape::negate()
{
   x = -x;
   y = -y;
   z = -z;
}

Mark Code

Negates a shape.

001
002
003
004
005

float shape::dot(shape d)
{
float dotproduct = x*d.x + y*d.y + z*d.z;
return dotproduct;
}

Mark Code

Dot product between two shapes.

001
002
003
004
005
006
007
008
009
010

shape shape::cross(shape d)
{
   shape crossproduct;

   crossproduct.x = y*d.z - z*d.y;
   crossproduct.y = z*d.x-x*d.z;
   crossproduct.z = x*d.y - y*d.x;

   return crossproduct;
}

Mark Code

Cross product between two shapes.

001
002
003
004
005
006

void shape::set(shape d)
{
   x = d.x;
   y = d.y;
   z = d.z;
}

Mark Code

Manipulate the x/y/z-entries of a shape.

001
002
003
004
005
006
007
008
009
010

shape shape::rot_x(float rot_x)
{
   shape return_shape;

   return_shape.x = x;
   return_shape.y = y * cos(rot_x * PI / 180.0) - z * sin(rot_x * PI / 180.0);
   return_shape.z = y * sin(rot_x * PI / 180.0) + z * cos(rot_x * PI / 180.0);

   return return_shape;
}

Mark Code

Rotates a shape. rot_x is the angle of rotation.

001
002
003
004
005
006
007
008
009
010
011
012
013
014
015
016
017
018
019
020

void simplex::add(shape simplex_add)
{
   int i = 3;

   while (i >= 0)
   {
      if (x[i] != -INFINITY)
      {
         break;
      }

      i--;
   }

   i++;

   x[i] = simplex_add.x;
   y[i] = simplex_add.y;
   z[i] = simplex_add.z;
}

Mark Code

Adds a new point to the end of the simples list.

001
002
003
004
005
006
007
008
009

void simplex::set_zero(void)
{
 for(int i = 0 ; i < 4 ; i++)
 {
 x[i] = -INFINITY;
 y[i] = -INFINITY;
 z[i] = -INFINITY;
 }
}

Mark Code

Initializes the simplex (set all values to -INFINITY).

001
002
003
004
005
006
007
008
009
010
011
012
013
014
015
016
017
018
019
020
021
022
023
024
025
026
027
028
029
030
031
032
033
034
035
036
037
038
039
040
041

void simplex::del(int id)
{
 int simplex_elements = 0;

 for(int i = 0; i < 4 ; i++)
 if (x[i] != -INFINITY)
 simplex_elements ++;

 if (simplex_elements < 4 or id == 4)
 {
 std::cout << "error -> simplex elements: " << simplex_elements << " | id to remove: " << id << std::endl;
 }
 else
 {
 float cx[2], cy[2], cz[2];

 int c = 0;
 id--;

 for (int i = 0 ; i < 4 ; i++)
 {
 cx[c] = x[i];
 cy[c] = y[i];
 cz[c] = z[i];

 x[i] = -INFINITY;
 y[i] = -INFINITY;
 z[i] = -INFINITY;

 if (i != id)
 c++;
 }

 for (int i = 0 ; i < 3 ; i++)
 {
 x[i] = cx[i];
 y[i] = cy[i];
 z[i] = cz[i];
 }
 }
}

Mark Code

Deletes an entry from the simplex list. This function takes an id (1 ... 4) and removes this entry from the simplex. Then the list is then being collapsed, so that if we add a new entry to the list, this newly added point will always be at the end.

001
002
003
004
005
006
007
008
009
010
011
012
013
014
015
016
017
018
019
020
021
022

shape simplex::getLast(void)
{
   shape return_last;

   int i = 3;

   while (i >= 0)
   {
      if (x[i] != -INFINITY)
      {
         break;
      }

      i--;
   }

   return_last.x = x[i];
   return_last.y = y[i];
   return_last.z = z[i];

   return return_last;
}

Mark Code

Returns the last point which was added to the simplex.

001
002
003
004
005
006
007
008
009
010
011
012
013
014
015
016
017
018

shape simplex::get(int id)
{
 shape return_shape;

 if (id < 0 or id > 3)
 {
 std::cout << "cannot fetch simplex point with given index " << id << std::endl;
 }
 else
 {
 return_shape.x = x[id];
 return_shape.y = y[id];
 return_shape.z = z[id];

 }

 return return_shape;
}

Mark Code

Fetch a point from the simplex list.

001
002
003
004
005
006
007
008
009
010

int simplex::size(void)
{
 int found = 0;

 for(int i = 0; i < 4 ; i++)
 if (x[i] != -INFINITY)
 found ++;

 return found;
}

Mark Code

Returns the size, i.e., the elements containing the simplex.

GJK()

bool gjk(shape *A, shape *B, int dim_a, int dim_b)
{
 bool check_failsafe = true;
 bool check = true;

 int i = 0;
 simplex simplex;

 while (check_failsafe == true)
 {
 shape mma = mass_middle_point(A, dim_a);
 shape mmb = mass_middle_point(B, dim_b);

 shape d; // vector pointing from geom. middle point of shape A to geom. middle point of shape B

 d.x = mma.x-mmb.x;
 d.y = mma.y-mmb.y;
 d.z = mma.z-mmb.z;

 if (i > 10)
 {
 i = 0;

 check_failsafe = true;
 check = true;

 int r1 = 0;
 int r2 = 0;

 int r3 = 0;
 int r4 = 0;

 int r5 = 0;
 int r6 = 0;

 while (r1 == 0 or r2 == 0 or r3 == 0 or r4 == 0 or r5 == 0 or r6 == 0)
 {
 r1 = rand() % 19 + (-9);
 r2 = rand() % 19 + (-9);

 r3 = rand() % 19 + (-9);
 r4 = rand() % 19 + (-9);

 r5 = rand() % 19 + (-9);
 r6 = rand() % 19 + (-9);
 }

 float s1 = (float)r1/(float)r2;
 float s2 = (float)r3/(float)r4;
 float s3 = (float)r5/(float)r6;

 // random seeded (new search direction)
 d.x = s1;
 d.y = s2;
 d.z = s3;
 // random seeded (new search direction)
 }

 simplex.set_zero(); // clear simplex
 simplex.add(support_function(A, B, dim_a, dim_b, d));
 d.negate();

 while (check == true)
 {
 simplex.add(support_function(A, B, dim_a, dim_b, d));

 if(simplex.size() == 4)
 i++;

 if (simplex.getLast().dot(d) < 0) // no collision -> break loop
 {
 check_failsafe = false;
 check = false; // break the while loop
 }
 else // there may be a collision -> continue
 {
 if (containsOrigin(simplex, d))
 {
 check = true;
 check_failsafe = false; // collision
 break;
 }
 else if (i > 10)
 {
 check = false;
 }
 }
 }
 }

 return check;
}

bool gjk(shape *A, shape *B, int dim_a, int dim_b) 
{ 
	bool check_failsafe = true; 
	bool check = true; 
 
	int i = 0; 
	simplex simplex; 
 
	while (check_failsafe == true) 
	{ 
		shape mma = mass_middle_point(A, dim_a); 
		shape mmb = mass_middle_point(B, dim_b); 
 
		shape d; // vector pointing from geom. middle point of shape A to geom. middle point of shape B 
 
		d.x = mma.x-mmb.x; 
		d.y = mma.y-mmb.y; 
		d.z = mma.z-mmb.z; 
 
		if (i > 10) 
		{ 
			i = 0; 
 
			check_failsafe = true; 
			check = true; 
 
			int r1 = 0; 
			int r2 = 0; 
 
			int r3 = 0; 
			int r4 = 0; 
 
			int r5 = 0; 
			int r6 = 0; 
 
			while (r1 == 0 or r2 == 0 or r3 == 0 or r4 == 0 or r5 == 0 or r6 == 0) 
			{ 
				r1 = rand() % 19 + (-9); 
				r2 = rand() % 19 + (-9); 
 
				r3 = rand() % 19 + (-9); 
				r4 = rand() % 19 + (-9); 
 
				r5 = rand() % 19 + (-9); 
				r6 = rand() % 19 + (-9); 
			} 
 
			float s1 = (float)r1/(float)r2; 
			float s2 = (float)r3/(float)r4; 
			float s3 = (float)r5/(float)r6; 
 
			// random seeded (new search direction) 
			d.x = s1; 
			d.y = s2; 
			d.z = s3; 
			// random seeded (new search direction) 
		} 
 
		simplex.set_zero(); // clear simplex 
		simplex.add(support_function(A, B, dim_a, dim_b, d)); 
		d.negate(); 
 
		while (check == true) 
		{ 
			simplex.add(support_function(A, B, dim_a, dim_b, d)); 
 
			if(simplex.size() == 4) 
				i++; 
 
			if (simplex.getLast().dot(d) &amp;lt; 0) // no collision -> break loop 
			{ 
				check_failsafe = false; 
				check = false; // break the while loop 
			} 
			else // there may be a collision -> continue 
			{ 
				if (containsOrigin(simplex, d)) 
				{ 
					check = true; 
					check_failsafe = false; // collision 
					break; 
				} 
				else if (i > 10) 
				{ 
					check = false; 
				} 
			} 
		} 
	} 
 
	return check; 
}

Mark Code

To call the function we define two shapes and calculate the amount of points in each array:

001
002
003
004
005
006
007

shape ts1[] = {t2.x, t2.y, t2.z, t1.x, t1.y, t1.z, t3.x, t3.y, t3.z};
shape cs1_1[] = {size, size, size, size, size, -size, size, -size, -size, size, -size, size };

int dim_ts = sizeof(ts1)/sizeof(*ts1);
int dim_cs1_1 = sizeof(cs1_1)/sizeof(*cs1_1);

bool check1 = gjk(ts1, cs1_1, dim_ts, dim_cs1_1);

Mark Code

The call of the function gjk() is here illustrated with a triangle (ts1[] - one side of a tetrahedron) and a square (cs1_1[] - one side of a cube). These two polygons are being tested on collision.

GJK - main function:

We define two variables of type bool (check_failsafe and check). The bool check will indicate if there is a collision or not (which will be returned at the end). The bool check_failsafe is being triggered, when the algorithm gets stuck and subsequently adds and removes the same points. This will result in an infinite loop. Usually one or two tetrahedral cases are enough to determine if there is a collision or not. If the algorithm goes through the tetrahedral case 10 times, a new search direction is chosen randomly. The number 10 is chosen completely arbitrary.

In the first while loop, the geometrical middle points of the two shapes are calculated using the function mass_middle_point(). Then the direction between these two points is calculated.

We clear the simplex (simplex.set_zero()) and add the first point to the list (simplex.add(support_function(A, B, dim_a, dim_b, d));). support_function(A, B, dim_a, dim_b, d) determines the point farthest away in the Minkowski difference along a given direction d. After that we negate the search direction and start the second while loop. Here we start with adding a new point to the simplex. Then it is checked, if the size of the simplex is four (tetrahedral case). If that is true, we increment the variable for the fail safe.

If the last added point to the simplex is not exceeding the point of origin, then there is no collision and we can end here. We set check and check_failsafe to false and break both while loops: There is no collision!

If the last added point in the simplex (towards the search direction) list is exceeding the point of origin, we don't know yet if there is a collision between the two convex shapes. The function containsOrigin(simplex, d) is called.

Based on how many points there are in the simplex list the function containsOrigin(simplex, d) does the following:

line-case: 2 points in the simplex list: determine the new search direction towards the point of origin
triangle-case: 3 points in the simplex list: similar to the case above (determine a new search direction towards the point of origin)
tetrahedral case: 4 points in the simplex list: determine, which face of the tetrahedron is closest to the origin. Delete the point of the simplex, which is not element of this side and feed this triangle back into the loop.

Each time after the function containsOrigin(simplex, d) is called, a new point is added to the simplex list according to the determined search direction and then it is checked, whether this point exceeds the origin.

The function containsOrigin(simplex, d) also checks, if the point of origin is contained in the tetrahedron (more further down).

The support function:

001
002
003
004
005
006
007
008
009
010
011
012
013
014
015
016
017
018
019
020
021
022
023
024
025
026
027
028
029
030
031
032
033
034
035
036
037
038
039
040
041
042
043
044

shape support_function(shape *A, shape *B, int dim_a, int dim_b, shape sd)
{
 // shape A -> direction = sd
 float max_val1 = -INFINITY;
 int max_index1 = -1;

 float dotp = 0.0;

 for (int i = 0 ; i < dim_a ; i++)
 {
 dotp = dot_prod(A[i].x, A[i].y, A[i].z, sd.x, sd.y, sd.z);

 if (dotp > max_val1)
 {
 max_val1 = dotp;
 max_index1 = i;
 }
 }
 // shape A -> direction = sd

 // shape B -> direction = -sd
 float max_val2 = -INFINITY;
 int max_index2 = -1;

 for (int j = 0 ; j < dim_b ; j++)
 {
 dotp = dot_prod(B[j].x, B[j].y, B[j].z, -sd.x, -sd.y, -sd.z);

 if (dotp > max_val2)
 {
 max_val2 = dotp;
 max_index2 = j;
 }
 }
 // shape B -> direction = -sd

 shape return_vec;

 return_vec.x = A[max_index1].x - B[max_index2].x;
 return_vec.y = A[max_index1].y - B[max_index2].y;
 return_vec.z = A[max_index1].z - B[max_index2].z;

 return return_vec;
}

shape support_function(shape *A, shape *B, int dim_a, int dim_b, shape sd) 
{ 
	// shape A -> direction = sd 
	float max_val1 = -INFINITY; 
	int max_index1 = -1; 
 
	float dotp = 0.0; 
 
	for (int i = 0 ; i &amp;lt; dim_a ; i++) 
	{ 
		dotp = dot_prod(A[i].x, A[i].y, A[i].z, sd.x, sd.y, sd.z); 
 
		if (dotp > max_val1) 
		{ 
			max_val1 = dotp; 
			max_index1 = i; 
		} 
	} 
	// shape A -> direction = sd 
 
	// shape B -> direction = -sd 
	float max_val2 = -INFINITY; 
	int max_index2 = -1; 
 
	for (int j = 0 ; j &amp;lt; dim_b ; j++) 
	{ 
		dotp = dot_prod(B[j].x, B[j].y, B[j].z, -sd.x, -sd.y, -sd.z); 
 
		if (dotp > max_val2) 
		{ 
			max_val2 = dotp; 
			max_index2 = j; 
		} 
	} 
	// shape B -> direction = -sd 
 
	shape return_vec; 
 
	return_vec.x = A[max_index1].x - B[max_index2].x; 
	return_vec.y = A[max_index1].y - B[max_index2].y; 
	return_vec.z = A[max_index1].z - B[max_index2].z; 
 
	return return_vec; 
}

Mark Code

This function (support_function()) takes the two shapes and a search direction and computes the point in the Minkowski difference, which is farthest away in this given direction.

The function containsOrigin(simplex& simplex, shape& d):

001
002
003
004
005
006
007
008
009
010
011
012
013
014
015
016
017
018
019
020
021
022
023
024
025
026
027
028
029
030
031
032
033
034
035
036
037
038
039
040
041
042
043
044
045
046
047
048
049
050
051
052
053
054
055
056
057
058
059
060
061
062
063
064
065
066
067
068
069
070
071
072
073
074
075
076
077
078
079
080
081
082
083
084
085
086
087
088
089
090
091
092
093
094
095
096
097
098
099
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224

bool containsOrigin(simplex& simplex, shape& d)
{
 shape a = simplex.getLast(); // fetch the point which was last added to the simplex

 shape a0 = a;
 a0.negate();

 if (simplex.size() == 4) // 4 elements in the simplex -> tetrahedral case
 {
 shape p_d = simplex.get(0);
 shape p_c = simplex.get(1);
 shape p_b = simplex.get(2);

 // AC
 shape ac;
 ac.x = p_c.x - a.x;
 ac.y = p_c.y - a.y;
 ac.z = p_c.z - a.z;
 // AC

 // AB
 shape ab;
 ab.x = p_b.x - a.x;
 ab.y = p_b.y - a.y;
 ab.z = p_b.z - a.z;
 // AB

 // AD
 shape ad;
 ad.x = p_d.x - a.x;
 ad.y = p_d.y - a.y;
 ad.z = p_d.z - a.z;
 // AD

 shape ac_c_ab = ac.cross(ab); // ac_c_ab = AC x AB
 float v_abc = ac_c_ab.dot(a0);

 shape ab_c_ad = ab.cross(ad); // ab_c_ad = AB x AD
 float v_abd = ab_c_ad.dot(a0);

 shape ad_c_ac = ad.cross(ac); // ad_c_ac = AD x AC
 float v_acd = ad_c_ac.dot(a0);

 int amount_neg = 0;
 int amount_pos = 0;

 if (v_acd > 0)
 amount_pos++;
 else
 amount_neg++;

 if (v_abd > 0)
 amount_pos++;
 else
 amount_neg++;

 if (v_abc > 0)
 amount_pos++;
 else
 amount_neg++;

 if (amount_pos == 3) // origin enclosed in the tetrahedron -> there is a collision
 {
 return true;
 }
 else if (amount_neg == 3) // origin enclosed in the tetrahedron -> there is a collision
 {
 return true;
 }
 else // remove this point
 {
 if (amount_neg == 2 and amount_pos == 1)
 {
 if (v_acd > 0)
 {
 simplex.del(3); // remove this point
 d.set(ad_c_ac); // set new search direction
 }
 else if (v_abd > 0)
 {
 simplex.del(2); // remove this point
 d.set(ab_c_ad); // set new search direction
 }
 else
 {
 simplex.del(1); // remove this point
 d.set(ac_c_ab); // set new search direction
 }
 }
 else if (amount_neg == 1 and amount_pos == 2)
 {
 if (v_acd < 0)
 {
 ad_c_ac.negate();

 simplex.del(3); // remove this point
 d.set(ad_c_ac); // set new search direction
 }
 else if (v_abd < 0)
 {
 ab_c_ad.negate();

 simplex.del(2); // remove this point
 d.set(ab_c_ad); // set new search direction
 }
 else
 {
 ac_c_ab.negate();

 simplex.del(1); // remove this point
 d.set(ac_c_ab); // set new search direction
 }
 }
 else
 {
 std::cout << "error(number pos/neg)" << std::endl;
 }
 }
 }
 else if (simplex.size() == 3) // triangle case
 {
 shape return_sd;
 return_sd.x = 0;
 return_sd.y = 0;
 return_sd.z = 0;

 shape b = simplex.get(1);
 shape c = simplex.get(0);

 shape ab; // b - a
 ab.x = b.x - a.x;
 ab.y = b.y - a.y;
 ab.z = b.z - a.z;

 shape ac; // c - a
 ac.x = c.x - a.x;
 ac.y = c.y - a.y;
 ac.z = c.z - a.z;

 shape abc = ab.cross(ac); // ABC = AB x AC

 shape x; x.x = 1; x.y = 0; x.z = 0;
 shape y; y.x = 0; y.y = 1; y.z = 0;
 shape z = x.cross(y);

 shape abc_c_ac = abc.cross(ac); // ABC x AC
 shape ab_c_abc = ab.cross(abc); // AB x ABC

 if (abc_c_ac.dot(a0) > 0)
 {
 if (ac.dot(a0) > 0)
 {
 return_sd = triple_cross_prod(ac, a0, ac);
 }
 else
 {
 if (ab.dot(a0) > 0)
 {
 return_sd = triple_cross_prod(ab, a0, ab);
 }
 else
 {
 return_sd = a0;
 }
 }
 }
 else
 {
 if (ab_c_abc.dot(a0) > 0)
 {
 if (ab.dot(a0) > 0)
 {
 return_sd = triple_cross_prod(ab, a0, ab);
 }
 else
 {
 return_sd = a0;
 }
 }
 else
 {
 if (abc.dot(a0) > 0)
 {
 return_sd = abc;
 }
 else
 {
 return_sd.x = -abc.x;
 return_sd.y = -abc.y;
 return_sd.z = -abc.z;
 }
 }
 }

 d.set(return_sd);
 return false;
 }
 else if (simplex.size() == 2) // line case
 {
 shape b = simplex.get(0);

 shape ab; // b - a
 ab.x = b.x - a.x;
 ab.y = b.y - a.y;
 ab.z = b.z - a.z;

 shape triple_cp;

 if (ab.dot(a0) > 0)
 {
 triple_cp = triple_cross_prod(ab, a0, ab);
 }
 else
 {
 triple_cp.x = a0.x;
 triple_cp.y = a0.y;
 triple_cp.z = a0.z;
 }

 d.set(triple_cp);
 }

 return false;
}

bool containsOrigin(simplex& simplex, shape& d) 
{ 
	shape a = simplex.getLast(); // fetch the point which was last added to the simplex 
 
	shape a0 = a; 
	a0.negate(); 
 
	if (simplex.size() == 4) // 4 elements in the simplex -> tetrahedral case 
	{ 
		shape p_d = simplex.get(0); 
		shape p_c = simplex.get(1); 
		shape p_b = simplex.get(2); 
 
		// AC 
		shape ac; 
		ac.x = p_c.x - a.x; 
		ac.y = p_c.y - a.y; 
		ac.z = p_c.z - a.z; 
		// AC 
 
		// AB 
		shape ab; 
		ab.x = p_b.x - a.x; 
		ab.y = p_b.y - a.y; 
		ab.z = p_b.z - a.z; 
		// AB 
 
		// AD 
		shape ad; 
		ad.x = p_d.x - a.x; 
		ad.y = p_d.y - a.y; 
		ad.z = p_d.z - a.z; 
		// AD 
 
		shape ac_c_ab = ac.cross(ab); // ac_c_ab = AC x AB 
		float v_abc = ac_c_ab.dot(a0); 
 
		shape ab_c_ad = ab.cross(ad); // ab_c_ad = AB x AD 
		float v_abd = ab_c_ad.dot(a0); 
 
		shape ad_c_ac = ad.cross(ac); // ad_c_ac = AD x AC 
		float v_acd = ad_c_ac.dot(a0); 
 
		int amount_neg = 0; 
		int amount_pos = 0; 
 
		if (v_acd > 0) 
			amount_pos++; 
		else 
			amount_neg++; 
 
		if (v_abd > 0) 
			amount_pos++; 
		else 
			amount_neg++; 
 
		if (v_abc > 0) 
			amount_pos++; 
		else 
			amount_neg++; 
 
		if (amount_pos == 3) // origin enclosed in the tetrahedron -> there is a collision 
		{ 
			return true; 
		} 
		else if (amount_neg == 3) // origin enclosed in the tetrahedron -> there is a collision 
		{ 
			return true; 
		} 
		else // remove this point 
		{ 
			if (amount_neg == 2 and amount_pos == 1) 
			{ 
				if (v_acd > 0) 
				{ 
					simplex.del(3); // remove this point 
					d.set(ad_c_ac); // set new search direction 
				} 
				else if (v_abd > 0) 
				{ 
					simplex.del(2); // remove this point 
					d.set(ab_c_ad); // set new search direction 
				} 
				else 
				{ 
					simplex.del(1); // remove this point 
					d.set(ac_c_ab); // set new search direction 
				} 
			} 
			else if (amount_neg == 1 and amount_pos == 2) 
			{ 
				if (v_acd &amp;lt; 0) 
				{ 
					ad_c_ac.negate(); 
 
					simplex.del(3); // remove this point 
					d.set(ad_c_ac); // set new search direction 
				} 
				else if (v_abd &amp;lt; 0) 
				{ 
					ab_c_ad.negate(); 
 
					simplex.del(2); // remove this point 
					d.set(ab_c_ad); // set new search direction 
				} 
				else 
				{ 
					ac_c_ab.negate(); 
 
					simplex.del(1); // remove this point 
					d.set(ac_c_ab); // set new search direction 
				} 
			} 
			else 
			{ 
				std::cout &amp;lt;&amp;lt; "error(number pos/neg)" &amp;lt;&amp;lt; std::endl; 
			} 
		} 
	} 
	else if (simplex.size() == 3) // triangle case 
	{ 
		shape return_sd; 
		return_sd.x = 0; 
		return_sd.y = 0; 
		return_sd.z = 0; 
 
		shape b = simplex.get(1); 
		shape c = simplex.get(0); 
 
		shape ab; // b - a 
		ab.x = b.x - a.x; 
		ab.y = b.y - a.y; 
		ab.z = b.z - a.z; 
 
		shape ac; // c - a 
		ac.x = c.x - a.x; 
		ac.y = c.y - a.y; 
		ac.z = c.z - a.z; 
 
		shape abc = ab.cross(ac); // ABC = AB x AC 
 
		shape x; x.x = 1; x.y = 0; x.z = 0; 
		shape y; y.x = 0; y.y = 1; y.z = 0; 
		shape z = x.cross(y); 
 
		shape abc_c_ac = abc.cross(ac); // ABC x AC 
		shape ab_c_abc = ab.cross(abc); // AB x ABC 
 
		if (abc_c_ac.dot(a0) > 0) 
		{ 
			if (ac.dot(a0) > 0) 
			{ 
				return_sd = triple_cross_prod(ac, a0, ac); 
			} 
			else 
			{ 
				if (ab.dot(a0) > 0) 
				{ 
					return_sd = triple_cross_prod(ab, a0, ab); 
				} 
				else 
				{ 
					return_sd = a0; 
				} 
			} 
		} 
		else 
		{ 
			if (ab_c_abc.dot(a0) > 0) 
			{ 
				if (ab.dot(a0) > 0) 
				{ 
					return_sd = triple_cross_prod(ab, a0, ab); 
				} 
				else 
				{ 
					return_sd = a0; 
				} 
			} 
			else 
			{ 
				if (abc.dot(a0) > 0) 
				{ 
					return_sd = abc; 
				} 
				else 
				{ 
					return_sd.x = -abc.x; 
					return_sd.y = -abc.y; 
					return_sd.z = -abc.z; 
				} 
			} 
		} 
 
		d.set(return_sd); 
		return false; 
	} 
	else if (simplex.size() == 2) // line case 
	{ 
		shape b = simplex.get(0); 
 
		shape ab; // b - a 
		ab.x = b.x - a.x; 
		ab.y = b.y - a.y; 
		ab.z = b.z - a.z; 
 
		shape triple_cp; 
 
		if (ab.dot(a0) > 0) 
		{ 
			triple_cp = triple_cross_prod(ab, a0, ab); 
		} 
		else 
		{ 
			triple_cp.x = a0.x; 
			triple_cp.y = a0.y; 
			triple_cp.z = a0.z; 
		} 
 
		d.set(triple_cp); 
	} 
 
	return false; 
}

Mark Code

The last point in the simplex is always taken as a point of reference. Then, based on how many points are in the simplex list, a new search direction is being determined and if there are four points in the simplex list, one is being removed.

I will not explain the cases where two and three points are in the simplex list since these cases are well described in the video which can be found at http://mollyrocket.com/849.

The tetrahedral case

The tetrahedral case however is not covered in the video. We know which three sides we have to test of the tetrahedron, because we evolved a triangle into a tetrahedron. The search direction from the triangle to the tetrahedron was towards the origin. Therefore the origin can not be closest to the original triangle. First we extract the three points and compute the face normal vectors of the three triangle sides we need to check. Then we calculate the dor product of these normal vectors with a0.

We count the amount of positive and negative dot products. If all are negative or all are positive, we can conclude that the point of origin is inside the tetrahedron. If not, we remove the according point and set a new search direction.

Picture of the main cases:

The picture above shows the case, where the algorith stops after the line-case. Red and green are the two shapes, which are being tested on collision and white their geometrical middle points. The yellow dots are the points of the Minkowski difference. First the red point in the Minkowski difference is added to the simplex and then the green one. After that, the case that there is no collision is being determined (line-case). The big green square at the bottom right is the indicator for no collision (green).

Above the tetrahedral case: A third (blue) and fourth (pink) point is added to the simplex list, which spans now a tetrahedron. For each of the three triangles the geometric middle point and the surface normal is drawn. The point of origin lies inside of the tetrahedron and there is a collision.

Like above: collision with alle surface normals pointing to the outside of the tetrahedron.

Above the tetrahedral case where a collision is uncertain and the new search direction (red) is determined by negating the surface normal (turquoise).

GJK - tetrahedral case (3 iterations, no collision)

Above the tetrahedral case with three iterations (three pink points added, and 3 red search direction). One can see clearly, that the tetrahedrons are propagating towards the origin.

A video of the last case can be found at:
https://www.youtube.com/watch?v=Lg2wR12WQ9M

Using GJK to calculate the collisions between two tetrahedra bouncing inside a cube:
https://www.youtube.com/watch?v=dQZldWqHqlY

Source code files:
https://github.com/clauskovacs/GJK-collision-detection-3d