Bifrostではじめる位置ベース物理シミュ 第3回 ~制約条件を追加する~

ハイどうもこんにちは！

[前回](https://qiita.com/akasaki1211/items/d7272303d44d4823b39d)に引き続き位置ベースなんちゃって物理で遊んでいきます。今回は導き出された位置を直接操作し、制約条件を加えていきます。

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

::: note
ところで、ここまで扱ってきた位置、速度、力などはすべて3次元ベクトルです。シレっとベクトルの足し引きなど扱ってきましたが、今回から正規化とか内積とかよう分らんものも出てきます。気が向いたら以下の記事などで理解を深めておくと良いかもしれません！スルーしてもBifrostグラフは組めます。たぶん！

🔗[ゲーム作りとかCGとかに関わる数学(初歩)① #数学 - Qiita](https://qiita.com/tsuchinokoman/items/938f62f62ac0e9bcb3bc)
:::

# 点と点の距離を一定に保つ

点と点の距離を保つといっても大きく分けて2種類あります。

- 2点のうち1点が固定　→　1点だけ動かせばいい
- 2点の両方がフリー　→　2点とも動かす必要あり

前者の方が簡単なのでまずは片方が固定の場合からいきます。

# 片方が固定の場合

補正したい点を$p$、固定されている点を$o$として、$p$と$o$の間の距離を$d$に保ちたいとします。もし間の距離がずれていた場合、次の式で$p$のみを補正し、指定した距離に引き戻すことができます。

```math
p' = o + \frac{p - o}{\| p - o \|} d
```

$\frac{p - o}{\| p - o \|}$は$o$から$p$へ向かうベクトルを正規化（長さを1に）したものです。正規化ベクトルに本来の長さを掛け、固定点の位置に足しています。方向だけは維持しつつ長さだけ強引に戻してしまう形になります。
![fig_03_01.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/5ebd54db-e03e-211d-72f5-b79beb02b10c.jpeg)

Bfrostグラフは[前回の最後のもの](https://qiita.com/akasaki1211/items/d7272303d44d4823b39d#%E4%BB%8A%E5%9B%9E%E3%81%AE%E3%82%B0%E3%83%A9%E3%83%95%E3%81%BE%E3%81%A8%E3%82%81)から始めます。Distance Constraintが追加した部分です。制約は絶対！なので計算箇所は最後のほうになります。速度更新の直前ですね。
![img_03_01a.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/55cec0f7-6810-9bf0-1c8a-73ae90766ef5.png)
▲bifrostGraphShape > pbd

![img_03_01b.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/9e771de9-15fc-40b3-344b-a7ba01f1c39d.png)
▲bifrostGraphShape（長さは外側で事前に計算しておきます）

固定されている点としてロケーターを新たに作成し、Parent Positionに接続しておきます。
![img_03_01c.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/e342bc43-0015-1344-f764-43b94984f533.png)
▲parentが固定点

再生すると、前回までの重力やバネなどの影響は生きたまま、固定点から長さを一定に保ち回転するような動きになりました。
![gif_03_01.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/88a37103-d05e-c36c-0783-f24fc1dbce28.gif)

実際に、この2点を連ねていくとなんだか見覚えのあるボーンダイナミクスが出来上がります。これについては[後の回](https://qiita.com/akasaki1211/items/39440c98e54999673ecf)で取り上げます。

::: note
先ほど、Bifrostグラフへの位置入力にdecomposeMatrixノードを挟みましたが、これは入力ロケーターのワールド位置座標を得るためです。ワールド位置を得るのはBifrostグラフ内でもできます。処理速度的な違いはあるかもしれませんがお好みでいいかと思います。

下の2種の結果は同じです。
![img_03_01d.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/d31e0c80-7bbd-0706-f624-901929d74406.png)
![img_03_01e.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/c62105e8-90e5-ebe0-3d26-a2256bd3be11.png)
:::

# 両方がフリーの場合

このケースはこちら（[Position Based Dynamics](https://matthias-research.github.io/pages/publications/posBasedDyn.pdf)）の論文から計算式を拝借してきます。

2つの点$p_1, p_2$の間の距離を$d$に保ちたいとします。$\Delta p_1, \Delta p_2$がこれから求めたい各点の移動量で、それぞれどれだけ動かせば$d$になるか、という式です。　

```math
\displaylines{
\Delta p_1 = - \frac{w_1}{w_1 + w_2}(\|p_1 - p_2\| - d) \frac{p_1 - p_2}{\|p_1 - p_2\|} \\\ 
\Delta p_2 = + \frac{w_2}{w_1 + w_2}(\|p_1 - p_2\| - d) \frac{p_1 - p_2}{\|p_1 - p_2\|} \\\
w_i = \frac{1}{m_i}
}
```

$\frac{w_1}{w_1 + w_2}, \frac{w_2}{w_1 + w_2}$は正規化済み（合計を1にしてある）ウェイトです。2点の質量が同じだとするとここはどちらも$\frac{1}{2}$でいいのですが、重さが違う場合は重い方がウェイトが小さくなり動きにくくなります。$(\\|p_1 - p_2\\| - d)$ は今の2点間の距離と$d$の差分、$\frac{p_1 - p_2}{\\|p_1 - p_2\\|}$は正規化したベクトルです。距離差分を重さに基づいて2点で分け合い、それぞれがお互いの方向に移動する、ということになります。
![fig_03_02.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/17e81b13-aff2-9062-2dab-a542cdfdc955.jpeg)

2点がフリーなので、まずこれまでの処理を適用した2点を用意します。[前回の最後のBifrostグラフ](https://qiita.com/akasaki1211/items/d7272303d44d4823b39d#%E4%BB%8A%E5%9B%9E%E3%81%AE%E3%82%B0%E3%83%A9%E3%83%95%E3%81%BE%E3%81%A8%E3%82%81)から始め、pbd内のノードをまるっと複製して2点分の計算が出来るようにしておきます。position, velocity, massだけは入力を共有せずしっかり分けておきます。
![img_03_02a.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/a2f2fc3e-1591-7efe-a7c2-6cb11861b844.png)
▲bifrostGraphShape > pbd（黄色い枠が増やした部分）

![img_03_02b.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/e667d965-ec88-3d65-f2f8-37c986db0b87.png)
▲bifrostGraphShape（黄色い枠が増やした部分）

![img_03_02c.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/5796e383-cc5c-20ec-88e0-62721d37b3df.png)

再生するとこのような状態になります。どちらの点も初速度を0、dampingを0.1、elasticityを30、stiffnessを0、**massだけ差をつけていて、左を1.0、右を0.3**、にしてあります。当然ですが複製しただけなので2点間の距離は変化します。
![gif_03_02a.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/6b93e877-b243-fff8-4c63-4a2db3f58efb.gif)

ここから前述の式を元に距離制約を追加します。
まずpbdの外側で2点間の距離$d$とウェイト$\frac{1}{m_1}, \frac{1}{m_2}$を事前計算します。
![img_03_02d.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/45cad930-2e11-3182-28cf-732e880d8d0a.png)
▲bifrostGraphShape

制約は絶対！なので2点の位置更新が終わったところで距離制約を挟み込みます。追加したのはDistance_Constraint_2というコンパウンドです。
![img_03_02e.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/8438e308-a6e9-d9c8-6719-e39d678c009f.png)
▲bifrostGraphShape > pbd

Distance_Constraint_2の中身はこのようになってます。前述の式と、得られた$\Delta p_1, \Delta p_2$を元の位置に足して更新するところまでが含まれています。
![img_03_02f.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/73d65958-67bc-d565-3a84-dd97c6a02b8d.png)
▲bifrostGraphShape > pbd > Distance_Constraint_2

再生すると、このようになりました。重力、バネ、質量の差、距離制約がすべて表現できていそうです！
![gif_03_02b.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/3b5d58fc-0ece-9d18-2697-db5c7b5e8ccc.gif)

今回の「制約条件」は位置ベースの便利なところが最も現れているところな気がします。物理的な正しさからは離れていきますが、単純で扱いやすいのがメリットです。次回はコリジョンを追加します！

# 今回のグラフまとめ

### 片方が固定

bifrostGraphShape
![img_03_03a.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/13a5ad86-b435-7b91-15bd-9675a23222d8.png)

2つのベクトル間の距離を計算（bifrostGraphShape > distance）
![img_03_03b.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/c6b39821-218b-09b8-fc90-e5c4a650875c.png)

bifrostGraphShape > pbd
![img_03_03c.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/c2671cc8-b0f8-9c34-b972-e519f8c38128.png)

距離制約①：片方が固定（bifrostGraphShape > pbd > Distance_Constraint）
![img_03_03d.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/acb888f9-71ba-0cc0-10da-b870539ec54b.png)

### 両方がフリー

bifrostGraphShape
![img_03_04a.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/ee71342a-8049-61f7-b58d-4ebddba8f6c1.png)

質量からウェイトを計算（bifrostGraphShape > mass_to_weight）
![img_03_04b.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/61d609fb-0b9a-2083-3067-5121e0e584e6.png)

bifrostGraphShape > pbd
![img_03_02e.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/8438e308-a6e9-d9c8-6719-e39d678c009f.png)

距離制約②：両方がフリー（bifrostGraphShape > pbd > Distance_Constraint_2）
![img_03_04d.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/3462b483-eb51-a75a-53fa-adb56a6e81fe.png)

2つのウェイトの合計が1になるように正規化（bifrostGraphShape >  pbd > Distance_Constraint_2 > normalize_weight）
![img_03_04c.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/aa039662-f6e9-62e0-5d4c-59d350655c63.png)

::: note
だんだんコンパウンドが溜まってきたので、今後も使うものはPublishして保存しておくと便利かもしれません！
参考🔗：[#09 コンパウンドにしてチームで共有! | プラチナゲームズ公式ブログ](https://www.platinumgames.co.jp/magazine/detail/?id=2_18269)

![img_03_05.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211093/f754b1f5-632e-3f06-8ed8-2c3be1ce326a.png)
:::

---

前の記事

https://qiita.com/akasaki1211/items/d7272303d44d4823b39d

次の記事

https://qiita.com/akasaki1211/items/971ccad3c50d88749ba1