Jekyll2020-09-18T13:06:30-07:00https://filipecn.github.io/feed.xmlPersonal Webitepersonal descriptionFilipe Nascimentofilipedecn@gmail.comConvex polyhedra collision test (GJK Algorithm)2019-07-26T00:00:00-07:002019-07-26T00:00:00-07:00https://filipecn.github.io/posts/2019/07/gjk<p>If we interpret a polyhedron as a set of points, two polyhedra
represented by sets \(A\) and \(B\), for example, collide if \(A \cap B \neq \emptyset\).
The intersection set \(A \cap B\) represents all pairs of points
between \(A\) and \(B\) which have distance \(0\) between them, because well… each of these pairs
is composed by the same point (shared by \(A\) and \(B\)).</p>
<p>A very nice operation between sets of points is the <em>Minkowski Sum</em>:</p>
<!-- <hr\> -->
<p>Let \(A\) and \(B\) be two point sets. The Minkowski sum \(A \oplus B\) is defined as the set
\(A \oplus B = \{ a + b : a \in A, b \in B\}.\)
The Minkowski difference is obtained by \(A \ominus B = A \oplus (-B) \).</p>
<hr />
<p>The Minkowski sum is very useful because it can give us the distance between two sets of points \(A\) and \(B\):</p>
\[distance(A, B) = min \{ \parallel c\parallel : c \in A \ominus B \}.\]
<blockquote>
<p>The Euclidian distance between two polyhedra is equivalent to the distance between their Minkowski difference and the origin.</p>
</blockquote>
<p>For two convex polyhedra, \(A\) and \(B\), the Minkowski Sum \(C = A \oplus B\) has the following properties:</p>
<ul>
<li>\(C\) is a convex polyhedron;</li>
<li>The vertices of \(C\) are sums of vertices of \(A\) and \(B\).</li>
</ul>
<p>Thus, the collision exists if and only if \(C\) contains the origin. The red region bellow represents the Minkowski difference set of the two shapes,
play around with the vertices to visualize the final set, notice the origin point.</p>
<div id="myCanvas"></div>
<script src="/assets/js/posts/draw2d.js" type="text/javascript"></script>
<h2 id="gilbert---johnson---keerthi-algorithm">Gilbert - Johnson - Keerthi Algorithm</h2>
<p>In short, the GJK algorithm tests if two objects \(A\) and \(B\) are colliding by checking if \(0 \in A \ominus B\) is <strong>true</strong>
(simply \(distance(A, B)\) ). Although it seems very straightforward (and it is indeed), the real magic and beauty of the GJK algorithm is how \(distance\) is
implemented (A very good description of the algorithm is given by <a href="https://www.youtube.com/watch?v=Qupqu1xe7Io">Casey Muratori</a>), but first, a simple observation:</p>
<blockquote>
<p>The resulting Minkowski Sum of two convex polyhedra is also a convex polyhedron. Since all we care about is to check if \(0\) belongs to the final polyhedron, we only need to focus on the vertices of these geometric shapes (the convex hull), because any operation with interior points will lead to interior points of the resulting shape.</p>
</blockquote>
<p>The brute force algorithm would be to compute <strong>all</strong> pairs of vertices between the polyhedra, which leaves us with <em>quadratic</em> complexity.
In the GJK algorithm on the other hand, instead of computing the entire set \(C = A \ominus B\) explicitly, it only computes points necessary
to find the point in \(C\) that is closest to the origin. The GJK algorithm samples these points using a <em>support mapping</em> of \(C\).</p>
<hr />
<p>Briefly, a <em>support mapping</em> is a function that maps a given direction (d) into a supporting point for the convex polyhedron (A).
\(S_A(d) = max \{ d^Tp, p \in A \}.\)</p>
<hr />
<p>In our 2D code, we could have something like this:</p>
<figure class="highlight"><pre><code class="language-cpp" data-lang="cpp"><span class="k">struct</span> <span class="nc">ConvexPolygon</span> <span class="p">{</span>
<span class="n">vec2</span> <span class="n">support</span><span class="p">(</span><span class="n">vec2</span> <span class="n">d</span><span class="p">)</span> <span class="p">{</span>
<span class="c1">// for all vertices, find for which index the value </span>
<span class="c1">// of dot(vertices[i], d) is greater</span>
<span class="p">...</span>
<span class="c1">// instead of a O(n) algorithm here, we can use hill climbing </span>
<span class="c1">// search. Assuming our vertex list is topologically sorted</span>
<span class="k">return</span> <span class="n">vertices</span><span class="p">[</span><span class="n">max_dot_i</span><span class="p">];</span>
<span class="p">}</span>
<span class="c1">// suppose vertices form a convex shape</span>
<span class="n">std</span><span class="o">::</span><span class="n">vector</span><span class="o"><</span> <span class="n">vec2</span> <span class="o">></span> <span class="n">vertices</span><span class="p">;</span>
<span class="p">};</span></code></pre></figure>
<p>Since \(A \ominus B\) is a linear operation, we have</p>
\[S_{A \ominus B}(d) = max \{d^Tp, p \in A \ominus B\}\]
\[= max \{ d^Ta - d^Tb, a \in A, b \in B \}\]
\[= max \{ d^Ta, a \in A \} - max \{ -d^Tb, b \in B \}\]
\[= S_A(d) - S_B(-d)\]
<p>It means that the <em>support function</em> of the Minkowski difference can be computed from the supporting points of the individual polyhedra \(A\) and \(B\).
Remember, the <em>support function</em> will help the algorithm to “walk” towards the origin, which is our point of interest, so we will choose the direction \(d\) based on this goal. But how do we know the right direction to pick? This interesting theorem will help us answering this question:</p>
<h3 id="carathéodorys-theorem"><em>Carathéodory’s theorem</em></h3>
<p>For a convex body \(H\) of \( \mathbb{R}^d \) each point of \(H\) can be expressed as the convex combination of no more than \(d + 1\) points from \(H\).</p>
<hr />
<p>Ok, it didn’t answered directly our question, but soon it is going to make sense. The message behind the theorem is that
each point of a polyhedron needs no more than 4 points of that polyhedron to be expressed,
in fact this sub-set of \(d + 1\) points has a name:</p>
<hr />
<p>Suppose \(d + 1\) points \(p_0, \dots, p_d \in \mathbb{R}^d \) are affinely independent, than the set of points
\(S = \{ \theta_0 p_0 + \dots + \theta_d p_d \mid \theta_i \geq 0, 0 \leq i \leq d, \sum_{0}^{d} \theta_i = 1\}\)
is named <strong><i>k-Simplex</i></strong>. In other words, the simplex is the simplest polygon of its dimension, here are
the 0-Simplex, 1-Simplex, 2-Simplex and 3-Simplex in order:</p>
<center><img align="middle" src="/images/posts/simplex.svg" /></center>
<hr />
<p>Since our Minkowski difference is a convex body it means that we can split it into a set of simplices and
search for the origin inside them.</p>
<p>However, we don’t need do this explicitly, otherwise we would need to compute the Minkowski difference set explicitly too.
The strategy is to iteratively create a new simplex each step that contains points closer to the origin than the step before until
the origin happens to be inside the current simplex or it be proved to be outside. We start with a 0-Simplex and keep updating with
new points (creating a 1-Simplex, then a 2-Simplex and so on (up to \(d\)-Simplex)) until the process is finished.</p>
<p>Before my text get even more confuse lets take a look at some code (2-dimensional case) to have a more general idea of the whole process.</p>
<figure class="highlight"><pre><code class="language-cpp" data-lang="cpp"><span class="kt">bool</span> <span class="nf">testCollision</span><span class="p">(</span><span class="k">const</span> <span class="n">ConvexPolygon</span><span class="o">&</span> <span class="n">a</span><span class="p">,</span> <span class="k">const</span> <span class="n">ConvexPolygon</span><span class="o">&</span> <span class="n">b</span><span class="p">)</span> <span class="p">{</span>
<span class="c1">// start with an arbitrarily direction for the support mapping</span>
<span class="n">vec2</span> <span class="n">d</span><span class="p">(</span><span class="mi">1</span><span class="p">,</span> <span class="mi">0</span><span class="p">);</span>
<span class="c1">// simplex vertices (since it is 2D, we have at most 3 vertices)</span>
<span class="n">vec2</span> <span class="n">s</span><span class="p">[</span><span class="mi">3</span><span class="p">];</span>
<span class="c1">// number of vertices of the current simplex, initially an empty simplex</span>
<span class="kt">int</span> <span class="n">k</span> <span class="o">=</span> <span class="mi">0</span><span class="p">;</span>
<span class="k">while</span><span class="p">(</span><span class="mi">1</span><span class="p">)</span> <span class="p">{</span>
<span class="c1">// sample a new point from the Minkowski difference set</span>
<span class="n">vec2</span> <span class="n">p</span> <span class="o">=</span> <span class="n">a</span><span class="p">.</span><span class="n">support</span><span class="p">(</span><span class="n">d</span><span class="p">)</span> <span class="o">-</span> <span class="n">b</span><span class="p">.</span><span class="n">support</span><span class="p">(</span><span class="o">-</span><span class="n">d</span><span class="p">);</span>
<span class="c1">// check if the origin outside the set</span>
<span class="k">if</span><span class="p">(</span><span class="n">dot</span><span class="p">(</span><span class="n">p</span><span class="p">,</span> <span class="n">d</span><span class="p">)</span> <span class="o"><</span> <span class="mi">0</span><span class="p">)</span>
<span class="k">return</span> <span class="nb">false</span><span class="p">;</span>
<span class="c1">// build and test the new simplex and compute the new direction d</span>
<span class="k">if</span><span class="p">(</span><span class="n">buildAndTestSimplex</span><span class="p">(</span><span class="n">s</span><span class="p">,</span> <span class="n">p</span><span class="p">,</span> <span class="n">k</span><span class="p">,</span> <span class="n">d</span><span class="p">))</span>
<span class="k">return</span> <span class="nb">true</span><span class="p">;</span>
<span class="p">}</span>
<span class="p">}</span></code></pre></figure>
<p>As you may notice, the code is quite simple (and it really is!), each step we “jump” in the
direction of the origin to a new simplex of our set of simplicies that exists implicitly and look for the origin point (not exactly,
since there is more than one possible set of simplicies we are jumping between different simplicies of different sets… but that is not important here).
The real magic though, is inside <code class="language-plaintext highlighter-rouge">buildAndTestSimplex</code>, each step we need to decide what direction to take using the current simplex, here is
what happens:</p>
<h2 id="first-step-k--1">first step (\(k = 1\))</h2>
<p>First we start with a single point \(p\) of our \(A \ominus B\) set. There is not much to do here. The new direction to take is \(-p\).</p>
<center><img align="middle" src="/images/posts/first.svg" /></center>
<h2 id="second-step-k--2">second step (\(k = 2\))</h2>
<p>Now we have a 1-Simplex. As you can see, our plane is divided into 4 regions where the origin can be found. The first observation is that regions
<strong><span style="color:#ff8080;">1</span></strong> and <strong><span style="color:#ff8080;">4</span></strong>
don’t contain the origin because the vertices were found by the support function in each direction of each of these regions, it means that
there are no more points of the \(A \ominus B\) set there. If the origin was in regions <strong><span style="color:#ff8080;">1</span></strong> or
<strong><span style="color:#ff8080;">4</span></strong> then the algorithm would had stopped at line 12.</p>
<center><img align="middle" src="/images/posts/second.svg" /></center>
<p>So the origin is certainly in <strong><span style="color:#afe9af;">2</span></strong> or <strong><span style="color:#afdde9;">3</span></strong> and
the new direction is</p>
<p>\(v = \overline{S_0S_1}\)
\(\begin{cases}
& d = (-v_y, v_x), & (v \times -S_0)_z > 0\\\
& d = (v_y, -v_x), & (v \times -S_0)_z < 0
\end{cases}\)</p>
<h2 id="nth-step-k--n-n--2">\(n^{th}\) step (\(k = n, n > 2\))</h2>
<p>As the same case above, we observe our plane divided:</p>
<center><img align="middle" src="/images/posts/third.svg" alt="542px-four-level_z-svg" /></center>
<p>If we keep the order of our vertices and use the same logic to exclude the regions <strong><span style="color:#ff8080;">1</span></strong> and <strong><span style="color:#ff8080;">4</span></strong> of the second step, then we can exclude regions <strong><span style="color:#ff8080;">1</span></strong>, <strong><span style="color:#ff8080;">2</span></strong>, <strong><span style="color:#ff8080;">3</span></strong> and <strong><span style="color:#ff8080;">7</span></strong>
of this step. To save some computations, we can verify first if the origin is in region <strong><span style="color:#afe9af;">5</span></strong> and <strong><span style="color:#5599ff;">6</span></strong>, and if is not then certainly is in region <strong><span style="color:#afdde9;">4</span></strong>.
<br />
Defining
\(v_{02} = \overline{S_0S_2}, v_{12} = \overline{S_1S_2}\)</p>
<p>If \((v_{02} \times -S_2).z > 0 \) then the origin is in <strong><span style="color:#afe9af;">5</span></strong>
and the new direction is \((-v_{02}.y, v_{02}.x)\).</p>
<p>If \((v_{12} \times -S_2).z < 0\) then the origin is in <strong><span style="color:#5599ff;">6</span></strong>
and the new direction is \((v_{12}.y, -v_{12}.x)\).</p>
<p>Otherwise we return <strong>true</strong>.
<br />
<strong>Note:</strong> <i>remember to arrange the order of the vertices conveniently so these equations work properly.</i></p>
<p>Here is a very simple example of the algorithm in action:</p>
<div id="myCanvasGJK"></div>
<script src="/assets/js/posts/gjk.js" type="text/javascript"></script>
<p>The code:</p>
<figure class="highlight"><pre><code class="language-cpp" data-lang="cpp"><span class="c1">// the z coordinate of the cross product of vectors (a.x, a.y, 0) </span>
<span class="c1">// and (b.x, b.y, 0)</span>
<span class="kt">float</span> <span class="nf">cross2D</span><span class="p">(</span><span class="k">const</span> <span class="n">vec2</span><span class="o">&</span> <span class="n">a</span><span class="p">,</span> <span class="k">const</span> <span class="n">vec2</span><span class="o">&</span> <span class="n">b</span><span class="p">)</span> <span class="p">{</span>
<span class="k">return</span> <span class="n">a</span><span class="p">.</span><span class="n">x</span> <span class="o">*</span> <span class="n">b</span><span class="p">.</span><span class="n">y</span> <span class="o">-</span> <span class="n">a</span><span class="p">.</span><span class="n">y</span> <span class="o">*</span> <span class="n">b</span><span class="p">.</span><span class="n">x</span><span class="p">;</span>
<span class="p">}</span>
<span class="kt">bool</span> <span class="nf">buildAndTestSimplex</span><span class="p">(</span><span class="n">vec2</span> <span class="n">s</span><span class="p">[],</span> <span class="n">vec2</span> <span class="n">p</span><span class="p">,</span> <span class="kt">int</span> <span class="o">&</span><span class="n">k</span><span class="p">,</span> <span class="n">vec2</span> <span class="o">&</span><span class="n">d</span><span class="p">)</span> <span class="p">{</span>
<span class="n">k</span> <span class="o">=</span> <span class="n">std</span><span class="o">::</span><span class="n">min</span><span class="p">(</span><span class="n">k</span> <span class="o">+</span> <span class="mi">1</span><span class="p">,</span> <span class="mi">3</span><span class="p">);</span>
<span class="n">s</span><span class="p">[</span><span class="n">k</span> <span class="o">-</span> <span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="n">p</span><span class="p">;</span>
<span class="k">if</span><span class="p">(</span><span class="n">k</span> <span class="o">==</span> <span class="mi">1</span><span class="p">)</span> <span class="p">{</span>
<span class="n">D</span> <span class="o">=</span> <span class="o">-</span><span class="n">s</span><span class="p">[</span><span class="mi">0</span><span class="p">];</span>
<span class="k">return</span> <span class="nb">false</span><span class="p">;</span>
<span class="p">}</span>
<span class="k">if</span><span class="p">(</span><span class="n">k</span> <span class="o">==</span> <span class="mi">2</span><span class="p">)</span> <span class="p">{</span>
<span class="n">vec2</span> <span class="n">a</span> <span class="o">=</span> <span class="n">s</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">-</span> <span class="n">s</span><span class="p">[</span><span class="mi">0</span><span class="p">];</span>
<span class="k">if</span><span class="p">(</span><span class="n">cross</span><span class="p">(</span><span class="n">a</span><span class="p">,</span><span class="o">-</span><span class="n">s</span><span class="p">[</span><span class="mi">0</span><span class="p">])</span> <span class="o"><</span> <span class="mi">0</span><span class="p">)</span> <span class="p">{</span>
<span class="n">s</span><span class="p">[</span><span class="mi">2</span><span class="p">]</span> <span class="o">=</span> <span class="n">s</span><span class="p">[</span><span class="mi">1</span><span class="p">];</span>
<span class="n">s</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="n">s</span><span class="p">[</span><span class="mi">0</span><span class="p">];</span>
<span class="n">s</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="n">s</span><span class="p">[</span><span class="mi">2</span><span class="p">];</span>
<span class="n">D</span> <span class="o">=</span> <span class="n">a</span><span class="p">.</span><span class="n">right</span><span class="p">();</span>
<span class="p">}</span>
<span class="k">else</span> <span class="n">D</span> <span class="o">=</span> <span class="n">a</span><span class="p">.</span><span class="n">left</span><span class="p">();</span>
<span class="k">return</span> <span class="nb">false</span><span class="p">;</span>
<span class="p">}</span>
<span class="n">vec2</span> <span class="n">a</span> <span class="o">=</span> <span class="n">s</span><span class="p">[</span><span class="mi">2</span><span class="p">]</span> <span class="o">-</span> <span class="n">s</span><span class="p">[</span><span class="mi">0</span><span class="p">];</span>
<span class="k">if</span><span class="p">(</span><span class="n">cross</span><span class="p">(</span><span class="n">a</span><span class="p">,</span> <span class="o">-</span><span class="n">s</span><span class="p">[</span><span class="mi">0</span><span class="p">])</span> <span class="o">></span> <span class="mi">0</span><span class="p">)</span> <span class="p">{</span>
<span class="n">D</span> <span class="o">=</span> <span class="n">a</span><span class="p">.</span><span class="n">left</span><span class="p">();</span>
<span class="n">s</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="n">s</span><span class="p">[</span><span class="mi">2</span><span class="p">];</span>
<span class="n">k</span><span class="o">--</span><span class="p">;</span>
<span class="k">return</span> <span class="nb">false</span><span class="p">;</span>
<span class="p">}</span>
<span class="n">vec2</span> <span class="n">b</span> <span class="o">=</span> <span class="n">s</span><span class="p">[</span><span class="mi">2</span><span class="p">]</span> <span class="o">-</span> <span class="n">s</span><span class="p">[</span><span class="mi">1</span><span class="p">];</span>
<span class="k">if</span><span class="p">(</span><span class="n">cross</span><span class="p">(</span><span class="n">b</span><span class="p">,</span> <span class="o">-</span><span class="n">s</span><span class="p">[</span><span class="mi">1</span><span class="p">])</span> <span class="o"><</span> <span class="mi">0</span><span class="p">)</span> <span class="p">{</span>
<span class="n">D</span> <span class="o">=</span> <span class="n">b</span><span class="p">.</span><span class="n">right</span><span class="p">();</span>
<span class="n">s</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="n">s</span><span class="p">[</span><span class="mi">2</span><span class="p">];</span>
<span class="n">k</span><span class="o">--</span><span class="p">;</span>
<span class="k">return</span> <span class="nb">false</span><span class="p">;</span>
<span class="p">}</span>
<span class="k">return</span> <span class="nb">true</span><span class="p">;</span>
<span class="p">}</span></code></pre></figure>
<p>The 3D case follows the same idea, the difference is that in case \(k = 3\), we have to check which side of the triangle plane
the origin is and construct a tetrahedron. The cross products for the other cases must be adapted as well.</p>Filipe Nascimentofilipedecn@gmail.comIf we interpret a polyhedron as a set of points, two polyhedra represented by sets \(A\) and \(B\), for example, collide if \(A \cap B \neq \emptyset\). The intersection set \(A \cap B\) represents all pairs of points between \(A\) and \(B\) which have distance \(0\) between them, because well… each of these pairs is composed by the same point (shared by \(A\) and \(B\)).