GJK 3d

Im Internet gibt es zahlreiche Infos über den GJK-Algorithmus. Diese Seite hier ist hauptsächlich für diejenigen gedacht, die eine eigene Implementation anstreben.

Was ist GJK?
Mit dem GJK(Gilbert–Johnson–Keerthi)-Algorithmus kann u.a. bestimmt werden, ob sich zwei konvexe Polynome überlappen (kollidieren). Die Anwendung kann in zwei oder drei Dimensionen erfolgen.

Informationen über den GJK-Algorithmus:

http://mollyrocket.com/849 - allgemeiner Überblick der Funktionsweise
http://dyn4j.org/2010/04/gjk-gilbert-johnson-keerthi - 2d Implementation des GJK-Algorithmus
http://in2gpu.com/2014/05/18/gjk-algorithm-3d/ - Tetraeder Fall

Eine allgemeine Einführung (Minkowski-Differenz, Simplex ...) wird hier ausgelassen, da diese sehr genau und ausführlich im Video (erster Link in der obigen Liste) zu finden ist. Stattdessen werde ich mehr Beachtung der Implementation zuwenden (im Speziellen dem Tetraeder Fall).

Hinweis: Ich bin kein professioneller Programmierer. Meine Implementation ist nicht die schnellste und vielleicht auch nicht die schönste. Der Pfad der Optimierung ist lang und weit. Die hier bereitgestellten Informationen sind als ergänzende Informationen für Personen gedacht, die selber einen GJK-Algorithmus programmieren wollen (und schon Wissen haben). Einführendes Material ist in der Liste oben angeführt.

Header - Dateien:

Als erstes definieren wir zwei Klassen (shape & simplex) für die Headerdatei 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); // inneres Produkt
      shape cross(shape cpt); // Kreuzprodukt von zwei Vektoren(shapes)
      void set(shape set); // Ändern der x/y/z Werte einer shape

      shape rot_x(float rot_x); // rotiert einen Vektor um die x-Achse
      shape rot_y(float rot_y); // rotiert einen Vektor um die y-Achse
      shape rot_z(float rot_z); // rotiert einen Vektor um die z-Achse
};

Markieren

Als Klassenbezeichnung wurde hier das Englische Wort für Form (shape) gewählt. Jedes Klassenobjekt besteht aus drei Werten x/y/z, was einem Vektoräquivalent in drei Dimensionen entspricht.

Es kann aber genauso ein Array aus Klassenobjekten initialisiert werden.

Die (Header)Funktionen sind:

negate() - ändert das Vorzeichen eines Klassenelementes
dot(shape d) - Skalarprodukt von zwei Vektoren (shapes)
cross(shape cpt) - Kreuzprodukt
set(shape set) - ändert die x/y/z-Werte einer Klassenelementes
rot_x/y/z(float rot_x/y/z) - rotiert einen Vektor (shape) (wird hauptsächlich zur graphischen Darstellung genutzt)

simplex:

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

class simplex
{
   public:
      void add(shape simplex_add); // einen neuen Punkt in den Simplex hinzufügen
      void set_zero(void); // kompletten Simplex zurücksetzen
      void del(int id);    // Eintrag aus dem Simplex löschen mit gegebener id (id = 0, 1, 2)
      shape getLast(void); // den letzten Punkt aus dem Simplex zurückgeben
      shape get(int id); // beliebigen Eintrag aus dem Simplex holen id (= 0 ... 3)
      int size(void); // Anzahl der Elemente im Simplex ermitteln (0 ... 4)

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

Markieren

Die Klasse simplex and deren Funktionen werden genutzt um die Punkte (des simplex) zu speichern und zu manipulieren. Im simplex werden maximal vier Punkte gespeichert.

GJK Funktion(en):

001

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

Markieren

Die Funktion gjk(): Sie übernimmt zwei Arrays aus Klassenobjekten der Klasse shape (shape *A und shape *B) und die Anzahl der x/y/z-Einträge der jeweiligen Arrays. Als Rückgabewert erfolgt true wenn die Polynome kollidieren und false falls sie dies nicht tun. Weiter unten ist ein kurzes Beispiel, wie diese Funktion aufgerufen wird.

Die Funktionen der Klasse shape sind:

001
002
003
004
005
006

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

Markieren

Richtung des Vektors (shape) umkehren.

001
002
003
004
005

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

Markieren

Skalarprodukt.

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;
}

Markieren

Kreuzprodukt.

001
002
003
004
005
006

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

Markieren

x/y/z-Werte eines Objektes der Klasse shape ändern.

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;
}

Markieren

Ein Objekt der Klasse shape um den Winkel rot_x rotieren.

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;
}

Markieren

Einen neuen Punkt der simplex-Liste hinzufügen. Der Eintrag wird immer am Ende der Liste hinzugefügt.

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;
 }
}

Markieren

Die simplex-Liste initialisieren (alle Werte zu -INFINITY setzen).

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];
 }
 }
}

Markieren

Einen Eintrag aus der simplex-Liste entfernen (id = 1 ... 4). Die Liste wird dann von vorne nach hinten befüllt, sodass immer am Ende der Liste ein freier Platz ist.

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;
}

Markieren

Den letzten Punkt aus dem simplex holen.

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;
}

Markieren

Holt einen beliebigen Punkt aus der simplex-Liste

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;
}

Markieren

Ermittelt die Anzahl der Punkte im 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; // Vektor vom geometrischen Mittelpunkt (shape A -> 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;

 // zufällig ausgewählte (neue Suchrichtung)
 d.x = s1;
 d.y = s2;
 d.z = s3;
 // zufällig ausgewählte (neue Suchrichtung)
 }

 simplex.set_zero(); // simplex initialisieren
 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) // keine Kollision -> Schleife beenden
 {
 check_failsafe = false;
 check = false; // While-Schleife beenden
 }
 else // Kollision u.U. möglich
 {
 if (containsOrigin(simplex, d))
 {
 check = true;
 check_failsafe = false; // Kollision
 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; // Vektor vom geometrischen Mittelpunkt (shape A -> 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; 
 
			// zufällig ausgewählte (neue Suchrichtung) 
			d.x = s1; 
			d.y = s2; 
			d.z = s3; 
			// zufällig ausgewählte (neue Suchrichtung) 
		} 
 
		simplex.set_zero(); // simplex initialisieren 
		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) // keine Kollision -> Schleife beenden 
			{ 
				check_failsafe = false; 
				check = false; // While-Schleife beenden 
			} 
			else // Kollision u.U. möglich 
			{ 
				if (containsOrigin(simplex, d)) 
				{ 
					check = true; 
					check_failsafe = false; // Kollision 
					break; 
				} 
				else if (i > 10) 
				{ 
					check = false; 
				} 
			} 
		} 
	} 
 
	return check; 
}

Markieren

Um die Funktion aufzurufen, definieren wir zwei Array-Objekte des Typs shape und berechnen die Anzahl der x/y/z-Elemente von beiden Arrays:

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);

Markieren

Der Aufruf der Funktion gjk() ist hier an einem Beispiel illustriert: eine Seite eines Dreiecks (ts1[]) wird mit einer Seite eines Würfels (cs1_1[]) auf Kollision geprüft.

GJK - Hauptfunktion:

Zuerst werden zwei Variablen definiert vom typ bool (check und check_failsafe). Die Variable check wird auf true oder false gesetzt, je nachdem ob die Polynome kollidieren oder nicht. Diese Variable wird auch als Rückgabewert der Funktion gjk() genutzt. Die Variable check_failsafe ist eine Sicherheitsmaßnahme, damit der Algorithmus nicht in einer Endlosschleife hängen bleibt (wenn nacheinander die selben Punkte aus der Minkowski-Differenz hinzugefügt und gelöscht werden). Üblicherweise reichen zwei oder drei Tetraeder Fälle um eine Kollision (oder das ausbleiben einer) zu ermitteln. Wird der Tetraeder Fall mehr als 10 mal durchlaufen, so kommt die Variable check_failsafe zum Einsatz und eine neue zufällige Suchrichtung wird ermittelt und der Algorithmus startet mit dieser. Die Zahl 10 ist hier komplett willkürlich gewählt.

In der ersten while-Schleife wird der geometrische Mittelpunkt beider Polynome mit der Funktion mass_middle_point() ermittelt. Dann wird der Verbindungsvektor zwischen beiden Mittelpunkten berechnet.

Daraufhin wird der simplex initialisiert (mit simplex.set_zero()) und der erste Punkt wird dem simplex hinzugefügt (simplex.add(support_function(A, B, dim_a, dim_b, d));). Danach wird die Suchrichtung negiert und die äußere While-Schleife beginnt. In dieser wird am Anfang ein neuer Punkt dem simplex hinzugefügt. Sind im simplex vier Punkte (Tetraeder Fall), so wird die Variable für den failsafe erhöht.

Ist der zuletzt hinzugefügte Punkt (in Suchrichtung) nicht über dem Nullpunkt, so finded keine Kollision zwischen den Polynomen statt und wir können die while-Schleifen beenden. check und check_failsafe werden auf false gesetzt und beide while-Schleifen beendet.

Wenn der letzte Punkt aber den Nullpunkt überschreitet, so können wir noch nichts genaues über eine Kollision sagen: Die Funktion containsOrigin(simplex, d) wird aufgerufen.

Je nachdem, wie viele Punkte im simplex sind, macht die Funktion containsOrigin(simplex, d) folgendes:

Linien-Fall: 2 Punkte befinden sich im simplex: bestimme eine neue Suchrichtung in Richtung des Nullpunktes
Dreieck-Fall: 3 Punkte im simplex: bestimme, welche Seite des Tetraeders am nähesten zum Ursprung ist. Der Punkt, welcher nicht element dieses Dreiecks ist, wird aus der simplex-Liste entfernt und das Dreieck wird wieder der while-Schleife zugeführt (Dreieck Fall -> Tetrader Fall ...).

Immer nachdem die Funktion containsOrigin(simplex, d) aufgerufen wurde, wird ein neuer Punkt der simplex-Liste hinzugefügt (in Suchrichtung) und dann wird überprüft, ob dieser Punkt in Suchrichtung den Nullpunkt überschreitet.

Die Funktion containsOrigin(simplex, d) überprüft ebenso, ob der Ursprung im Tetraeder enthalten ist (weiter unten mehr).

Die Supportfunktion:

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 -> Richtung = 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 -> Richtung = sd

 // shape B -> Richtung = -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 -> Richtung = -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 -> Richtung = 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 -> Richtung = sd 
 
	// shape B -> Richtung = -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 -> Richtung = -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; 
}

Markieren

Diese Funktion (support_function()) nimmt zwei Arrays des Typs shape und eine Suchrichtung, und berechnet den Punkt in der Minkowski-Differenz, welcher in Suchrichtung am weitesten entfernt ist.

Die Funktion 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(); // den Eintrag im simplex ermitteln, welcher zuletzt hinzugefügt wurde

 shape a0 = a;
 a0.negate();

 if (simplex.size() == 4) // vier Einträge im simplex -> Tetraeder Fall
 {
 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) // Ursprung befindet sich im Tetraeder -> Kollision
 {
 return true;
 }
 else if (amount_neg == 3) // Ursprung befindet sich im Tetraeder -> Kollision
 {
 return true;
 }
 else // Punkt entfernen
 {
 if (amount_neg == 2 and amount_pos == 1)
 {
 if (v_acd > 0)
 {
 simplex.del(3); // Punkt entfernen
 d.set(ad_c_ac); // neue Suchrichtung speichern
 }
 else if (v_abd > 0)
 {
 simplex.del(2); // Punkt entfernen
 d.set(ab_c_ad); // neue Suchrichtung speichern
 }
 else
 {
 simplex.del(1); // Punkt entfernen
 d.set(ac_c_ab); // neue Suchrichtung speichern
 }
 }
 else if (amount_neg == 1 and amount_pos == 2)
 {
 if (v_acd < 0)
 {
 ad_c_ac.negate();

 simplex.del(3); // Punkt entfernen
 d.set(ad_c_ac); // neue Suchrichtung speichern
 }
 else if (v_abd < 0)
 {
 ab_c_ad.negate();

 simplex.del(2); // Punkt entfernen
 d.set(ab_c_ad); // neue Suchrichtung speichern
 }
 else
 {
 ac_c_ab.negate();

 simplex.del(1); // Punkt entfernen
 d.set(ac_c_ab); // neue Suchrichtung speichern
 }
 }
 else
 {
 std::cout << "error(number pos/neg)" << std::endl;
 }
 }
 }
 else if (simplex.size() == 3) // Dreieck (drei Punkte im simplex)
 {
 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) // Linie (zwei Punkte im simplex)
 {
 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(); // den Eintrag im simplex ermitteln, welcher zuletzt hinzugefügt wurde 
 
	shape a0 = a; 
	a0.negate(); 
 
	if (simplex.size() == 4) // vier Einträge im simplex -> Tetraeder Fall 
	{ 
		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) // Ursprung befindet sich im Tetraeder -> Kollision 
		{ 
			return true; 
		} 
		else if (amount_neg == 3) // Ursprung befindet sich im Tetraeder -> Kollision 
		{ 
			return true; 
		} 
		else // Punkt entfernen 
		{ 
			if (amount_neg == 2 and amount_pos == 1) 
			{ 
				if (v_acd > 0) 
				{ 
					simplex.del(3); // Punkt entfernen 
					d.set(ad_c_ac); // neue Suchrichtung speichern 
				} 
				else if (v_abd > 0) 
				{ 
					simplex.del(2); // Punkt entfernen 
					d.set(ab_c_ad); // neue Suchrichtung speichern 
				} 
				else 
				{ 
					simplex.del(1); // Punkt entfernen 
					d.set(ac_c_ab); // neue Suchrichtung speichern 
				} 
			} 
			else if (amount_neg == 1 and amount_pos == 2) 
			{ 
				if (v_acd &amp;lt; 0) 
				{ 
					ad_c_ac.negate(); 
 
					simplex.del(3); // Punkt entfernen 
					d.set(ad_c_ac); // neue Suchrichtung speichern 
				} 
				else if (v_abd &amp;lt; 0) 
				{ 
					ab_c_ad.negate(); 
 
					simplex.del(2); // Punkt entfernen 
					d.set(ab_c_ad); // neue Suchrichtung speichern 
				} 
				else 
				{ 
					ac_c_ab.negate(); 
 
					simplex.del(1); // Punkt entfernen 
					d.set(ac_c_ab); // neue Suchrichtung speichern 
				} 
			} 
			else 
			{ 
				std::cout &amp;lt;&amp;lt; "error(number pos/neg)" &amp;lt;&amp;lt; std::endl; 
			} 
		} 
	} 
	else if (simplex.size() == 3) // Dreieck (drei Punkte im simplex) 
	{ 
		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) // Linie (zwei Punkte im simplex) 
	{ 
		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; 
}

Markieren

Der letzte Punkt im simplex wird immer als Referenz (Koordinatensystem) genommen. Dann wird eine neue Suchrichtung bestimmt und wenn vier Punkte im simplex sind wird einer entfernt.

Auf den Linien-Fall und Dreieck-Fall wird hier nicht eingegangen, da dieser in http://mollyrocket.com/849 ausführlich erklärt wird.

Der Tetraeder Fall

Der Tetraeder Fall wird in dem obigen Video aber nicht erklärt. Wir wissen, wie der Tetraeder programmiertechnisch ermittelt wurde: Eine Grundfläche (Dreiecks-Fall), welche durch eine neue Suchrichtung (Richtung Ursprung) ein neuer Punkt hinzugefügt wurde. Der Ursprung kann nun weder im Tetraeder (Kollision!) noch unter dem Tetraeder (Suchrichtung) liegen. Somit wissen wir, welche Seiten wir überprüfen müssen (zu welcher Seite der Nullpunkt am nähesten liegt). Um dies zu ermitteln, werden die Flächennormalvektoren zu allen drei Flächen gebildet und das Skalarprodukt dieser Vektoren mit a0 wird berechnet.

Dann wird die Anzahl von positiven und negativen Skalarprodukten gezählt. Sind alle positiv oder negativ können wir auf eine Kollision schließen (der Nullpunkt liegt im Tetraeder). Wenn dies nicht der Fall ist, so entfernen wir den jeweiligen Punkt (welcher nicht Teil des Dreiecks ist, welches am nähesten zum Ursprung ist).

Bilder von den wichtigsten Fällen:

Das Bild oben zeigt den Fall, wenn der Algorithmus nach dem Linien-Fall stoppt. Rot und Grün sind die Polygone, welche auf Kollision getestet werden und Weiß die jeweiligen geometrischen Mittelpunkte. Die gelben Punkte stellen die Minkowski-Differenz dar. Der rote Punkt in der Minkowski-Differenz wird zuerst zum simplex hinzugefügt, dann der grüne. Das große grüne Rechteck rechts unten zeigt an, dass keine Kollision stattgefunden hat (Indikator).

Oben der Tetraeder Fall: Ein dritter (Blau) und vierter Punkt (Pink) wurde dem simplex hinzugefügt. Diese spannen mit den anderen zwei Punkten den Tetraeder auf. Für jedes der Dreiecke (Tetraeder Fall) ist der geometrische Mittelpunkt (Weiß) und die Seitenflächennormale eingezeichnet. Der Ursprung liegt im Tetraeder und die beiden Polynome kollidieren.

Wie im obigen Fall: Kollision mit den Flächennormalen nach außen zeigend.

Oben dargestellt der Tetraeder Fall, wo nicht direkt klar ermittelt wurde, ob eine Kollision stattfindet oder nicht. Die neue Suchrichtung (Rot) wird ermittlet, indem die Flächennormale (türkis) negiert wird.

GJK - Tetraeder Fall (3 Iterationen, keine Kollision)

Oben der Tetraeder Fall mit drei Iterationen (die pinken Punkte sind jeweils die Punkte aus dem Tetraeder Fall). Schön ersichtlich ist, wie sich die pinken Punkte Richtung Ursprung annähern (Suchrichtung!).

Von dem letzten Fall gibt es ein kurzes Video:
https://www.youtube.com/watch?v=Lg2wR12WQ9M

Zwei Tetraeder, welche in einem größeren Würfel kollidieren (Kollisionen ausschließlich mit GJK berechnet):
https://www.youtube.com/watch?v=dQZldWqHqlY

Quellcode-Dateien:
https://github.com/clauskovacs/GJK-collision-detection-3d