【Unity】シェイプマッチング弾性体の実装

[CYBIRD Advent Calendar 2024](https://qiita.com/advent-calendar/2024/cybird) 15日目担当の@cy-tatsuya-sakaiです。
14日目は@moffunnyaさんの「[Terraformで複数CodeBuildの複数環境変数を管理したい](https://qiita.com/moffunnya/private/9327bbeb56e87fbc6716)」でした。

# はじめに
趣味で作ったゲームにて、シェイプマッチング法による弾性体を実装したので紹介します！

# 作ったゲーム
https://unityroom.com/games/morphing_masashi

ぷにぷに物体の形を変えて進むアクションゲームです。お暇なときにどうぞ:bow_tone1:


# 資料
https://cedil.cesa.or.jp/cedil_sessions/view/688

https://www.siliconstudio.co.jp/rd/presentations/

シリコンスタジオ様の CEDEC2011「コンピュータ・グラフィクス関連の最新論文紹介　～Shape Matching法とその周辺技術～」を参照して実装しました。感謝:raised_hands_tone1:

また、サンプルのUnityプロジェクトを作成しました。

https://github.com/cy-tatsuya-sakai/ShapeMatchingTest/tree/main

シーンは2つ準備しました。
* Sample001：シンプルな2Dシェイプマッチング実装サンプル
* Sample002：作ったゲームのコード整理したサンプル

併せてご覧ください。

# シェイプマッチング
自由移動する現在の形状の各頂点を、元形状に向かって動かすことで弾性体を表現する手法です。
![sss.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/214438/8a1f4340-c4f6-2e7e-773d-c2bb6f9181c1.gif)

現在の各頂点から元形状の移動・回転を算出し目標にします。

元形状は重心を原点にしておけば、移動は同じく重心の計算（現在の各頂点を足して割る）でOK。なので、回転をどう算出するかがこの手法の肝となります。

## 回転行列を計算する
シェイプマッチングでは、回転行列を計算して元形状を回転します。
2Dの回転行列は以下。

```math
R\theta = 
\begin{bmatrix}
cos\theta & -sin\theta \\
sin\theta & cos\theta
\end{bmatrix}
```

この回転行列をベクトルに乗算すると、ベクトルが回転します。
```cs
var vec = new float2(1, 0);

float rad = math.radians(90);
var mtx = new float2x2(
    math.cos(rad), -math.sin(rad),
    math.sin(rad),  math.cos(rad)
);

// 90°回転する (0, 1)
vec = math.mul(mtx, vec);
```

2Dシェイプマッチングの場合、この回転行列に解析解が存在するそうです。
資料から計算式を書き写してみます。

---
現形状における、重心から各頂点へのベクトルを $p_i$
元形状における、重心から各頂点へのベクトルを $q_i$

ベクトルの各要素を
$p_i = (a_i, b_i)$
$q_i = (c_i, d_i)$

としたとき、

```math
回転行列R = \frac{1}{\mu_r}\sum_im_i
\begin{bmatrix}
a_ic_i + b_id_i & a_id_i - b_ic_i \\
b_ic_i - a_id_i & a_ic_i + b_id_i
\end{bmatrix}
```
```math
正規化定数\mu_r = \sqrt{\Bigl(\sum_im_i(a_ic_i + b_id_i)\Bigr)^2 + \Bigl(\sum_im_i(a_id_i - b_ic_i)\Bigr)^2}
```
```math
m_i : i番目の頂点の質量
```

で回転行列を計算できる。

---

とのことです。…ちょっと噛み砕いてみます…！
### 回転行列の計算の中身
簡単に考えるために、質量は無視して…行列の各要素を見てみます。
各要素にはどんな値が入るでしょうか？

```math
\sum_i
\begin{bmatrix}
a_ic_i + b_id_i & a_id_i - b_ic_i \\
b_ic_i - a_id_i & a_ic_i + b_id_i
\end{bmatrix}
```

まずは1列目。
左上の$a_ic_i + b_id_i$から。これは[内積](http://marupeke296.com/COL_Basic_No1_InnerAndOuterProduct.html#:~:text=Dot%20%3D%20v1%E3%83%BBv2%20%3D%20x1*x2%20%2B%20y1*y2%20%3D%20%7Cv1%7C%7Cv2%7Ccos(%CE%B8))です。
内積の性質から、$|p_i||q_i|cos\theta$が入ります。

次に左下の$b_ic_i - a_id_i$。これは[2D外積](http://marupeke296.com/COL_Basic_No1_InnerAndOuterProduct.html#:~:text=v1%C3%97v2%3D%20x1*y2%2Dx2*y1%20%3D%20%7Cv1%7C%7Cv2%7Csin(%CE%B8))です。
外積の性質から、$|p_i||q_i|sin\theta$が入ります。

```math
\sum_i
\begin{bmatrix}
|p_i||q_i|cos\theta & a_id_i - b_ic_i \\
|p_i||q_i|sin\theta & a_ic_i + b_id_i
\end{bmatrix}
```

2列目。よく見ると、1列目の要素を入れ替えて、$sin\theta$の符号を反転した値が入っているだけです。

```math
\sum_i
\begin{bmatrix}
|p_i||q_i|cos\theta & -(|p_i||q_i|sin\theta) \\
|p_i||q_i|sin\theta & |p_i||q_i|cos\theta
\end{bmatrix}
```

（既にそれっぽくなってきました）

そこに正規化定数での除算$\frac{1}{\mu_r}$が入ります。
$\mu_r$の中身もよく見ると行列と同じ内積と外積で、
```math
\mu_r = \sqrt{\Bigl(\sum_i|p_i||q_i|cos\theta\Bigr)^2 + \Bigl(\sum_i|p_i||q_i|sin\theta\Bigr)^2}
```
ピタゴラスの定理 $cos^2\theta + sin^2\theta = 1$から、これはベクトルの長さ計算です。
```math
\mu_r = \sqrt{\Bigl(\sum_i|p_i||q_i|\Bigr)^2} = \sum_i|p_i||q_i|
```

最後に行列をベクトルの長さで割って…
```math
\frac{1}{\sum_i|p_i||q_i|}
\sum_i
\begin{bmatrix}
|p_i||q_i|cos\theta & -(|p_i||q_i|sin\theta) \\
|p_i||q_i|sin\theta & |p_i||q_i|cos\theta
\end{bmatrix}
 = 
 \begin{bmatrix}
cos\theta & -sin\theta \\
sin\theta & cos\theta
\end{bmatrix}
```
回転行列を計算できました！
内積、外積を正規化してcos, sinを算出しているようでした。簡単で素晴らしい。

### （余談）回転行列のイメージ
ところで、何で回転行列を乗算したらベクトルが回転するんじゃい！と思う方はいらっしゃいますでしょうか？
新人の頃、先輩プログラマの方に教えてもらって、ふんわり納得できた説明を思い出したので書いておきます。

自分が教えてもらったのは、行列には**軸が入っている**んだ、という説明でした。

つまり、こう。
```math
R\theta = 
\begin{bmatrix}
\color{red}{cos\theta} & \color{green}{-sin\theta} \\
\color{red}{sin\theta} & \color{green}{cos\theta}
\end{bmatrix}
= \begin{bmatrix}
\color{red}{X軸(cos\theta, sin\theta)} & \color{green}{Y軸(-sin\theta, cos\theta)}
\end{bmatrix}
```
$\theta = 0°$なら、X軸は$(cos(0°), sin(0°)) = (1, 0)$。
$\theta = 90°$なら、X軸は$(cos(90°), sin(90°)) = (0, 1)$。
![uuu.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/214438/38332142-543e-77da-b233-4853d9d1b07c.gif)

回転行列には、回転した後の軸の方向ベクトルが入っている！
そう思うと、何だか回転しそうな気がしてきますね？

## 頂点を移動する
移動・回転を算出して元形状を動かしたら、あとはそこに向かって、現在の頂点を移動するだけです。

これは位置ベースで計算するのが簡単です。
頂点座標を直接動かして、その後に頂点の移動量から速度を決定する積分方法です。

位置ベース物理については、以前書いた記事があるのでそちらをご覧ください。
https://qiita.com/cy-tatsuya-sakai/items/a028f2a639083786a5a0
（素人説明です…他にもっと良い説明有ると思います。探そう！）

## 実際のソースコード
最後にシェイプマッチングのコードを抜粋して貼ります。
詳細はサンプルプロジェクトSample001を動かしてご確認ください！

```頂点クラス.cs
public class Point
{
    public float2 position;     // 座標
    public float2 prevPosition; // 前フレームの座標
    public float2 velocity;     // 速度

    public Point(float2 pos)
    {
        position = prevPosition = pos;
        velocity = float2.zero;
    }

    /// <summary>
    /// 座標を更新
    /// </summary>
    public void UpdatePosition(float dt)
    {
        prevPosition = position;
        position += velocity * dt;
    }

    /// <summary>
    /// 速度を更新
    /// </summary>
    public void UpdateVelocity(float dt)
    {
        velocity = (position - prevPosition) * (1.0f / dt);
    }
}
```
```シェイプマッチング処理.cs
private void UpdateShape(float dt)
{
    // _points = 現在の形状の頂点
    // _shape  = 元形状の初期位置。重心からのベクトル

    // 重心を計算
    var center = float2.zero;
    foreach(var point in _points)
    {
        center += point.position;
    }
    center *= (1.0f / POINT_NUM);

    // 回転行列を計算
    var mtx = float2x2.zero;
    for(int i = 0; i < POINT_NUM; i++)
    {
        var p = _points[i].position - center;
        var q = _shape[i];

        var v = new float2(
            p.x * q.x + p.y * q.y,  // 内積 = cos
            p.y * q.x - p.x * q.y   // 外積 = sin
        );
        mtx.c0 += new float2( v.x, v.y);    // X軸
        mtx.c1 += new float2(-v.y, v.x);    // Y軸
    }
    {
        // 軸ベクトルを正規化する
        var v = mtx.c0;
        var u = math.sqrt(v.x * v.x + v.y * v.y);
        mtx = (u > 0.0f) ? mtx * (1.0f / u) : float2x2.identity;
        _debugMtx = mtx;
    }

    // 目標に向かって点群を移動する
    for(int i = 0; i < POINT_NUM; i++)
    {
        var pos = _points[i].position;
        var tgt = math.mul(mtx, _shape[i]) + center;    // 目標。初期位置を回転、平行移動する
        _debugGoal[i] = tgt;

        // 適当に目標との距離を縮める
        _points[i].position += (tgt - pos) * 0.1f;
    }
}
```
```シミュレーション全体の流れ.cs
void FixedUpdate()
{
    float dt = Time.fixedDeltaTime;

    // 座標を更新
    foreach(var point in _points)
    {
        point.UpdatePosition(dt);
    }

    // この辺～～～座標を変更する処理～～～

    // シェイプマッチング処理
    UpdateShape(dt);

    // 速度を更新
    foreach(var point in _points)
    {
        point.UpdateVelocity(dt);
    }

    // この辺～～～速度を変更する処理～～～
}
```

# ゲームへの実装
シェイプマッチングをどのようにゲームに組み込んだか、今回作ったゲームでの実装を紹介します。
なお、1週間ゲームジャム（遅刻）での実装です。参考程度でお願いします:pray:

詳細はサンプルプロジェクトSample002をご確認ください！

## 物理演算にシェイプマッチングを混ぜる
シェイプマッチングは作ったけど、衝突判定は作りたくない～！
物理エンジンにお任せしたい！位置ベースの計算結果をRigidbody2Dに反映する良き場所は有るかな？

### コルーチン`WaitForFixedUpdate`で物理演算の完了を待つ
[Unityのイベント関数の実行順序](https://docs.unity3d.com/ja/2022.3/Manual/ExecutionOrder.html)を眺めていたら、FixedUpdateから始まるPhysicsループの最後に`yield return WaitForFixedUpdate`の記述を発見。

どうやらコルーチンでコリジョン含め諸々の物理演算が完了した後に処理を挟めるっぽい？
まあ良いや、今回はここに書こう！
```思いつき.cs
void Awake()
{
    StartCoroutine(Simurate());
}

/// <summary>
/// シミュレーション
/// </summary>
private IEnumerator Simurate()
{
    var waitForFixedUpdate = new WaitForFixedUpdate();
    while(true)
    {
        yield return waitForFixedUpdate;

        // ここで諸々の物理演算後にRigidbody2Dの座標と速度弄れる説（想像）
    }
}
```
```Rigidbody2Dを握ってる位置ベース計算用の点.cs
// Rigidbody2Dを握ってる位置ベース計算用の点
[RequireComponent(typeof(Rigidbody2D))]
[DisallowMultipleComponent]
public class PointMass : MonoBehaviour
{
    [SerializeField] private Collider2D _collider;
    public new Collider2D collider => _collider;

    public Rigidbody2D body         { get; private set; }
    public double2     position     { get; set; }
    public double2     prevPosition { get; private set; }
    public double2     velocity     { get; set; }
    public double      invMass      { get; private set; }

    void Awake()
    {
        body = gameObject.GetComponent<Rigidbody2D>();
    }

    /// <summary>
    /// 計算前の初期化
    /// </summary>
    public void Prepare()
    {
        var pos = body.position;
        position = prevPosition = new double2(pos.x, pos.y);
        velocity = double2.zero;
        invMass  = 1.0 / math.max(body.mass, 0.0001);
    }

    /// <summary>
    /// 速度を更新
    /// </summary>
    public void UpdateVelocity(double dt)
    {
        velocity = (position - prevPosition) * (1.0 / dt);
    }

    /// <summary>
    /// 位置ベースの計算結果をRigidbody2Dに書き戻す
    /// </summary>
    public void UpdateBody()
    {
        body.position = new Vector2((float)position.x, (float)position.y);

        var vel = new Vector2((float)velocity.x, (float)velocity.y);
        body.AddForce(vel, ForceMode2D.Impulse);
    }
}
```
```コルーチン WaitForFixedUpdate の後で計算.cs
// Rigidbody2D握ってる点のリスト。中身は別の場所でAddされる
private List<PointMass> _pointMassList = new();

/// <summary>
/// シミュレーション
/// </summary>
private IEnumerator Simurate()
{
    var waitForFixedUpdate = new WaitForFixedUpdate();
    while(true)
    {
        yield return waitForFixedUpdate;

        double dt = Time.fixedDeltaTime;

        // シェイプマッチング処理の前に、現在のRigidbody2Dの座標で初期化する
        foreach(var point in _pointMassList)
        {
            point.Prepare();
        }

        // シェイプマッチング処理。PointMassのpositionを直接動かす
        ～～～

        // シェイプマッチングで動かした座標から速度を計算
        foreach(var point in _pointMassList)
        {
            point.UpdateVelocity();
        }

        // 座標、速度をRigidbody2Dに書き戻す
        foreach(var point in _pointMassList)
        {
            point.UpdateBody();
        }
    }
}
```

合ってるかどうかは確認してません！それっぽく動いたので良しとしました！

## その他
### シェイプマッチングの輪郭をバネで包む
シェイプマッチングはあくまで各頂点ごとの移動になるので、隣り合う頂点の影響を受けません。
今回は隣り合う頂点の動きを伝搬させたかったので、輪郭を[バネ](https://qiita.com/cy-tatsuya-sakai/items/a028f2a639083786a5a0#%E8%B7%9D%E9%9B%A2%E6%8B%98%E6%9D%9F)で繋いでみました。

シェイプマッチングのみ
![Animation_shapemathing_only.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/214438/d07e9ebf-7025-1bca-98d6-ffa310e936d6.gif)

シェイプマッチング + 輪郭バネ
![Animation.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/214438/f5d64799-f42c-aa89-3920-b9ff3b2d560b.gif)

（分かりやすいよう極端なパラメータ設定にしてます）
動きが増えて良い感じになった気がします！

### 描画メッシュを丸っこい見た目にする
ゲーム内でシェイプマッチングの頂点は、実際はこの位置にあります。
![a_1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/214438/ca387671-f4bb-57a1-4066-2be630c7a4bd.png)

コライダーを可視化するとこんな感じ。
![a_2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/214438/32b1eb85-0fa4-bb22-6edb-3c345157db4a.png)


ぷにっとした感じを出す & コライダーの半径分サイズを拡張するため、描画メッシュの頂点座標を補正しています。

#### 隣り合う頂点との加重平均を取る
柔らかそうな見た目にするため、頂点同士の加重平均を取って丸めてみました。
今回は、`中央/左/右`の3点を`4/3/3`の割合で混ぜてみました。
```cs
// verticesに初期状態のメッシュ頂点が入っている
// tmpに加重平均の計算結果が入る

// 頂点を丸める
var tmp = new Vector3[num];
for(int i = 0; i < num; i++)
{
    Vector3 p1 = vertices[i];
    Vector3 p2 = vertices[(i + 1) % num];
    Vector3 p3 = vertices[(i + num - 1) % num];
    tmp[i] = (p1 * 4 + p2 * 3 + p3 * 3) * 0.1f;
}
```
少し角が丸まりました。
![a_3.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/214438/9e31d9c5-84f9-ec6f-0e8f-73bfe6108232.png)

#### 重心から頂点方向に膨らませる
頂点を丸めたあと、重心から頂点への方向ベクトルで拡大します。
面（辺？）の法線での拡大も考えましたが、より丸みが出たこちらを採用しました。
```cs
// 丸めた頂点tmpをverticesに戻しています
// 0.5fはコライダの半径です…

for(int i = 0; i < num; i++)
{
    vertices[i] = tmp[i] + tmp[i].normalized * 0.5f;
}
```

![a_5.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/214438/ffc56c30-58aa-f309-07ee-dab73f44cf78.png)

特に不自然な感じもしなかったので、良しとしました！


# おわりに
シェイプマッチング楽しい！無事ゲームも出せて、とても満足しました:blush:
何かの参考になれば幸いです:bow_tone1:

明日の [CYBIRD Advent Calendar 2024](https://qiita.com/advent-calendar/2024/cybird) 16日目は、@alexkさんの「TyranoScriptのWKWebViewメモリー最適化とクラッシュ対策」です！お楽しみに！

# 参考
ゲームつくろー！内積と外積の使い方
http://marupeke296.com/COL_Basic_No1_InnerAndOuterProduct.html