Bifrostではじめる位置ベース物理シミュ 第4回 ~コリジョンを追加する~

ハイどうもこんにちは！

[前回](https://qiita.com/akasaki1211/items/9d22075df1a51be1ed4c)と同じ要領で、導き出された位置を直接操作しコリジョンによる押し出しを加えていきます。ここで取り上げるコリジョン形状は球やカプセルなどのシンプルなプリミティブです。

さっそく始めていきます。

# 地面（水平固定の無限平面）

最も簡単な例です。何も難しい計算はありません。点$p$の位置座標$(p_x,p_y,p_z)$のうち$p_y$が地面の高さ+半径より下に埋まっていたら高さ+半径で上書きするだけです。

こうなっていたら、

```math
p_y < h + r_p
```

こうします。

```math
p_y = h + r_p
```

$h$は地面の高さ、$r_p$は点の半径です。

ベースのBifrostグラフは[第3回の「片方が固定の距離制約」が入ったもの](https://qiita.com/akasaki1211/items/9d22075df1a51be1ed4c#%E7%89%87%E6%96%B9%E3%81%8C%E5%9B%BA%E5%AE%9A)を使い、次のように地面コリジョンを追加します。

![image_04_01.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/09c6e27d-997b-4da1-6f1b-9a879bc84509.png)
▲bifrostGraphShape > pbd

が、ここで困ったことがあります。前回「制約は絶対！」と言って速度更新の直前に距離制約を足したのですが「コリジョンも絶対！」でないと困ります。**2つの条件を同時に満たす良い感じの場所**を見つけたいです。　
![gif_04_01.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/65135146-166c-b9f4-5b03-b553f7ba3002.gif)
▲後の距離補正により地面コリジョンの補正結果が壊されている

そこで、同時に満たしたい複数の位置補正を`iterate`の中に放り込みます。**『コリジョン補正(①)の後に距離補正(②)をしたらコリジョンにまた埋まったので、もう一度コリジョン補正(③)して、そしたら今度は距離がずれたのでまた補正(④)して、、、』** という具合に何回か繰り返すといい感じの場所に収束します。いわゆる反復法というやつです。
![fig_04_01.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/d8155ba3-da06-2c75-8679-64c96352e4ce.jpeg)

グラフはこのようになりました。
![image_04_02a.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/9cdbbe77-44b8-25ab-13d2-aacbf77cd556.png)
▲bifrostGraphShape > pbd

positionポート、updated_positionポートをState Portに設定します。
![image_04_02b.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/d23f2ef9-7957-61ad-d198-9c7236e6cf5c.png)
▲bifrostGraphShape > pbd > iterate

![image_04_02c.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/f52083c9-0066-5d7a-be3b-62de66bda2e3.png)
▲bifrostGraphShape > pbd > iterate > Ground_Collision

max_iterationsを5に設定して動かしてみると、距離制約を満たしつつ地面にも埋まりにくくなりました！
![gif_04_02.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/19c73765-9310-f9e7-f0e4-59e724d400e7.gif)

# 球

位置補正のイテレーションが組めたところでその他のコリジョン形状をイテレーション内に足していきます。詳細な解説はありがた〜いリンクを貼っておきますので気になる方は見てみてください。この場では最低限の説明でさらりと行きます。

https://light11.hatenadiary.com/entry/2018/08/31/001024

点$p$の半径を$r_p$、球コリジョンの中心を$c$、コリジョンの半径を$r_c$、とすると、$p,c$間の距離が$r_c + r_p$よりも小さかったら、衝突しています。

```math
\| p-c \| < r_c + r_p
```

このとき、$cp$ベクトルの方向へ$r_c + r_p$だけ離れた位置に$p$を移動します。

```math
p' = c + \frac{p-c}{\|p-c\|} (r_c+r_p)
```
![fig_04_02.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/2ab5eda9-1077-627c-4d31-27a2ff9068e3.jpeg)

::: note
ちなみに、単純な大小比較が目的であれば、正確な距離を測る必要はなく、距離の2乗を使うことで計算が重めの平方根を取り除くことができます。先ほどの衝突判定式はこちらで置き換えられます。

```math
(p-c)\cdot(p-c) < (r_c+r_p)^2
```
:::

Bifrostグラフは次のようになります。さっきの地面コリジョンも残してあります。
![image_04_03a.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/7953a4b6-696c-74b0-6caa-d8b1ca4126ec.png)
▲bifrostGraphShape > pbd > iterate

![image_04_03b.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/05507a81-6a37-9a93-c708-2b7cce551dd2.png)
▲bifrostGraphShape > pbd > iterate > Sphere_Collision

![gif_04_03.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/1a381bf0-b012-2445-c251-cc6db933b1c4.gif)

::: note
再生しなくても0フレ（Resetされるフレーム）以外であればマニピュレーター操作で動きを確認できます。
:::

# カプセル

参考リンクはこちら。こちらも最低限の説明でいきます。

https://light11.hatenadiary.com/entry/2018/09/04/221302

カプセル両端の球の中心を$c_a, c_b$として、まず$p$から線分$c_ac_b$への最近傍点$q$を求めたいのですが、先に$c_a,q$間の距離$h$を出します。

```math
h = \frac{c_b-c_a}{\|c_b-c_a\|} \cdot (p-c_a)
```
![fig_04_03a.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/13123e49-9cb9-f6f2-a7bc-f34649aaa750.jpeg)

$h$を$c_a, c_b$間の距離$d_c$で割った値を見ると$p$がどの領域にあるかが分かります。

```math
\displaylines{
k = \frac{h}{d_c} \\
\begin{cases}
k \leqq 0 \\ 
1 \leqq k \\
0 < k < 1 
\end{cases}
}
```
![fig_04_03b.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/7b0be280-dab9-e127-c5a5-d8d0e2525042.jpeg)

$k$が0以下の場合と1以上の場合は球の時と同じです。$k$が0~1の間の場合は次の式で衝突判定を行います。$r_c$はカプセルの半径です。

```math
\displaylines{
q = c_a + \frac{c_b-c_a}{\|c_b-c_a\|} h \\\
(p-q) \cdot (p-q) < (r_c+r_p)^2
}
```

衝突していたら、$q$から$qp$方向に向かって$r_c + r_p$だけ離れた位置に$p$を修正します。

```math
p' = q + \frac{p-q}{\|p-q\|} (r_c + r_p)
```

Bifrostグラフは次のようになります。
![image_04_04a.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/fd93f4f8-e95b-22dd-f0cc-7162a825d1ef.png)
▲bifrostGraphShape > pbd > iterate

ちょっとグラフの威圧感が強めになってきましたね…。右上のほうで、先ほど作った球コリジョンも使われてます。
![image_04_04b.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/0bbaf756-eb65-47ed-aa24-f0fc221464e0.png)
▲bifrostGraphShape > pbd > iterate > Capsule_Collision

![gif_04_04.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/b129c274-1c63-d743-b4c0-802f46461baa.gif)

ちなみに、先ほどの$k$は$q$がカプセル上のどの位置にいるかの割合です。もしカプセル両端の球の半径が違っていてそれぞれ$r_{ca}, r_{cb}$の場合、これらを$k$で補間すれば$q$地点での半径が出ます。

```math
(1-k)r_{ca} + kr_{cb}
```

Bifrostグラフはこのようになります。
![image_04_05.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/a2457ebb-f4c4-6df1-567b-f9b070bcd84a.png)
▲bifrostGraphShape > pbd > iterate > Capsule_Collision（変わっているのは黄色枠の部分です）

![gif_04_05.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/21b03c7c-5ecc-7bca-5dbf-eb752ed318ed.gif)

::: note

カプセル型のプリミティブはMayaにないのでこちらのスクリプトを使ってみて下さい。ちょうどいい具合のカプセルコライダーが生成されます。

```python
from maya import cmds

root = cmds.createNode('transform', n='capsuleCollider')
cmds.addAttr(root, ln='radiusA', nn='Radius A', at='double', dv=0.5, min=0.001, k=True)
cmds.addAttr(root, ln='radiusB', nn='Radius B', at='double', dv=0.5, min=0.001, k=True)
cmds.addAttr(root, ln='height', nn='Height', at='double', dv=2.0, min=0.001, k=True)

sphere1 = cmds.sphere(ssw=180, esw=360, nsp=8, ax=[1,0,0])
sphere2 = cmds.sphere(ssw=0, esw=180, nsp=8, ax=[1,0,0])
circle1 = cmds.circle(s=16, nr=[0,1,0])
circle2 = cmds.circle(s=16, nr=[0,1,0])
loftedSurface = cmds.loft(circle1[0], circle2[0])
cmds.parent(sphere1[0], root, r=1)
cmds.parent(sphere2[0], root, r=1)
cmds.parent(circle1[0], root, r=1)
cmds.parent(circle2[0], root, r=1)
cmds.parent(loftedSurface[0], root, r=1)
cmds.setAttr(circle1[0] + '.v', 0)
cmds.setAttr(circle2[0] + '.v', 0)

md1 = cmds.createNode('multiplyDivide')
md2 = cmds.createNode('multiplyDivide')
cmds.setAttr(md1 + ".operation", 1)
cmds.setAttr(md1 + ".input2X", 0.5)
cmds.setAttr(md2 + ".operation", 1)
cmds.setAttr(md2 + ".input2X", -0.5)

cmds.connectAttr(root + '.worldInverseMatrix[0]', loftedSurface[0] + '.offsetParentMatrix', f=True)
cmds.connectAttr(md1 + '.outputX', sphere1[0] + '.ty', f=True)
cmds.connectAttr(md2 + '.outputX', sphere2[0] + '.ty', f=True)
cmds.connectAttr(md1 + '.outputX', circle1[0] + '.ty', f=True)
cmds.connectAttr(md2 + '.outputX', circle2[0] + '.ty', f=True)
cmds.connectAttr(root + '.height', md1 + '.input1X', f=True)
cmds.connectAttr(root + '.height', md2 + '.input1X', f=True)
cmds.connectAttr(root + '.radiusA', sphere1[1] + '.radius', f=True)
cmds.connectAttr(root + '.radiusB', sphere2[1] + '.radius', f=True)
cmds.connectAttr(root + '.radiusA', circle1[1] + '.radius', f=True)
cmds.connectAttr(root + '.radiusB', circle2[1] + '.radius', f=True)
```
:::

# 無限平面

参考リンクはこちら。「ある点から平面までの距離」のところです。同じく説明は最低限でいきます。

http://marupeke296.com/COL_Basic_No2_ShortTec.html#%E3%81%82%E3%82%8B%E7%82%B9%E3%81%8B%E3%82%89%E5%B9%B3%E9%9D%A2%E3%81%BE%E3%81%A7%E3%81%AE%E8%B7%9D%E9%9B%A2

平面上の点を$c$、平面の法線を$n_c$として、$p$から平面までの最短距離$l$を出します。この距離がマイナス値になっていたら、衝突しています。

```math
\displaylines{
l = n_c \cdot (p-c) \\\
l - r_p < 0
}
```

このとき、$n_c$方向に向かって埋まった距離分だけ$p$を移動します。

```math
p' = p + n_c |l-r_p|
```
![fig_04_04.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/006b8f52-c64e-2675-29ec-8fc996681b87.jpeg)

Bifrostグラフは次のようになります。
![image_04_06a.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/7fd32c84-c6a8-2c35-3699-7672a158f09f.png)
▲bifrostGraphShape > pbd > iterate

![image_04_06b.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/81c9b2c5-a775-f367-598c-35ba5b56aefe.png)
▲bifrostGraphShape > pbd > iterate > Infinite_Plane_Collision

![gif_04_06.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/961f095b-35a1-6dcb-e1ae-a5e8e4dedf8d.gif)

プリミティブのみとはいえコリジョンが入るとだいぶワクワク感が増してきたかと思います！次回はついに、第1回冒頭にチラ見せしたようなぷるぷるゼリーやロープを作ってみます。

# 今回のグラフまとめ

地面コリジョン（bifrostGraphShape > pbd > iterate > Ground_Collision）
![image_04_02c.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/f52083c9-0066-5d7a-be3b-62de66bda2e3.png)

球コリジョン（bifrostGraphShape > pbd > iterate > Sphere_Collision）
![image_04_03b.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/05507a81-6a37-9a93-c708-2b7cce551dd2.png)

カプセルコリジョン：半径共有（bifrostGraphShape > pbd > iterate > Capsule_Collision）
![image_04_04b.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/0bbaf756-eb65-47ed-aa24-f0fc221464e0.png)

カプセルコリジョン：半径個別（bifrostGraphShape > pbd > iterate > Capsule_Collision）
![image_04_05.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/a2457ebb-f4c4-6df1-567b-f9b070bcd84a.png)

無限平面コリジョン（bifrostGraphShape > pbd > iterate > Infinite_Plane_Collision）
![image_04_06b.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/81c9b2c5-a775-f367-598c-35ba5b56aefe.png)

制約のイテレーション例（bifrostGraphShape > pbd > iterate）
![image_04_06a.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/7fd32c84-c6a8-2c35-3699-7672a158f09f.png)

---

前の記事

https://qiita.com/akasaki1211/items/9d22075df1a51be1ed4c

次の記事

https://qiita.com/akasaki1211/items/39440c98e54999673ecf