1 /* Copyright Jukka Jylanki
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.h
16         @author Jukka Jylanki
17         @brief Implementation of the Gilbert-Johnson-Keerthi (GJK) convex polyhedron intersection test. */
18 #pragma once
19
20 #include "../MathGeoLibFwd.h"
21 #include "../Math/float3.h"
22
23 MATH_BEGIN_NAMESPACE
24
25 vec UpdateSimplex(vec *s, int &n);
26
27 #define SUPPORT(dir) (a.ExtremePoint(dir, maxS) - b.ExtremePoint(-dir, minS));
28
29 template<typename A, typename B>
30 bool GJKIntersect(const A &a, const B &b)
31 {
32         vec support[4];
33         float maxS, minS;
34         // Start with an arbitrary point in the Minkowski set shape.
35         support[0] = a.AnyPointFast() - b.AnyPointFast();
36         vec d = -support[0]; // First search direction is straight toward the origin from the found point.
37         if (d.LengthSq() < 1e-7f) // Robustness check: Test if the first arbitrary point we guessed produced the zero vector we are looking for!
38                 return true;
39         int n = 1; // Stores the current number of points in the search simplex.
40         int nIterations = 50; // Robustness check: Limit the maximum number of iterations to perform to avoid infinite loop if types A or B are buggy!
41         while(nIterations-- > 0)
42         {
43                 // Compute the extreme point to the direction d in the Minkowski set shape.
44                 vec newSupport = SUPPORT(d);
45 #ifdef MATH_VEC_IS_FLOAT4
46                 assume(newSupport.w == 0.f);
47 #endif
48                 // If the most extreme point in that search direction did not walk past the origin, then the origin cannot be contained in the Minkowski
49                 // convex shape, and the two convex objects a and b do not share a common point - no intersection!
50                 if (minS + maxS < 0.f)
51                         return false;
52                 // Add the newly evaluated point to the search simplex.
53                 support[n++] = newSupport;
54                 // Examine the current simplex, prune a redundant part of it, and produce the next search direction.
55                 d = UpdateSimplex(support, n);
56                 if (n == 0) // Was the origin contained in the current simplex? If so, then the convex shapes a and b do share a common point - intersection!
57                         return true;
58         }
59         assume2(false && "GJK intersection test did not converge to a result!", a.SerializeToString(), b.SerializeToString());
60         return false; // Report no intersection.
61 }
62
63 MATH_END_NAMESPACE