【UE4】RVOを実装してみる

#はじめに
AIによって制御されるキャラクター（以下エージェント）のナビゲーションには**大局的なナビゲーション**と**局所的なナビゲーション**があります。

**大局的なナビゲーション**：エージェントをA地点からB地点まで移動させる際に「ナビゲーションメッシュ」や「2D・3Dグリッド」等のナビゲーションデータから経路探索アルゴリズムを用いて望ましい経路を導き出す典型的なナビゲーションです。壁や岩、エージェントが侵入できない建物等の**静的な障害物**はナビゲーションデータから取り除かれます。エージェントが経路を辿る際にはあたかも、静的な障害物を避けるような振る舞いをします。

**局所的なナビゲーション**：**動的な障害物（人や動物、車等）を回避するため**の極短期間のナビゲーションです。目的地へは向かいつつも障害物を回避するために少しだけ進路を変更します。

殆どの場面で大局的なナビゲーションはエージェントを良い感じに目的地へ導いてくれますが、道中に有る動的障害物に対してのナビゲートはしてくれません。エージェント同士が向かい合えば当たり前のように正面衝突し衝突判定の跳ね返りで何とか避けるような動きをします。
そこで局所的なナビゲーションとを組み合わせることで相手を避けるといった、より自然で違和感のないナビゲートでエージェントを更に知的に見せる事が出来ます。

この記事では[Reciprocal Velocity Obstacles for Real-Time Multi-Agent Navigation](http://gamma.cs.unc.edu/RVO/)にある論文やライブラリを参考に、Unreal Engine 4（UE4）にてReciprocal Velocity Obstacles（以下RVO）を実装していきます。

#プロジェクトはこちらから
https://1drv.ms/u/s!Au-8FqgREBKZhTkblzBKSuTBTzJw

#プロジェクト内のシーンを実行してみる
プロジェクトには３つのシーンがあり、それぞれ次のような結果を見ることが出来ます。

<blockquote class="twitter-tweet" data-lang="ja"><p lang="ja" dir="ltr">実装したRVOとビルトインのRVOとの比較 <a href="https://t.co/rYIuZnvpuv">pic.twitter.com/rYIuZnvpuv</a></p>&mdash; PavilionDV7 (@Dv7Pavilion) <a href="https://twitter.com/Dv7Pavilion/status/1058318098502049792?ref_src=twsrc%5Etfw">2018年11月2日</a></blockquote>
<script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>
結果1. 実装したRVOとビルトインのRVOとの比較

<blockquote class="twitter-tweet" data-lang="ja"><p lang="ja" dir="ltr">回避の積極性パラメータの比較 <a href="https://t.co/IH7O7HTu5M">pic.twitter.com/IH7O7HTu5M</a></p>&mdash; PavilionDV7 (@Dv7Pavilion) <a href="https://twitter.com/Dv7Pavilion/status/1058318352936984577?ref_src=twsrc%5Etfw">2018年11月2日</a></blockquote>
<script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>
結果2. 回避の積極性パラメータの比較

<blockquote class="twitter-tweet" data-lang="ja"><p lang="ja" dir="ltr">静的な障害物がある環境での動作 <a href="https://t.co/L3XtOTGn7l">pic.twitter.com/L3XtOTGn7l</a></p>&mdash; PavilionDV7 (@Dv7Pavilion) <a href="https://twitter.com/Dv7Pavilion/status/1058318524873986048?ref_src=twsrc%5Etfw">2018年11月2日</a></blockquote>
<script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>
結果3. 静的な障害物がある環境での動作

#RVOの実装
RVOは**BP_AGENT_RVO**に実装されています。

##CalculateRVOVelocity関数の呼び出し
衝突回避速度の計算の呼び出しはカスタムイベントの**CalcRVO**イベントにあります。
![2018-11-02_12h41_53.png](https://qiita-image-store.s3.amazonaws.com/0/104467/b326303f-5db4-4226-f55e-c37b3e7d617d.png)
CalcRVOイベントはBegin Playにて**Set Timer by Event**で一定間隔で呼ばれるようにしています。
![2018-11-02_12h45_26.png](https://qiita-image-store.s3.amazonaws.com/0/104467/35f0727c-826e-8a55-07a2-ecdce6e53827.png)

CalcRVOイベントではエージェントの近くにいる動的障害物（近傍領域内にいる動的障害物）を知るために、**Sphere Overlap Actors**ノードを使用しています。参考にした[RVO Library v1.1.1](http://gamma.cs.unc.edu/RVO2/downloads/)では近くにいる動的障害物を探すために**k-d木**を採用しています。（自分はk-d木を実装したことが無いので手軽に同様の結果を得られるSphereOverlapActorsを採用しました。）

近くにいる障害物が何もなかった場合、CalcPrefferdVelocity関数で**目的地（もしくは目的地に到達するために通る必要のある位置）へと向かう速度**を計算し、自身の移動速度とします。
避けるべき障害物があるなら、CalculateRVOVelocity関数で衝突回避速度を計算します。

##CalculateRVOVelocity関数
###候補速度の選定
衝突回避速度を求めるには、最初に**候補速度**を決める必要があります。いくつかの候補速度を作成し、それらに**どれだけのペナルティがあるのか**を求め、最終的に**ペナルティが最も小さい候補速度**を衝突回避速度とします。
![2018-11-02_13h09_30.png](https://qiita-image-store.s3.amazonaws.com/0/104467/c540e016-696d-49ed-2490-1be0f50079f3.png)
今回のプロジェクトでは上の図のように**RotateVectorAroundAxis**ノードを使ってPrefV（最も望ましい速度）をAngleDeg度回転させていますが、何度回転するかは変数AngleOffsets配列にて決め打ちしています。

###Time to Collisionの計算
候補速度を選んだ後は**衝突までの時間（Time to Collision）**を計算します。
![2018-11-02_14h13_13.png](https://qiita-image-store.s3.amazonaws.com/0/104467/177d2555-63d4-a931-bb0d-a7ea4daf6d25.png)
上の図はTimetoCollision関数に渡す**vAB**を求めている箇所です。
これは論文にもある

RVO（責任の分配率$\alpha_B^A = \alpha_A^B = \frac{1}{2}$）の場合

```math
\begin{align}
R V O _ { B } ^ { A } \left( \mathbf { v } _ { B } , \mathbf { v } _ { A } \right) &= \left\{ \mathbf { v } _ { A } ^ { \prime } | 2 \mathbf { v } _ { A } ^ { \prime } - \mathbf { v } _ { A } \in V O _ { B } ^ { A } \left( \mathbf { v } _ { B } \right) \right\}
\\
\lambda \left( \mathbf { p } _ { A } , 2 \mathbf { v } _ { A } ^ { \prime } - \mathbf { v } _ { A } - \mathbf { v } _ { B } \right) &= B \oplus - A
\end{align}
```

GRVOの場合

```math
\begin{align}
R V O _ { B } ^ { A } \left( \mathbf { v } _ { B } , \mathbf { v } _ { A } , \alpha _ { B } ^ { A } \right) &= \left\{ \mathbf { v } _ { A } ^ { \prime } | \frac { 1 } { \alpha _ { B } ^ { A } } \mathbf { v } _ { A } ^ { \prime } + \left( 1 - \frac { 1 } { \alpha _ { B } ^ { A } } \right) \mathbf { v } _ { A } \in V O _ { B } ^ { A } \left( \mathbf { v } _ { B } \right) \right\}
\\
\lambda \left( \mathbf { p } _ { A } , \frac { 1 } { \alpha _ { B } ^ { A } } \mathbf { v } _ { A } ^ { \prime } + \left( 1 - \frac { 1 } { \alpha _ { B } ^ { A } } \right) \mathbf { v } _ { A } - \mathbf { v } _ { B }  \right) &= B \oplus - A
\end{align}
```

が、そのまま実装された形となります。

###候補速度のペナルティの計算
ある候補速度が適した衝突回避速度なのかを調べるために**その候補速度に対するペナルティ**を求めます。
![2018-11-02_14h23_36.png](https://qiita-image-store.s3.amazonaws.com/0/104467/79f41f93-377f-f928-db5a-9f19ab345c47.png)
上の図ではペナルティを計算していますが、これは論文での

```math
\text { penalty } _ { i } \left( \mathbf { v } _ { i } ^ { \prime } \right) = w _ { i } \frac { 1 } { t c _ { i } \left( \mathbf { v } _ { i } ^ { \prime } \right) } + \left\| \mathbf { v } _ { i } ^ { \mathrm { pref } } - \mathbf { v } _ { i } ^ { \prime } \right\|
```

です。

###Time to Collision関数の中身
<img src="https://qiita-image-store.s3.amazonaws.com/0/104467/44a5ed24-1e1b-001f-875a-e49f5b7834a2.png" width=720>

スクショを撮るためにノードの位置を変更していますが、TimeToCollision関数で行っている計算は論文の

```math
\begin{align}
&w = P_B - P_A \\
&v = v_B - v_A \\
&a = v \cdot v \\
&c = w \cdot w - (r_A + r_B)^2 \\
&b = −w \cdot v = w \cdot (v_A − v_B) \\
\\
&\tau^\pm = (b \pm \sqrt{b^2 − ac}) / a \\
\end{align}
```
と同様です。
$v$や$r_A + r_B$については引数に渡された時点で計算済みですので、この関数内では省略されています。

>2018/11/18 追記
Time To Collision関数の計算は**線分と球（もしくは円）との衝突判定**とほぼ内容は同じです。
[ゲームつくろー！3D衝突編 その24 レイと球の貫通点](http://marupeke296.com/COL_3D_No24_RayToSphere.html)

#おわりに
UE4にはRVOが既にビルトインされているのですが、勉強のために実装してみました。
力試しとして論文だけで実装してみようかなと思いましたが、無理でした。論文からプログラムに落とし込める人はすごいですね。
幸い[Reciprocal Velocity Obstacles for Real-Time Multi-Agent Navigation](http://gamma.cs.unc.edu/RVO/)にはC++での実装例が紹介されていましたし、他にも多くの実装例が見つかるのでUE4で実装するのにそれほど苦労しませんでした。

Velocity ObstacleにはRVOの他にも**The Hybrid Reciprocal Velocity Obstacle（HRVO）**や**Optimal Reciprocal Collision Avoidance（ORCA）**、**Human Like**等々、派生が多くありますので、ゆっくりと勉強しながら解説できればなと思います。

#参考資料
http://gamma.cs.unc.edu/RVO/
RVOアルゴリズムのルーツ（？）。プロジェクトデータもあるので是非。

https://hasht.hatenablog.com/entry/2018/04/19/193301
調べた中で唯一の日本語で解説されたブログ。とても有り難かった。

https://software.intel.com/en-us/articles/reciprocal-collision-avoidance-and-navigation-for-video-games
Intel Developer Zoneにある衝突回避アルゴリズムについての記事。パフォーマンス面にフォーカスされている。