1 /* Copyright Jukka Jyl�nki
2
3    Licensed under the Apache License, Version 2.0 (the "License");
4    you may not use this file except in compliance with the License.
5    You may obtain a copy of the License at
6
7        http://www.apache.org/licenses/LICENSE-2.0
8
9    Unless required by applicable law or agreed to in writing, software
10    distributed under the License is distributed on an "AS IS" BASIS,
11    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12    See the License for the specific language governing permissions and
13    limitations under the License. */
14
15 /** @file GJK.cpp
16         @author Jukka Jyl�nki
17         @brief Implementation of the Gilbert-Johnson-Keerthi (GJK) convex polyhedron intersection test. */
18 #include "GJK.h"
19 #include "../Geometry/LineSegment.h"
20 #include "../Geometry/Triangle.h"
21 #include "../Geometry/Plane.h"
22
23 MATH_BEGIN_NAMESPACE
24
25 /// This function examines the simplex defined by the array of points in s, and calculates which voronoi region
26 /// of that simplex the origin is closest to. Based on that information, the function constructs a new simplex
27 /// that will be used to continue the search, and returns a new search direction for the GJK algorithm.
28 /** @param s [in, out] An array of points in the simplex. When this function returns, this point array is updated to contain the new search simplex.
29         @param n [in, out] The number of points in the array s. When this function returns, this reference is updated to specify how many
30                            points the new search simplex contains.
31         @return The new search direction vector. */
32 vec UpdateSimplex(vec *s, int &n)
33 {
34         if (n == 2)
35         {
36                 // Four voronoi regions that the origin could be in:
37                 // 0) closest to vertex s[0].
38                 // 1) closest to vertex s[1].
39                 // 2) closest to line segment s[0]->s[1]. XX
40                 // 3) contained in the line segment s[0]->s[1], and our search is over and the algorithm is now finished. XX
41
42                 // By construction of the simplex, the cases 0) and 1) can never occur. Then only the cases marked with XX need to be checked.
43 #ifdef MATH_ASSERT_CORRECTNESS
44                 // Sanity-check that the above reasoning is valid by testing each voronoi region and assert()ing that the ones we assume never to
45                 // happen never will.
46                 float d0 = s[0].DistanceSq(vec::zero);
47                 float d1 = s[1].DistanceSq(vec::zero);
48                 float d2 = LineSegment(s[0], s[1]).DistanceSq(vec::zero);
49                 assert2(d2 <= d0, d2, d0);
50                 assert2(d2 <= d1, d2, d1);
51                 // Cannot be in case 0: the step 0 -> 1 must have been toward the zero direction:
52                 assert(Dot(s[1]-s[0], -s[0]) >= 0.f);
53                 // Cannot be in case 1: the zero direction cannot be in the voronoi region of vertex s[1].
54                 assert(Dot(s[1]-s[0], -s[1]) <= 0.f);
55 #endif
56
57                 vec d01 = s[1] - s[0];
58                 vec newSearchDir = Cross(d01, Cross(d01, s[1]));
59                 if (newSearchDir.LengthSq() > 1e-7f)
60                         return newSearchDir; // Case 2)
61                 else
62                 {
63                         // Case 3)
64                         n = 0;
65                         return vec::zero;
66                 }
67         }
68         else if (n == 3)
69         {
70                 // Nine voronoi regions:
71                 // 0) closest to vertex s[0].
72                 // 1) closest to vertex s[1].
73                 // 2) closest to vertex s[2].
74                 // 3) closest to edge s[0]->s[1].
75                 // 4) closest to edge s[1]->s[2].  XX
76                 // 5) closest to edge s[0]->s[2].  XX
77                 // 6) closest to the triangle s[0]->s[1]->s[2], in the positive side.  XX
78                 // 7) closest to the triangle s[0]->s[1]->s[2], in the negative side.  XX
79                 // 8) contained in the triangle s[0]->s[1]->s[2], and our search is over and the algorithm is now finished.  XX
80
81                 // By construction of the simplex, the origin must always be in a voronoi region that includes the point s[2], since that
82                 // was the last added point. But it cannot be the case 2), since previous search took us deepest towards the direction s[1]->s[2],
83                 // and case 2) implies we should have been able to go even deeper in that direction, or that the origin is not included in the convex shape,
84                 // a case which has been checked for already before. Therefore the cases 0)-3) can never occur. Only the cases marked with XX need to be checked.
85 #ifdef MATH_ASSERT_CORRECTNESS
86                 // Sanity-check that the above reasoning is valid by testing each voronoi region and assert()ing that the ones we assume never to
87                 // happen never will.
88                 float d[7];
89                 d[0] = s[0].DistanceSq(vec::zero);
90                 d[1] = s[1].DistanceSq(vec::zero);
91                 d[2] = s[2].DistanceSq(vec::zero);
92                 d[3] = LineSegment(s[0], s[1]).DistanceSq(vec::zero);
93                 d[4] = LineSegment(s[1], s[2]).DistanceSq(vec::zero);
94                 d[5] = LineSegment(s[2], s[0]).DistanceSq(vec::zero);
95                 d[6] = Triangle(s[0], s[1], s[2]).DistanceSq(vec::zero);
96
97                 bool isContainedInTriangle = (d[6] <= 1e-3f); // Are we in case 8)?
98                 float dist = FLOAT_INF;
99                 int minDistIndex = -1;
100                 for(int i = 4; i < 7; ++i)
101                         if (d[i] < dist)
102                         {
103                                 dist = d[i];
104                                 minDistIndex = i;
105                         }
106
107                 assert4(isContainedInTriangle || dist <= d[0] + 1e-4f, d[0], dist, isContainedInTriangle, minDistIndex);
108                 assert4(isContainedInTriangle || dist <= d[1] + 1e-4f, d[1], dist, isContainedInTriangle, minDistIndex);
109                 assert4(isContainedInTriangle || dist <= d[2] + 1e-4f, d[2], dist, isContainedInTriangle, minDistIndex);
110                 assert4(isContainedInTriangle || dist <= d[3] + 1e-4f, d[3], dist, isContainedInTriangle, minDistIndex);
111 #endif
112                 vec d12 = s[2]-s[1];
113                 vec d02 = s[2]-s[0];
114                 vec triNormal = Cross(d02, d12);
115
116                 vec e12 = Cross(d12, triNormal);
117                 float t12 = Dot(s[1], e12);
118                 if (t12 < 0.f)
119                 {
120                         // Case 4: Edge 1->2 is closest.
121 #ifdef MATH_ASSERT_CORRECTNESS
122                         assert4(d[4] <= dist + 1e-3f * Max(1.f, d[4], dist), d[4], dist, isContainedInTriangle, minDistIndex);
123 #endif
124                         vec newDir = Cross(d12, Cross(d12, s[1]));
125                         s[0] = s[1];
126                         s[1] = s[2];
127                         n = 2;
128                         return newDir;
129                 }
130                 vec e02 = Cross(triNormal, d02);
131                 float t02 = Dot(s[0], e02);
132                 if (t02 < 0.f)
133                 {
134                         // Case 5: Edge 0->2 is closest.
135 #ifdef MATH_ASSERT_CORRECTNESS
136                         assert4(d[5] <= dist + 1e-3f * Max(1.f, d[5], dist), d[5], dist, isContainedInTriangle, minDistIndex);
137 #endif
138                         vec newDir = Cross(d02, Cross(d02, s[0]));
139                         s[1] = s[2];
140                         n = 2;
141                         return newDir;
142                 }
143                 // Cases 6)-8):
144 #ifdef MATH_ASSERT_CORRECTNESS
145                 assert4(d[6] <= dist + 1e-3f * Max(1.f, d[6], dist), d[6], dist, isContainedInTriangle, minDistIndex);
146 #endif
147                 float scaledSignedDistToTriangle = triNormal.Dot(s[2]);
148                 float distSq = scaledSignedDistToTriangle*scaledSignedDistToTriangle;
149                 float scaledEpsilonSq = 1e-6f*triNormal.LengthSq();
150
151                 if (distSq > scaledEpsilonSq)
152                 {
153                         // The origin is sufficiently far away from the triangle.
154                         if (scaledSignedDistToTriangle <= 0.f)
155                                 return triNormal; // Case 6)
156                         else
157                         {
158                                 // Case 7) Swap s[0] and s[1] so that the normal of Triangle(s[0],s[1],s[2]).PlaneCCW() will always point towards the new search direction.
159                                 std::swap(s[0], s[1]);
160                                 return -triNormal;
161                         }
162                 }
163                 else
164                 {
165                         // Case 8) The origin lies directly inside the triangle. For robustness, terminate the search here immediately with success.
166                         n = 0;
167                         return vec::zero;
168                 }
169         }
170         else // n == 4
171         {
172                 // A tetrahedron defines fifteen voronoi regions:
173                 //  0) closest to vertex s[0].
174                 //  1) closest to vertex s[1].
175                 //  2) closest to vertex s[2].
176                 //  3) closest to vertex s[3].
177                 //  4) closest to edge s[0]->s[1].
178                 //  5) closest to edge s[0]->s[2].
179                 //  6) closest to edge s[0]->s[3].  XX
180                 //  7) closest to edge s[1]->s[2].
181                 //  8) closest to edge s[1]->s[3].  XX
182                 //  9) closest to edge s[2]->s[3].  XX
183                 // 10) closest to the triangle s[0]->s[1]->s[2], in the outfacing side.
184                 // 11) closest to the triangle s[0]->s[1]->s[3], in the outfacing side. XX
185                 // 12) closest to the triangle s[0]->s[2]->s[3], in the outfacing side. XX
186                 // 13) closest to the triangle s[1]->s[2]->s[3], in the outfacing side. XX
187                 // 14) contained inside the tetrahedron simplex, and our search is over and the algorithm is now finished. XX
188
189                 // By construction of the simplex, the origin must always be in a voronoi region that includes the point s[3], since that
190                 // was the last added point. But it cannot be the case 3), since previous search took us deepest towards the direction s[2]->s[3],
191                 // and case 3) implies we should have been able to go even deeper in that direction, or that the origin is not included in the convex shape,
192                 // a case which has been checked for already before. Therefore the cases 0)-5), 7) and 10) can never occur and
193                 // we only need to check cases 6), 8), 9), 11), 12), 13) and 14), marked with XX.
194
195 #ifdef MATH_ASSERT_CORRECTNESS
196                 // Sanity-check that the above reasoning is valid by testing each voronoi region and assert()ing that the ones we assume never to
197                 // happen never will.
198                 float d[14];
199                 d[0] = s[0].DistanceSq(vec::zero);
200                 d[1] = s[1].DistanceSq(vec::zero);
201                 d[2] = s[2].DistanceSq(vec::zero);
202                 d[3] = s[3].DistanceSq(vec::zero);
203                 d[4] = LineSegment(s[0], s[1]).DistanceSq(vec::zero);
204                 d[5] = LineSegment(s[0], s[2]).DistanceSq(vec::zero);
205                 d[6] = LineSegment(s[0], s[3]).DistanceSq(vec::zero);
206                 d[7] = LineSegment(s[1], s[2]).DistanceSq(vec::zero);
207                 d[8] = LineSegment(s[1], s[3]).DistanceSq(vec::zero);
208                 d[9] = LineSegment(s[2], s[3]).DistanceSq(vec::zero);
209                 d[10] = Triangle(s[0], s[1], s[2]).DistanceSq(vec::zero);
210                 d[11] = Triangle(s[0], s[1], s[3]).DistanceSq(vec::zero);
211                 d[12] = Triangle(s[0], s[2], s[3]).DistanceSq(vec::zero);
212                 d[13] = Triangle(s[1], s[2], s[3]).DistanceSq(vec::zero);
213
214                 vec Tri013Normal = Cross(s[1]-s[0], s[3]-s[0]);
215                 vec Tri023Normal = Cross(s[3]-s[0], s[2]-s[0]);
216                 vec Tri123Normal = Cross(s[2]-s[1], s[3]-s[1]);
217                 vec Tri012Normal = Cross(s[2] - s[0], s[1] - s[0]);
218                 assert(Dot(Tri012Normal, s[3] - s[0]) <= 0.f);
219                 float InTri012 = Dot(-s[0], Tri012Normal);
220                 float InTri013 = Dot(-s[3], Tri013Normal);
221                 float InTri023 = Dot(-s[3], Tri023Normal);
222                 float InTri123 = Dot(-s[3], Tri123Normal);
223                 bool insideSimplex = InTri012 <= 0.f && InTri013 <= 0.f && InTri023 <= 0.f && InTri123 <= 0.f;
224
225                 float dist = FLOAT_INF;
226                 int minDistIndex = -1;
227                 for(int i = 6; i < 14; ++i)
228                         if (i == 6 || i == 8 || i == 9 || i == 11 || i == 12 || i == 13)
229                                 if (d[i] < dist)
230                                 {
231                                         dist = d[i];
232                                         minDistIndex = i;
233                                 }
234                 assert4(insideSimplex || dist <= d[0] + 1e-4f * Max(1.f, d[0], dist), d[0], dist, insideSimplex, minDistIndex);
235                 assert4(insideSimplex || dist <= d[1] + 1e-4f * Max(1.f, d[1], dist), d[1], dist, insideSimplex, minDistIndex);
236                 assert4(insideSimplex || dist <= d[2] + 1e-4f * Max(1.f, d[2], dist), d[2], dist, insideSimplex, minDistIndex);
237                 assert4(insideSimplex || dist <= d[4] + 1e-4f * Max(1.f, d[4], dist), d[4], dist, insideSimplex, minDistIndex);
238                 assert4(insideSimplex || dist <= d[5] + 1e-4f * Max(1.f, d[5], dist), d[5], dist, insideSimplex, minDistIndex);
239                 assert4(insideSimplex || dist <= d[7] + 1e-4f * Max(1.f, d[7], dist), d[7], dist, insideSimplex, minDistIndex);
240                 assert4(insideSimplex || dist <= d[10] + 1e-4f * Max(1.f, d[10], dist), d[10], dist, insideSimplex, minDistIndex);
241 #endif
242
243                 vec d01 = s[1] - s[0];
244                 vec d02 = s[2] - s[0];
245                 vec d03 = s[3] - s[0];
246                 vec tri013Normal = Cross(d01, d03); // Normal of triangle 0->1->3 pointing outwards from the simplex.
247                 vec tri023Normal = Cross(d03, d02); // Normal of triangle 0->2->3 pointing outwards from the simplex.
248                 assert(Dot(tri013Normal, d02) <= 0.f);
249                 assert(Dot(tri023Normal, d01) <= 0.f);
250
251                 vec e03_1 = Cross(tri013Normal, d03); // The normal of edge 0->3 on triangle 013.
252                 vec e03_2 = Cross(d03, tri023Normal); // The normal of edge 0->3 on triangle 023.
253                 float inE03_1 = Dot(e03_1, s[3]);
254                 float inE03_2 = Dot(e03_2, s[3]);
255                 if (inE03_1 <= 0.f && inE03_2 <= 0.f)
256                 {
257                         // Case 6) Edge 0->3 is closest. Simplex degenerates to a line segment.
258 #ifdef MATH_ASSERT_CORRECTNESS
259                         assert4(!insideSimplex && d[6] <= dist + 1e-3f * Max(1.f, d[6], dist), d[6], dist, insideSimplex, minDistIndex);
260 #endif
261                         vec newDir = Cross(d03, Cross(d03, s[3]));
262                         s[1] = s[3];
263                         n = 2;
264                         return newDir;
265                 }
266
267                 vec d12 = s[2] - s[1];
268                 vec d13 = s[3] - s[1];
269                 vec tri123Normal = Cross(d12, d13);
270                 assert(Dot(tri123Normal, -d02) <= 0.f);
271                 vec e13_0 = Cross(d13, tri013Normal);
272                 vec e13_2 = Cross(tri123Normal, d13);
273                 float inE13_0 = Dot(e13_0, s[3]);
274                 float inE13_2 = Dot(e13_2, s[3]);
275                 if (inE13_0 <= 0.f && inE13_2 <= 0.f)
276                 {
277                         // Case 8) Edge 1->3 is closest. Simplex degenerates to a line segment.
278 #ifdef MATH_ASSERT_CORRECTNESS
279                         assert4(!insideSimplex && d[8] <= dist + 1e-3f * Max(1.f, d[8], dist), d[8], dist, insideSimplex, minDistIndex);
280 #endif
281                         vec newDir = Cross(d13, Cross(d13, s[3]));
282                         s[0] = s[1];
283                         s[1] = s[3];
284                         n = 2;
285                         return newDir;
286                 }
287
288                 vec d23 = s[3] - s[2];
289                 vec e23_0 = Cross(tri023Normal, d23);
290                 vec e23_1 = Cross(d23, tri123Normal);
291                 float inE23_0 = Dot(e23_0, s[3]);
292                 float inE23_1 = Dot(e23_1, s[3]);
293                 if (inE23_0 <= 0.f && inE23_1 <= 0.f)
294                 {
295                         // Case 9) Edge 2->3 is closest. Simplex degenerates to a line segment.
296 #ifdef MATH_ASSERT_CORRECTNESS
297                         assert4(!insideSimplex && d[9] <= dist + 1e-3f * Max(1.f, d[9], dist), d[9], dist, insideSimplex, minDistIndex);
298 #endif
299                         vec newDir = Cross(d23, Cross(d23, s[3]));
300                         s[0] = s[2];
301                         s[1] = s[3];
302                         n = 2;
303                         return newDir;
304                 }
305
306                 float inTri013 = Dot(s[3], tri013Normal);
307                 if (inTri013 < 0.f && inE13_0 >= 0.f && inE03_1 >= 0.f)
308                 {
309                         // Case 11) Triangle 0->1->3 is closest.
310 #ifdef MATH_ASSERT_CORRECTNESS
311                         assert4(!insideSimplex && d[11] <= dist + 1e-3f * Max(1.f, d[11], dist), d[11], dist, insideSimplex, minDistIndex);
312 #endif
313                         s[2] = s[3];
314                         n = 3;
315                         return tri013Normal;
316                 }
317                 float inTri023 = Dot(s[3], tri023Normal);
318                 if (inTri023 < 0.f && inE23_0 >= 0.f && inE03_2 >= 0.f)
319                 {
320                         // Case 12) Triangle 0->2->3 is closest.
321 #ifdef MATH_ASSERT_CORRECTNESS
322                         assert4(!insideSimplex && d[12] <= dist + 1e-3f * Max(1.f, d[12], dist), d[12], dist, insideSimplex, minDistIndex);
323 #endif
324                         s[1] = s[0];
325                         s[0] = s[2];
326                         s[2] = s[3];
327                         n = 3;
328                         return tri023Normal;
329                 }
330                 float inTri123 = Dot(s[3], tri123Normal);
331                 if (inTri123 < 0.f && inE13_2 >= 0.f && inE23_1 >= 0.f)
332                 {
333                         // Case 13) Triangle 1->2->3 is closest.
334 #ifdef MATH_ASSERT_CORRECTNESS
335                         assert4(!insideSimplex && d[13] <= dist + 1e-3f * Max(1.f, d[13], dist), d[13], dist, insideSimplex, minDistIndex);
336 #endif
337                         s[0] = s[1];
338                         s[1] = s[2];
339                         s[2] = s[3];
340                         n = 3;
341                         return tri123Normal;
342                 }
343
344                 // Case 14) Not in the voronoi region of any triangle or edge. The origin is contained in the simplex, the search is finished.
345                 n = 0;
346                 return vec::zero;
347         }
348 }
349
350 MATH_END_NAMESPACE