GJK-EPAアルゴリズムによる凸包の衝突検出と最小めりこみベクトル

はじめに

こんにちは〜そばやで〜す☆
今回は衝突判定のこと書いちゃうね〜♡

今回扱うのは、凸包っていう形状の物体だよ！
凸多面体よりももうちょっと一般的で、曲面とかを持っていても大丈夫なんだ！

今回やりたいことは、

二つの凸包が重なっているかを調べる
重なっていたら、二つをどの方向に動かすと一番短い移動でめりこみを解消できるかを調べる

の2つだよ！
実際にゲームや物理エンジンを作るときは、めり込んでるのがわかっても｢これはどの向きに動かすのがいいんだろう？｣とか｢どの方向に力を加えたらいいんだろう？｣とか色々悩んじゃう...
そこでこの方法を使えば、その問題が一気に解消！これで春から新生活が始まるみんなもクラスの人気者になれるね！☆
上で書いた2つの｢今回やりたいこと｣のうち、1番にはGJKアルゴリズム、2番にはEPA法っていう名前の方法を使うんだ！この2つはどっちも必要なことが多いから、2つ合わせてGJK-EPAって呼ぶお友達もいるみたい！
早速これのやり方を紹介したいんだけど...ざんね〜ん☆これの前にいくつか説明しなきゃいけないことがあるんだ♪

図形の凸性

今回扱う図形は、凸性を持った図形だけ。
凸性っていうのは、

その物体に含まれる2つの点を結ぶ線分がその物から絶対にはみ出ない

っていう性質のことなんだ！
数式で書くとこうだね！

｢図形 $P$ が凸性を満たす｣ := ｢ $a, b \in P \Rightarrow \forall t \in [0, 1], (1 - t) a + t b \in P$ ｣

これは色々なところでよく出てくるから要チェックだゾ☆
だから、今から紹介する方法で色々な物体を扱えるけど、トーラスとかクラインの壺みたいな形はダメなんだ...残念(><)

サポート写像

次には｢さぽーとしゃぞう｣っていうもののお話をするね！
さっきから凸包凸包言ってるけど、これってどうやって定義すればいいんだろう？普通なにかの形をプログラムで扱うときには、その頂点をいっぱい持っておいたりするよね。でも今回はそんなわけにはいかない。
なんでかわかるかな？

...そう！その通り！凸図形ってだけだと曲線とか曲面を含んでてもOKになっちゃう。そしたら頂点が無限に必要になっちゃう...こんなの覚えてられないよぉ...ふぇぇ...:;(∩´﹏`∩);:
でも大丈夫！そんな悲しまなくったって平気さ！だって無限個の情報を持っておく方法、みんなも知ってるよね。
そう、関数を使えばいいんだ！関数なら必要なときに必要な情報を取り出せるから無限の情報も持っておける。無限リストみたいな考え方だね☆
関数を使って凸包を表現する、この関数のことを｢さぽーとしゃぞう｣って言うんだ。
じゃあ関数を使えば凸包が全部うまく表現できるかっていうと、それは君の努力次第。簡単なやつならできるけど、複雑なやつだと難しいかも...
まぁいいや。とりあえず関数でどうやって凸包を表現するのか教えてあげるね！

図形 $P$ に対するサポート写像 $f_{P}$ の定義はこんなかんじ。 $f_{P} (v) : =$ 「 $P$ に含まれるベクトルの中で、 $v$ との内積が最大のもの」

数式で書くとこうだね。 $f_{P} (v) : = p$ (ただし、 $p \in P, \forall q \in P, p \cdot v \geq q \cdot v$ )

どういうことかっていうと、物体の中で $v$ 方向に一番遠いやつ、みたいなかんじかなぁ。この定義だと $p$ としてとれるものが複数考えられるけど、GJK中ではどれでもいいのでここでは気にしないことにするね。(最終的には気にしないと辛い)

なんでこの関数で凸包を表現できるかっていうと、 $v$ をいろんな方向にとれば凸包の表面の点が全部手に入ることになるからなんだ！必要な情報は揃ってるってことだね☆
ただ、本当に全部手に入るのは曲面のときだけで平面に対してはそれを作ってる頂点しか取れないんだ...ぼくはここで何日か無駄にしたよ(>_<)

さて、これでサポート写像についてはわかったかな？じゃあ次は｢みんこふすきーわ｣と｢みんこふすきーさ｣ってやつの話ね！

ミンコフスキー和とミンコフスキー差

この2つは多分ロシア人のミンコフスキーさんが考えたんだね。調べてないから本当かどうかしーらない！
これの定義を紹介する前に、ちょっとだけ別の話をするね。
みんなは円と四角の当たり判定ってやったことあるかな？意外と考えるの難しいんだ...
どうやって考ればいいのかっていうと、四角のまわりに円をコロコロ転がしてみればいいんだ。コロコロ転がしながら円の真ん中がどうやって動くのか鉛筆でなぞってみると...

四角を膨らませたような形が見えてきたね。実はこの形の中に円の真ん中が入ってたら｢めりこんでる｣ってことになるんだ！面白いね！
そうすると、｢円と四角の当たり判定｣が｢四角を膨らませたやつと点との当たり判定｣に変わる。これが非常にナウい考え方なのサ！
この考え方をもっと広げていってみよう！

円とか四角とかっていう図形は、点がいっぱい集まったものだよね。点はベクトルとも言える。ということは、図形=ベクトルの集合ってことになるよね？
実はさっきの形っていうのは、円を表すベクトルと四角を表すベクトルを足すか引くかしてできる図形なんだ！足してできた図形はミンコフスキー和、引いてできた図形はミンコフスキー差って呼ばれてる。
数式で書いてみるね。
図形 $A$ はベクトルの集合だから、 ${x ∣ x \in A}$ と表せるね。 $B$ も同じ。 $A$ と $B$ のミンコフスキー和と差を $A \oplus B$ 、 $A ⊖ B$ と書くことにすると、 $A \oplus B = {a + b ∣ a \in A, b \in B}$ $A ⊖ B = {a - b ∣ a \in A, b \in B}$ って表わせるよね。すごーい！

一応定義はできるからしたけど、和のほうは使わないからいーらない！
ミンコフスキー差のほうがめっちゃ使えるんだ！
たとえば、 $A$ と $B$ がぶつかっていて、ちょっとでもめりこんでいたら、 $a$ と $b$ の中で等しくなるペアがいるはずだよね？これを考えてミンコフスキー差の式を見てみると...
なんと！ミンコフスキー差の中に $0$ 、つまり原点が入ってることになっちゃう！やべぇ！
これはさっきの円と四角の話とそっくりなんだ！つまり、｢ $A$ と $B$ の当たり判定｣が｢ $A ⊖ B$ と原点の当たり判定｣に置き換わっちゃう！

ミンコフスキー差の凸性

ミンコフスキー差は2つの図形があれば必ず作れちゃう。これはもとの図形に凸性があってもなくてもいいんだけど、もとの図形に凸性があるとミンコフスキー差のほうも凸性を持つんだ！
理由はこうだね。
凸性を持つ図形 $A, B$ に対して、 $P = A ⊖ B$ とし、 $P$ に含まれる点を任意に2つとり、 $p_{0}, p_{1}$ とする。
ミンコフスキー差の定義より、 $p_{0}, p_{1} \in P (= A ⊖ B)$ に対し、あるベクトル $a_{0}, a_{1} \in A, b_{0}, b_{1} \in B$ が存在し、 $p_{0} = a_{0} - b_{0}, p_{1} = a_{1} - b_{1}$ が成り立つ。 $A, B$ は凸性を持つので、任意の実数 $s, t \in [0, 1]$ に対し、 $(1 - s) a_{0} + s a_{1} \in A, (1 - t) b_{0} + t b_{1} \in B$ が成り立つ。
したげって、実数 $u \in [0, 1]$ に対し、 $p = (1 - u) p_{0} + u p_{1}$ を考えると、 $p = (1 - u) p_{0} + u p_{1} = (1 - u) (a_{0} - b_{0}) + u (a_{1} - b_{1}) = ((1 - u) a_{0} + u a_{1}) - ((1 - u) b_{0} + u b_{1})$ ここで、 $A, B$ の凸性から、 $(1 - u) a_{0} + u a_{1} \in A, (1 - u) b_{0} + u b_{1} \in B$ が成り立つ。
したがって、 $p \in P$ となる。 $P$ 内の任意の2点を結んだ線分が $P$ に含まれることになるため、図形 $P$ は凸。

ミンコフスキー差のサポート写像

ミンコフスキー差のサポート写像っていうのを考えてみよう！ $A ⊖ B$ のサポート写像 $f_{A ⊖ B}$ は、 $f_{A ⊖ B} (v) =$ 「 $p \cdot v$ を最大にする $p \in A ⊖ B$ 」

だったよね。 $p$ は必ず、 $p = a - b$ となる $a \in A, b \in B$ が存在するはずで、その場合は $f_{A ⊖ B} (v) =$ 「 $v \cdot a - v \cdot b$ を最大にする $a \in A, b \in B$ に対して $a - b$ 」

ってなるよね。 $a$ と $b$ はもちろん独立だから、 $v \cdot a - v \cdot b$ が最大 $\Leftrightarrow v \cdot a$ が最大かつ $- v \cdot b$ が最大

つまり、 $f_{A ⊖ B} (v) =$ 「 $v \cdot a$ を最大にする $a$ 」 - 「 $- v \cdot b$ を最大にする $b$ 」 $= f_{A} (v) - f_{B} (- v)$ ってことになるね！
これでミンコフスキー差のサポート写像がわかったよ！

ここでわかったミンコフスキー差とサポート写像の性質から、

図形の条件を、「サポート写像が定義されていること」のみとするアルゴリズムにおいて、 $A$ と $B$ が図形なら $A ⊖ B$ は図形としてよい

ってことがわかるね。

わーい！すごーい！

GJKアルゴリズム

G...なんとかさんと、J...なになにさんと、K...ほにゃららさんが作ったからGJKなんだって！どうでもいいね！
GJKアルゴリズムっていうのは、実は2つの図形の当たり判定をするものじゃなくて、原点と図形との当たり判定をするアルゴリズムなんだ！このアルゴリズムは「図形」の条件にサポート写像しか使っていないから、2つの凸包の当たり判定は原点とミンコフスキー差との当たり判定に置き換えられるよね。

あとややこしくなっちゃうから、今回はミンコフスキー差を閉集合として扱うね。

じゃあさっそくGJKアルゴリズムの手順を説明していこう！
今回は原点と凸包 $P$ の当たり判定をするってことにするね。
そして、2次元の場合と3次元の場合でちょっとだけ違うので分けて説明するよ！
2次元の場合

$P$ の表面上の点を適当に選ぶ。これを $p_{0}$ とする。
$p_{0}$ が $0$ の場合、衝突しているとして終了。
$- p_{0}$ 方向 $v_{1}$ で $P$ のサポート写像をとる。これを $p_{1}$ とする。
$v_{1}$ と $p_{1}$ の内積が負だったら衝突していないとして終了。
$p_{0}$ , $p_{1}$ を結ぶ直線と垂直な方向 $v_{2}$ でサポート写像を取る。これを $p_{2}$ とする。この垂直なベクトルは2本あるが、 $p_{0}$ との内積が負のものを採用する。内積が0の場合にはどっちでも良い。
$v_{2}$ と $p_{2}$ の内積が負だったら衝突していないとして終了。
$p_{0}, p_{1}, p_{2}$ のうち、1つ選んで $a$ とする。他の2点から成る線分の法線(原点に向かう方向のもの)との内積が負の点を消し、残ったものを $p_{0}, p_{1}$ とする。該当する点が複数ある場合、どれでもよい。該当する点がなかった場合、衝突しているとして終了。
5に戻る

3次元の場合

$P$ の表面上の点を適当に選ぶ。これを $p_{0}$ とする。
$p_{0}$ が $0$ の場合、衝突しているとして終了。
$- p_{0}$ 方向 $v_{1}$ で $P$ のサポート写像をとる。これを $p_{1}$ とする。
$v_{1}$ と $p_{1}$ の内積が負だったら衝突していないとして終了。
$p_{0}$ , $p_{1}$ を結ぶ直線と垂直な方向 $v_{2}$ でサポート写像を取る。これを $p_{2}$ とする。この垂直なベクトルは無数にあるが、 $p_{0}$ との内積が負になる範囲で適当に選んでよい。
$v_{2}$ と $p_{2}$ の内積が負だったら衝突していないとして終了。
$p_{0}, p_{1}, p_{2}$ から成る三角形の法線方向 $v_{3}$ でサポート写像を取る。これを $p_{3}$ とする。この法線ベクトルは2本あるが、 $p_{0}$ との内積が負のものを採用する。内積が0の場合にはどっちでも良い。
$v_{3}$ と $p_{3}$ の内積が正だったら衝突していないとして終了。
$p_{0}, p_{1}, p_{2}, p_{3}$ のうち、他の3点から成る三角形の法線(原点に向かう方向のもの)との内積が負の点を消し、残ったものを $p_{0}, p_{1}, p_{2}$ とする。該当する点が複数ある場合、どれでもよい。該当する点がなかった場合、衝突しているとして終了。
7に戻る

こんなかんじですね！
イメージ的には、三角形とか四面体とかをパタパタ折りながら原点に近づいていくってかんじかな？
近づいていったはずなのに、原点より手前側にしか来れなかったら絶対に原点を含んでいないし、凸包の一部である三角形が原点を含んでいたら凸包自体も原点を含んでいるはずってこと。
これでどんな凸包でも原点との当たり判定ができるね！すごーい！

EPA法

EPAっていうのはExpandingPolytopeAlgorithmのことなんだって！ぼく英語わかんなーい☆
EPA法は凸包をどの方向に動かすと一番短い移動で原点を含まないようにできるかを調べる方法だよ！凸包がミンコフスキー差だった場合には、2つの物体が衝突しなくなるなるように動かす、ってことになるね！
EPA法は、「凸包に含まれていて、原点を含んでいる単体」がないと始まらないんだ。ちなみに単体っていうのは、簡単に言うとn次元でn+1個の頂点からできている図形のことだよ。2次元なら三角形で、3次元なら四面体だね。これをちゃんと取ろうとすると意外と大変...
でも大丈夫！GJKアルゴリズムを突破したキミならそれはもう持っているはずなんだ！GJKアルゴリズムが「原点を含んでいるよ！」って言うときにはほとんどの場合、 $p_{i}$ から成る単体が原点を含んでいて終わっているからね。この単体を使えばいい。
EPA法はこの単体を凸多角形(多面体)としてみて、頂点を追加しながらどんどん膨らませる(Expand)ことで最小めり込みベクトルを求めるんだ。
ってことでEPA法の手順を説明していくね。

GJKを用いて、「ミンコフスキー差が原点を含んでいるか」と、含んでいる場合には「原点を含む単体」を得る
ミンコフスキー差が原点を含んでいない場合にはめりこみ解消ベクトルは存在しないとして終了。
単体を構成する頂点の数が足りていない場合はめりこみ解消ベクトルを $0$ として終了。
多角形(多面体)の中で、原点に最も近い辺(面)の法線方向(重心から離れる方向)に凸包のサポート頂点を取る。
得たサポート頂点を多角形(多面体)に加え、膨らませる。
前回得たサポート頂点と前々回得たサポート頂点との距離が一定以下になったら、原点に最も近い辺(面)と原点とを最短距離で結ぶベクトルを最小めり込みベクトルとして終了。
4に戻る。

この中ではいくつか注意する点があるんだ。

まず3番の「頂点の数が足りていれば」というところ。
GJKが「原点を含んでいるよ」と言うときにはほとんどの場合、2次元なら3つ、3次元なら4つの点をもう持っていて単体が作れるんだ。
でも例外として、点や線分や平面なんていう物体が考えられる。今回はミンコフスキー差を閉区間ってことにしたらこういう面積や体積を持たない図形が原点を含むことはあるよね。
ただ、こういう図形の場合にはそもそもめりこみを解消する必要がないじゃないか。だってめりこみ距離は0なんだもの。だからこういう図形に対してはEPAは $0$ を返せばいいってことになる。

次に4番の「辺(面)の法線方向」っていうところに注目しよう。
後の操作を見ていけばわかるかもしれないけど、ここでゲットした法線は多角形(多面体)を膨らませる方向として使うんだ。
もし間違えてこの法線の方向を逆向きにとってしまうと、物体が縮みだしてしまう。
じゃあどうやったら「膨らむ方向」を手に入れられるかを考えてみる。
3番で余計な図形を弾いたから、今ある図形は必ず面積(体積)を持つ図形ってことになるよね。
しかも凸図形。てことは、必ずその重心は物体の内側にあるよね。だからここ基準で方向を決めてあげればいいんだ。
「原点」を基準に考えればいいじゃないかと思う人もいるかもしれない。
でも今回ミンコフスキー差を閉区間として取ったから、原点が辺(面)にちょうど乗っかっている可能性もあるんだ。だから今回は重心を使うことにするよ。
(図. 法線方向の取り方原点比較Ver.だとうまくいかない例も)

次の「多角形(多面体)を膨らませる」ってところがかな～り面倒だから、詳しく説明するね。
2次元の場合にはある程度適当にやってもうまくいくんだ。つまりこんなかんじ。

追加する頂点を $p$ とする。
原点に最も近い辺を検出する。これを $e$ とし、端点を $p_{0}, p_{1}$ とする。
多角形の中から $e$ を削除する。
$p$ と $p_{0}$ から成る辺と $p$ と $p_{1}$ から成る辺の2本を追加する。

これでもう2次元のEPAはできるね！こんなかんじ！

でも、3次元の場合にはこうはいかない。
3次元の場合には、こうなっちゃうんだ。

追加する頂点を $p$ とする。
すでにある凸多面体のすべての面の中で、 $p$ から見える面をすべて削除する。このとき、削除された面を構成していた辺を $e_{0} \dots e_{n}$ とする。
$e_{0} \dots e_{n}$ の中で、重複する辺があればその2つの辺を削除し、残ったものをを $e_{0}^{'} \dots e_{m}^{'}$ とする。
$p$ と $e_{0}^{'}$ , $p$ と $e_{1}^{'} \dots p$ と $e_{m}^{'}$ を結んだ $m$ 個の三角形を凸多角形に追加する。

ポイントなのは、2の「 $p$ から見える面」という部分。実は2次元の場合には「 $p$ から見える辺」というのが「 $p$ に最も近い辺」ただ1つだったからあのようにして平気だったんだ。
だけど、3次元の場合にはこの面が複数ある場合が考えられてしまう。

↑みたいなときには2面消さなきゃいけない。

だから、どうしてもこの処理が必要なんだ。
それから3で書いているけど、重複する辺があったときにそれを使わないようにしないといけない。
じゃないと、多面体の中に面ができることがあるからね。
(図, 多面体の中に面ができてしまうず)

ちなみに、2次元でも3次元でも新しく張る辺(面)によって既にあった頂点が覆い隠されるようなことはないから、頂点の数は単調に増え続けるよ。
それで、「 $p$ から見える面」というのはどういうものなのかっていうと、平面によって空間を2つに分けた時、その面の表側に $p$ がいるような面ってこと。
で、3次元のEPAはこんなかんじになるね。

おまけ～衝突していない2物体の最短距離～

おまけだよ～
今回紹介したのGJKは「絶対に原点を含んでいない」って確かめられたらすぐそこで終わりにしちゃってたよね。
これをもしそこで終えずに続けていったらどうなるか考えてみよう。
もちろん、ミンコフスキー差が原点を含んでいればいつかそこで終了する。
でも原点を含んでいなかったらどうなるだろう？
GJKでは単体が原点のほうに近づくようにパタパタと折り返しながら移動してた。
もし原点がミンコフスキー差の外側にあるなら、やっぱり単体はそっちに近づいていくよね。そうすると最終的には、ミンコフスキー差と原点の最接近点で止まる...気がする。
でも、よく考えてほしい。GJKもEPAも、加える点は必ずサポート写像から手に入れていたよね。サポート写像が連続的に動かないような図形だった場合には、GJKが止まるところっていうのはミンコフスキー差と原点の最接近点じゃなくてミンコフスキー差のサポート写像の中で原点と一番近い点ってことにならないかな？

う～ん？てことは結局出てきた点には意味なんてないってこと？いやいや、そんなことはないんだ。結局惜しいとこまでは来てるはずなんだから。
原点とミンコフスキー差の最接近点っていうのは、GJKの収束先の点を含む辺(面)上にあるに違いない！
ってことは、とりあえずGJKで収束するのを待って、そのあと、最後に生きていた辺(面)と原点との最接近点が、ミンコフスキー差全体と原点との最接近点...ってことになりそうな気がするね。証明はしてないから確信はできないけど多分そんなかんじかな。
ところで、ミンコフスキー差上の点っていうのは必ず物体 $A, b$ 上の点から作られてるよね。サポート頂点からもとの $A$ と $B$ 上の点を復元できるようにしておけば、「ミンコフスキー差と原点との最接近点」と「 $A$ と $B$ との最接近点」が対応づけられるってことになる。実際にはこっちの技術も使ってるらしいよ。
今回はデモが間に合わなかったよ～ごめんね～(>_<)

おしまい

これでそばやのお話はおしまい！楽しんでもらえたかな～？
みんなとまた会える日を楽しみに待ってるよ！明日はto-hutohuとArkさんの担当だね！よろピクピク～～ ♡

そばやのドキ☆ドキ　当たり判定 ~凸形状編~ 【新歓ブログリレー2017 19日目】

GJK-EPAアルゴリズムによる凸包の衝突検出と最小めりこみベクトル

はじめに

図形の凸性

サポート写像

ミンコフスキー和とミンコフスキー差

ミンコフスキー差の凸性

ミンコフスキー差のサポート写像

GJKアルゴリズム

EPA法

おまけ～衝突していない2物体の最短距離～

おしまい

関連する記事

そばやのドキ☆ドキ 当たり判定 ~凸形状編~ 【新歓ブログリレー2017 19日目】

GJK-EPAアルゴリズムによる凸包の衝突検出と最小めりこみベクトル

はじめに

図形の凸性

サポート写像

ミンコフスキー和とミンコフスキー差

ミンコフスキー差の凸性

ミンコフスキー差のサポート写像

GJKアルゴリズム

EPA法

おまけ～衝突していない2物体の最短距離～

おしまい

この記事をシェア

関連する記事

そばやのドキ☆ドキ　当たり判定 ~凸形状編~ 【新歓ブログリレー2017 19日目】