PBDベースで揺れモノ用の物理エンジンを実装した話

# 概要

本記事は[Cluster,Inc. Advent Calendar 2017](https://qiita.com/advent-calendar/2017/cluster)の6日目の記事です。

Unityちゃん付属のSpringBoneが微妙に使いにくかったため、使いやすくなるようにガッツリと作り直してみました。
左がUnityちゃん付属の物理エンジンで、右が今回作ったものです。
![capture.gif](https://qiita-image-store.s3.amazonaws.com/0/52576/a565f039-4bd1-3167-e918-d761d0240966.gif)


# 従来のものとの違い
## 改善された点
- 重力の影響を受けるようになった
- 1クラスで完結するようになり、セットアップが簡単になった
- ボーンの軸方向を意識せずに済むようになった


## 劣化した点
- 少し重くなった
- 細かい調整ができなくなった

# 使い方
物理演算したいGameObjectに `Pbd Spring Bone`をアタッチし、揺れモノの起点となるボーンを指定するだけで動きます。
![SnapCrab_NoName_2017-12-6_19-38-40_No-00.png](https://qiita-image-store.s3.amazonaws.com/0/52576/49cab1cc-86ae-26d9-edb7-4b7522df708d.png)


# 仕組み
基本的な考え方は[Position Based Dynamics](http://matthias-mueller-fischer.ch/publications/posBasedDyn.pdf)と呼ばれるものです。

GameObjectそれぞれの各点を質点と見なし、現在の位置/1フレーム前の位置を保持しています。

```c#
readonly Dictionary<GameObject, Vector3> defaultLocalPositions = new Dictionary<GameObject, Vector3>();
readonly Dictionary<GameObject, Quaternion> defaultLocalRotations = new Dictionary<GameObject, Quaternion>();
```

前回の位置と現在の位置から速度を求め、重力と減衰、形を維持しようとする力を加えます。

```c#
var velocity = (positions[current] - prevPositions[current]) / prevDeltaTime;
var positionForce =
    (parent.transform.TransformPoint(defaultLocalPositions[current]) - positions[current]) * spring;
velocity += positionForce * Time.deltaTime;
velocity += Vector3.down * gravity * Time.deltaTime;
velocity *= damp;
```

求めた速度を元に位置を更新します。

```c#
prevPositions[current] = positions[current];
positions[current] = positions[current] + velocity * Time.deltaTime;
```

質点がコライダーにめり込んでいた場合、めり込まない位置まで移動させます。

```c#
foreach (var sphereCollider in colliders)
{
    var colliderScale = Mathf.Max(
        Mathf.Abs(sphereCollider.transform.lossyScale.x),
        Mathf.Abs(sphereCollider.transform.lossyScale.y),
        Mathf.Abs(sphereCollider.transform.lossyScale.z));
    var colliderPos = sphereCollider.transform.TransformPoint(sphereCollider.center);

    if ((positions[current] - colliderPos).magnitude > pointSize + colliderScale * sphereCollider.radius)
    {
        continue;
    }

    positions[current] = colliderPos +
                            (positions[current] - colliderPos).normalized *
                            (pointSize + colliderScale * sphereCollider.radius);
}
```

ボーンの長さが一定になるよう、位置を補正してやります。

```c#
positions[current] = positions[parent] + (positions[current] - positions[parent]).normalized *
                        defaultLocalPositions[current].magnitude;
```

求めた位置を元に、親ボーンの角度を計算します。

```c#
parent.transform.rotation =
    Quaternion.FromToRotation(parent.transform.TransformDirection(defaultLocalPositions[current]),
        positions[current] - positions[parent]) * parent.transform.rotation;
```

自分自身のメソッドを再帰呼び出しして、子要素についても同様のことを繰り返します。

```c#
foreach (var child in current.Children())
{
    UpdateRecursive(child, current);
}
```

# コード
```c#
/**
MIT Lisence

Copyright (c) 2017 Cluster,Inc.

This software is released under the MIT License.
http://opensource.org/licenses/mit-license.php
*/

using System.Collections.Generic;
using Unity.Linq;
using UnityEngine;

public class PbdSpringBone : MonoBehaviour
{
    readonly Dictionary<GameObject, Vector3> defaultLocalPositions = new Dictionary<GameObject, Vector3>();
    readonly Dictionary<GameObject, Vector3> prevPositions = new Dictionary<GameObject, Vector3>();
    readonly Dictionary<GameObject, Vector3> positions = new Dictionary<GameObject, Vector3>();

    float prevDeltaTime;

    [SerializeField] float spring = 100;
    [SerializeField] float damp = 0.85f;
    [SerializeField] float gravity = 9.8f;
    [SerializeField] float pointSize = 0.02f;
    [SerializeField] SphereCollider[] colliders;
    [SerializeField] GameObject[] rootBones;

    public float Spring
    {
        get { return spring; }
        set { spring = value; }
    }

    public float Damp
    {
        get { return damp; }
        set { damp = value; }
    }

    public float Gravity
    {
        get { return gravity; }
        set { gravity = value; }
    }

    public SphereCollider[] Colliders
    {
        get { return colliders; }
        set { colliders = value; }
    }

    public GameObject[] RootBones
    {
        get { return rootBones; }
        set { rootBones = value; }
    }

    void Start()
    {
        foreach (var child in rootBones)
        {
            InitializeRecursive(child);
        }
        prevDeltaTime = Time.deltaTime;
    }

    void LateUpdate()
    {
        foreach (var child in rootBones)
        {
            UpdateRecursive(child);
        }
        prevDeltaTime = Time.deltaTime;
    }

    void OnDrawGizmos()
    {
        if (!Application.isPlaying)
        {
            return;
        }
        foreach (var child in rootBones)
        {
            if (child == null)
            {
                continue;
            }
            DrawGizmosRecursive(child);
        }
        foreach (var sphereCollider in colliders)
        {
            if (sphereCollider == null)
            {
                continue;
            }
            Gizmos.color = Color.blue;
            var scale = Mathf.Max(
                Mathf.Abs(sphereCollider.transform.lossyScale.x),
                Mathf.Abs(sphereCollider.transform.lossyScale.y),
                Mathf.Abs(sphereCollider.transform.lossyScale.z));
            var pos = sphereCollider.transform.TransformPoint(sphereCollider.center);
            Gizmos.DrawWireSphere(pos, scale * sphereCollider.radius);
        }
    }

    void InitializeRecursive(GameObject current)
    {
        positions[current] = current.transform.position;
        defaultLocalPositions[current] = current.transform.localPosition;
        prevPositions[current] = current.transform.position;

        foreach (var child in current.Children())
        {
            InitializeRecursive(child);
        }
    }

    void UpdateRecursive(GameObject current, GameObject parent = null)
    {
        if (parent != null)
        {
            var velocity = (positions[current] - prevPositions[current]) / prevDeltaTime;
            var positionForce =
                (parent.transform.TransformPoint(defaultLocalPositions[current]) - positions[current]) * spring;
            velocity += positionForce * Time.deltaTime;
            velocity += Vector3.down * gravity * Time.deltaTime;
            velocity *= damp;

            prevPositions[current] = positions[current];
            positions[current] = positions[current] + velocity * Time.deltaTime;

            foreach (var sphereCollider in colliders)
            {
                var colliderScale = Mathf.Max(
                    Mathf.Abs(sphereCollider.transform.lossyScale.x),
                    Mathf.Abs(sphereCollider.transform.lossyScale.y),
                    Mathf.Abs(sphereCollider.transform.lossyScale.z));
                var colliderPos = sphereCollider.transform.TransformPoint(sphereCollider.center);

                if ((positions[current] - colliderPos).magnitude > pointSize + colliderScale * sphereCollider.radius)
                {
                    continue;
                }

                positions[current] = colliderPos +
                                        (positions[current] - colliderPos).normalized *
                                        (pointSize + colliderScale * sphereCollider.radius);
            }

            positions[current] = positions[parent] + (positions[current] - positions[parent]).normalized *
                                    defaultLocalPositions[current].magnitude;
        }
        else
        {
            prevPositions[current] = positions[current];
            positions[current] = current.transform.position;
        }

        if (parent != null)
        {
            parent.transform.rotation =
                Quaternion.FromToRotation(parent.transform.TransformDirection(defaultLocalPositions[current]),
                    positions[current] - positions[parent]) * parent.transform.rotation;
                
        }

        foreach (var child in current.Children())
        {
            UpdateRecursive(child, current);
        }
    }

    void DrawGizmosRecursive(GameObject current, GameObject parent = null)
    {
        Gizmos.color = parent != null ? Color.yellow : Color.red;
        Gizmos.DrawWireSphere(positions[current], pointSize);

        if (parent != null)
        {
            Gizmos.DrawLine(positions[current], positions[parent]);
        }

        foreach (var child in current.Children())
        {
            DrawGizmosRecursive(child, current);
        }
    }
}
```

# ライセンス
© Unity Technologies Japan/UCL