Zenn
⚡

【Unity C#】PBD法でヒモを実装してみる

2021/08/25に公開
C#
Unity
physics
tech

Position Based Dynamics (PBD法)を利用して、ヒモを実装してみたので紹介します。
https://www.youtube.com/watch?v=nqln4MNeOeU

Position Based Dynamicsについて

https://www.4gamer.net/games/032/G003263/20140904089/

ヒモの表現方法について

複数の質点をつなぎ合わせて、ヒモを表現します。

以下の動画は、ヒモの質点を可視化したものになります。(灰色の丸が質点です)
https://www.youtube.com/watch?v=MzWQNBnH83I

PBD法を利用したヒモのシミュレーション

質点の位置から速度を計算する方法を軽く紹介します。
こちらと同様の内容はPosition Based Dynamicsの論文にも載っています。

推定位置p_iの計算

位置x_iと定常状態の伸びdから推定位置p_iを計算します。 (i = 0, 1, 2, ..., n - 1)

速度v_iの計算

推定位置p_iからv_iを計算します。

v_iは位置x_iから p_iへ向かう速度になっています。

アルゴリズム

論文に載っているアルゴリズムは以下になります。

論文 : https://matthias-research.github.io/pages/publications/posBasedDyn.pdf

ヒモの実装

質点の速度計算を実装に落とし込んでいきます。

ヒモの初期化

MonoBehaviourのAwakeなどのタイミングで、初期化を行います。

  1. 質点を複数配置し、これを定常状態とする(質点間の伸びがない状態)
  2. 隣り合う質点を拘束(Constraint)し、質点間の距離をdとして覚えておく

ヒモの更新

MonoBehaviourのUpdateなどのタイミングで、ヒモの質点の速度を更新します

  1. 質点位置 x_i から推定位置 p_i を求める (i = 0, 1, 2, ... , n-1)
  2. x_i と p_i から 速度v_iを求める
  3. 速度v_iを利用して、質点位置 x_i を更新する

ヒモの表示

ヒモ自体はParticleSystemのTrailsを使って表示しています

Unity C# での実装

ヒモの初期化

i番目の質点に関して、位置x[i]、推定位置p[i]、速度v[i]、質量m[i]を初期化します。
また、Constraintでつながれている質点間の伸びdも計算しておきます。

変数の定義

変数の定義

PBD法で使用するパラメータなどを定義します。

PBD_ParticleString.cs
[SerializeField] int n = 24; //質点の個数
[SerializeField, Range(0, 1)] float k = 0.5f; // バネの硬さ(Stiffness)
[SerializeField] float dt = 0.01f; // Delta Time
[SerializeField] Vector3 gravity = Vector3.zero; // 重力
[SerializeField] float kDamping = 0.03f; // Velocity Dampingで使用する定数
[SerializeField] Transform startPoint; // ヒモの開始点
[SerializeField] Transform endPoint; // ヒモの終了点
ParticleSystem.Particle[] particles;
Vector3[] x = null; // 質点の位置
Vector3[] v = null; // 質点の速度
Vector3[] p = null; // 質点の推定位置
bool[] isFixed = null;
float[] m = null; // 質量
Constraint[] constraints; // 拘束
Constraintクラス

Constraintクラス

Constraintは i番目とj番目の質点の拘束を表現するためのクラスです。
実装上都合が良いので、定常状態の伸びdも持たせておきます。

PBD_ParticleString.cs
public class Constraint
{
    public int i; // 質点その1
    public int j; // 質点その2
    public float d; // 定常状態の伸び

    public Constraint(int i, int j, float d)
    {
        this.i = i;
        this.j = j;
        this.d = d;
    }
}
Awakeメソッドでの初期化

初期化

MonoBehaviourのAwakeにて、質点とConstraintを初期化します。

PBD_ParticleString.cs
void Awake()
{
    // n個の質点の初期化
    p = new Vector3[n]; // 推定位置
    x = new Vector3[n]; // 位置
    v = new Vector3[n]; // 速度
    m = new float[n]; // 質量
    for (int i = 0; i < n; i++)
    {
        float t = (float)(i) / (n - 1);

        // 位置
        x[i] = Vector3.Lerp(startPoint.position, endPoint.position, t);

        // 速度
        v[i] = Vector3.zero; 

        // 質量 
        m[i] = 1f; 
    }

    // Constraintの初期化
    constraints = new Constraint[n - 1];
    for (int i = 0; i < n - 1; i++)
    {
        // 定常状態の伸びの計算
        var d = Vector3.Magnitude(x[i] - x[i + 1]);

        // 隣り合う質点を接続
        constraints[i] = new Constraint(i, i + 1, d);
    }

    // ヒモの両端の質点は固定
    isFixed = new bool[n];
    isFixed[0] = true;
    isFixed[n - 1] = true;

    // ParticleSystemの初期化
    InitializeParticle();
}

private void InitializeParticle()
{
    particleSystem = GetComponent<ParticleSystem>();

    var main = particleSystem.main;
    main.maxParticles = n;

    var emission = particleSystem.emission;
    emission.SetBursts(new[] { new ParticleSystem.Burst(0f, n) });
    particleSystem.Play();

    particles = new ParticleSystem.Particle[n];
    particleSystem.GetParticles(particles);
}

ヒモの更新

Updateメソッドにて、質点を更新していきます。

  1. 外力(重力)を速度v[i]に反映する
  2. 速度v[i]を補正(Damping)
  3. 速度v[i]を利用して、質点の位置x[i]や推定位置p[i]を更新
  4. 推定位置p[i]を利用して、速度v[i]を更新
  5. 質点位置(i = 0,1,2,...,n)をParticleSystemに反映
void Update()
{
    // 外力による速度変化
    for (int i = 0; i < n; i++)
    {
        v[i] += gravity * dt; 
        if (isFixed[i]) v[i] = Vector3.zero;
    }

    // Velocity Damping
    VelocityDamping(n, x, v, m, kDamping);

    // 位置の更新
    for (int i = 0; i < n; i++)
    {
        p[i] = x[i] + v[i] * dt;
        x[i] = p[i];
    }
    x[0] = startPoint.position;
    x[n - 1] = endPoint.position;

    // 速度の更新 (Project Constraints)
    for (int i = 0; i < constraints.Length; i++)
    {
        var c = constraints[i];
        var p1 = p[c.i];
        var p2 = p[c.j];
        float w1 = 1f / m[c.i]; // 質量の逆数
        float w2 = 1f / m[c.j]; // 質量の逆数
        float diff = Vector3.Magnitude(p1 - p2);
        var dp1 = -k * w1 / (w1 + w2) * (diff - c.d) * Vector3.Normalize(p1 - p2);
        var dp2 = k * w2 / (w1 + w2) * (diff - c.d) * Vector3.Normalize(p1 - p2);

        v[c.i] += dp1 / dt;
        v[c.j] += dp2 / dt;
    }

    // パーティクルへ位置反映
    SetParticle();
}

Damping

推定位置から求めた速度v[i]は、そのままでは安定しません。
https://www.youtube.com/watch?v=qVoF3l5Q-VY

ヒモを安定させるため、Damping という速度の補正を行います。

アルゴリズム(Damping)

論文では、Dampingは以下のように書かれています。

論文 : https://matthias-research.github.io/pages/publications/posBasedDyn.pdf

Dampingの実装 (Unity C#)

上記のDampingをUnity C#で実装したものは以下になります。

/// <summary>
/// 速度のDamping
/// </summary>
static void VelocityDamping(
    int n, // 質点の個数
    Vector3[] x, // 質点位置
    Vector3[] v, // 速度
    float[] m, // 質量
    float k // kDampingの値 (0 ~ 1)
) {
    var xcm = Vector3.zero;
    var vcm = Vector3.zero;
    var totalMass = 0f;
    for (int i = 0; i < n; i++)
    {
        xcm += x[i];
        vcm += v[i];
        totalMass += m[i];
    }
    xcm /= totalMass;
    vcm /= totalMass;

    var L = Vector3.zero;
    var I = new SquareMatrix(3);
    var rs = new Vector3[n];
    for (int i = 0; i < n; i++)
    {
        Vector3 r = x[i] - xcm;
        rs[i] = r;

        var R = new SquareMatrix(3);
        R[0, 1] = r[2];
        R[0, 2] = -r[1];
        R[1, 0] = r[2];
        R[1, 2] = -r[0];
        R[2, 0] = -r[1];
        R[2, 1] = r[0];

        L += Vector3.Cross(r, m[i] * v[i]);
        I += R * R.Transpose() * m[i];
    }

    Vector3 omega = I.Inverse() * L;
    for (int i = 0; i < n; i++)
    {
        Vector3 deltaV = vcm + Vector3.Cross(omega, rs[i]) - v[i];
        v[i] += k * deltaV;
    }
}

UnityC#には3x3の行列計算を行うクラスが無いため、
SquareMatrixクラスという自前の行列クラスを利用しています。

SquareMatrixクラスの実装
SquareMatrix.cs
using UnityEngine;

/// <summary>
/// N次正方行列
/// </summary>
[System.Serializable]
public class SquareMatrix
{
    [SerializeField] private double[] m;
    [SerializeField] private int n; 
    [SerializeField] private double det;

    public int N => n;
    public double Det => det; // 行列式

    public double this[int i]
    {
        get => m[i];
        set => m[i] = value;
    }

    public double this[int i, int j]
    {
        get => m[i * n + j];
        set => m[i * n + j] = value;
    }

    public SquareMatrix(int n)
    {
        this.n = n;
        m = new double[n * n];
    }

    public SquareMatrix(int n, double[,] values)
    {
        this.n = n;
        m = new double[n * n];
        for (int j = 0; j < n; j++)
        {
            for (int i = 0; i < n; i++)
            {
                this[i, j] = values[i, j];
            }
        }
    }

    public SquareMatrix(int n, double[,] values, double det)
    {
        this.n = n;
        this.det = det;
        m = new double[n * n];
        for (int j = 0; j < n; j++)
        {
            for (int i = 0; i < n; i++)
            {
                this[i, j] = values[i, j];
            }
        }
    }

    public SquareMatrix(Vector3 rhs)
    {
        n = 3;
        this.m = new double[n];
        for (int i = 0; i < n; i++)
        {
            this[i] = rhs[i];
        }
    }

    /// <summary>
    /// 転置行列
    /// </summary>
    public SquareMatrix Transpose()
    {
        var matrix = new SquareMatrix(this.n);
        for (int i = 0; i < n; i++)
        {
            for (int j = 0; j < n; j++)
            {
                matrix[j, i] = this[i, j];
            }
        }

        return matrix;
    }

    /// <summary>
    /// 逆行列
    /// </summary>
    public SquareMatrix Inverse()
    {
        var m = new double[n, n];
        for (int i = 0; i < n; i++)
        {
            for (int j = 0; j < n; j++)
            {
                m[i, j] = this[i, j];
            }
        }

        GaussianElimination(n, m, out var det, out double[,] m2, out var I);
        return new SquareMatrix(n, I, det);
    }


    /// <summary>
    /// 掃き出し法(ガウスの消去法)
    /// </summary>
    /// <param name="n">行数・列数</param>
    /// <param name="m0">元の行列</param>
    /// <param name="m">掃き出し法の結果(階段形)</param>
    /// <param name="I">行列m0の逆行列</param>
    public static void GaussianElimination(
        int n, 
        double[,] m0, 
        out double det, 
        out double[,] m,
        out double[,] I)
    {
        m = (double[,])m0.Clone();

        I = SquareMatrix.Identity(n);

        double diag = 1; // 対角成分の乗算

        bool[,] seen = new bool[n, n];
        for (int targetX = 0; targetX < n; targetX++)
        {
            int srcY = -1; // 0ではない要素
            bool findNonZero = false;  // 0ではない要素を見つけた
            for (int y = 0; y < n; y++)
            {
                if (seen[targetX, y]) continue;

                // 0 ではない要素を探す
                if (m[targetX, y] == 0f) continue;
                srcY = y;

                for (int xi = targetX; xi < n; xi++)
                {
                    seen[xi, srcY] = true;
                }

                findNonZero = true;
                diag *= m[targetX, y];
                break;
            }

            if (!findNonZero) 
            {
                diag *= 0.0;
            }
            else
            {
                // 見ている行の左端が1になるように、行を割る
                DivideRow(n, I, srcY, m[targetX, srcY]);
                DivideRow(n, m, srcY, m[targetX, srcY]);

                for (int yi = 0; yi < n; yi++)
                {
                    if (yi == srcY) continue;

                    // 見ている行をk倍して、他の行に足す
                    double k = -m[targetX, yi];
                    AddRow(n, I, srcY, yi, k);
                    AddRow(n, m, srcY, yi, k);
                }
            }

        }

        det = diag;
    }

    public static Vector3 operator* (SquareMatrix lhs, Vector3 rhs)
    {
        Vector3 v = new Vector3();
        for (int i = 0; i < lhs.n; i++)
        {
            float sum = 0f;
            for (int j = 0; j < lhs.n; j++)
            {
                sum += (float)lhs[j, i] * rhs[j];
            }
            v[i] = sum;
        }
        return v;
    }

    /// <summary>
    /// 単位行列
    /// </summary>
    /// <param name="n"></param>
    private static double[,] Identity(int n)
    {
        var m = new double[n, n];
        for (int i = 0; i < n; i++)
        {
            m[i, i] = 1;
        }
        return m;
    }


    /// <summary>
    /// src行目をk倍してdst行目に足す
    /// </summary>
    static void AddRow(int n, double[,] m, int src, int dst, double k)
    {
        for (int x = 0; x < n; x++)
        {
            m[x, dst] += k * m[x, src];
        }
    }

    /// <summary>
    /// src行目をkで割る
    /// </summary>
    static void DivideRow(int n, double[,] m, int y, double k)
    {
        for (int x = 0; x < n; x++)
        {
            m[x, y] /= k;
        }
    }

    
    public static SquareMatrix operator *(SquareMatrix lhs, float rhs)
    {
        var matrix = new SquareMatrix(lhs.n);
        int n = lhs.n;

        // matrixの(i, j)要素の計算
        for (int j = 0; j < n; j++)
        {
            for (int i = 0; i < n; i++)
            {
                matrix[i, j] = lhs[i, j] * rhs;
            }
        }
        return matrix;
    }

    public static SquareMatrix operator /(SquareMatrix lhs, float rhs)
    {
        var matrix = new SquareMatrix(lhs.n);
        int n = lhs.n;

        // matrixの(i, j)要素の計算
        for (int j = 0; j < n; j++)
        {
            for (int i = 0; i < n; i++)
            {
                matrix[i, j] = lhs[i, j] / rhs;
            }
        }
        return matrix;
    }

    public static SquareMatrix operator *(SquareMatrix lhs, double rhs)
    {
        var matrix = new SquareMatrix(lhs.n);
        int n = lhs.n;

        for (int j = 0; j < n; j++)
        {
            for (int i = 0; i < n; i++)
            {
                matrix[i, j] = lhs[i, j] * rhs;
            }
        }
        return matrix;
    }

    public static SquareMatrix operator +(SquareMatrix lhs, SquareMatrix rhs)
    {
        if (lhs.n != rhs.n)
            throw new System.Exception("operator+ : invalid matrix size");

        var matrix = new SquareMatrix(lhs.n);
        int n = lhs.n;

        for (int j = 0; j < n; j++)
        {
            for (int i = 0; i < n; i++)
            {
                matrix[i, j] = lhs[i, j] + rhs[i, j];
            }
        }
        return matrix;
    }

    public static SquareMatrix operator *(SquareMatrix lhs, SquareMatrix rhs)
    {
        if (lhs.n != rhs.n)
            throw new System.Exception("operator* : invalid matrix size");

        var matrix = new SquareMatrix(lhs.n);
        int n = lhs.n;

        for (int j = 0; j < n; j++)
        {
            for (int i = 0; i < n; i++)
            {
                double sum = 0f;
                for (int k = 0; k < n; k++)
                {
                    sum += lhs[k, j] * rhs[i, k];
                }
                matrix[i, j] = sum;
            }
        }
        return matrix;
    }
}

参考情報

https://www.4gamer.net/games/032/G003263/20140904089/
https://qiita.com/doRA9876/items/0f2c79204f2412c0f7a3
https://github.com/nobuo-nakagawa/cedec2017/blob/master/cedec2017.pdf

Discussion