CYBIRD Advent Calendar 2022 17日目担当の@cy-tatsuya-sakaiです。
16日目は@utamakuraさんの「MySQLデータを復活させる方法」でした。
サーバーサイドの方々に守られながら日々を生きています…！

はじめに

突然ですが自分、Position Based Dynamics(PBD) 大好きなんです。
この動画とかトキめきます。

すごく雑な説明をすると、点をバネで繋ぐとロープやら布やら色んな物体を表現できる！実装が簡単！楽しい〜！な物理シミュレーション手法の一種です。
CEDEC2011の講演が分かりやすかった気がします。

CEDEC2011…はい、最近は全く情報を追っていませんでした。

そんな状態で今年CEDEC2022でのPBDの講演を見たら、

PBDからXPBD、SubstepXPBDと発展していて、品質が向上している
- →知らなかった…
ソースコードをGithubで公開中
- →すごい…！

これは実装確認せねば！ということで今回はXPBD、SubstepXPBDのコードをUnityで写経してみようと思います。
解説は出来ませんが、PBDとの実装の違いを眺めていければ。

参考資料

中川展男さん：Position Based Dynamics Onramp: 物理シミュレーション関連の最新論文実装
https://cedil.cesa.or.jp/cedil_sessions/view/2512
https://github.com/nobuo-nakagawa/cedec2022

ソースコード
https://github.com/nobuo-nakagawa/xpbd/blob/master/src/xpbd.cpp
https://github.com/nobuo-nakagawa/xpbd2022/blob/main/src/xpbd.cpp
https://github.com/nobuo-nakagawa/smallsteps/blob/main/src/smallsteps.cpp

写経したUnityプロジェクト

今回は単純な2Dのロープを作ってみました。TestPBDシーンを再生すると実行できます。
TestPBD.cs先頭の#defineをコメントアウト、アンコメントすると動作を変更できます。

両方コメントアウト：PBD
#define XPBDをアンコメント：XPBD
#define XPBD, #define SUBSTEPをアンコメント：SubstepXPBD

あとマウスでロープの先端を掴めます。

PBD

写経の前に、まず通常のPBDを実装してみます。
ポイントはざっくり以下の3点。

質点
距離拘束
拘束の反復処理

順番に見ていきます。

質点

MassPoint.cs
PBDで動かす点。いわゆるRigidbodyみたいなクラスです。
回転は持たず位置と質量だけです。

ポイントは、速度を現在の座標 - 前フレームの座標との差で算出するところ。

velocity = (position - prevPosition) * (1.0f / dt);

そして座標を更新する際に、前フレームの座標を保存しておきます。

prevPosition = position;
position += velocity * dt;

こうすることで、後々の拘束計算で直接座標を動かした際、速度も自動的に調節される仕組みです。
Verlet(ベルレ)積分と言うらしいです。

距離拘束

DistanceConstraint_PBD.cs
質点を一定の形状に保つ拘束(コンストレイント)。基本の距離拘束です。
Rigidbodyに対するHingeJointやSpringJointのようなものでしょうか。
2点間の距離が一定になるよう座標を動かします。

キモとなる処理は以下。

DistanceConstraint_PBD.cs

public void SolvePosition(float _)
{
    var sumMass = _a.invMass + _b.invMass;
    if(sumMass <= 0.0f) { return; }

    var v = _b.position - _a.position;
    var d = v.magnitude;
    if(d <= 0.0f) { return; }

    var constraint  = d - length;   // 目標の距離
    v = v * constraint / (d * sumMass) * stiffness;

    _a.position += v * _a.invMass;
    _b.position -= v * _b.invMass;
}

_a, _bが操作対象の質点、lengthが保ちたい距離、stiffnessが拘束の固さです。
stiffnessは0～1の係数で、1で最も固くなります。

やってることは単純で、_a, _b間の距離がlengthになるよう座標を移動しているだけです。
質量による移動量の分配が入っていて分かりにくいので、少し分解してみます。

移動量の計算

    var v = _b.position - _a.position;    // 2点間の差分ベクトル
    var d = v.magnitude;                  // 2点間の距離
    if(d <= 0.0f) { return; }             // ゼロ除算回避

    var constraint  = d - length;   // 目標の距離
    // v = v * constraint / d * stiffness; を更に分解
    v /= d;             // 差分ベクトルを正規化して
    v *= constraint;    // 目標とする距離になるようベクトルの長さを変更する
    v *= stiffness;     // 目標に近づく具合を調整する。stiffnessが小さいほどゆっくり近づく = 軟らかい動きになる

    _a.position += v * 0.5f;    // 2点それぞれの座標を動かす
    _b.position -= v * 0.5f;    //

これはそのままでしたね。2点間のベクトルの長さをlengthに近づける値d - lengthにした上で、2点それぞれを移動させています。

質量による移動量の分配

    var sumMass = _a.invMass + _b.invMass;    // 質量の逆数の合計
    if(sumMass <= 0.0f) { return; }           // ゼロ除算回避。合計がゼロの場合、どちらの点も質量無限 = 動かない物体とみなしてリターン

    v /= sumMass;    // 質量の逆数の合計で割る

    _a.position += v * _a.invMass;    // 質量に応じて2点を動かす。重い物体の方が移動量が減る
    _b.position -= v * _b.invMass;    //

質量の逆数を使っているのがミソです。ちょっと代入して確認してみましょう。

    // _a, _bともに質量1の場合
    var sumMass = 1.0f + 1.0f;  // 2

    v /= sumMass;               // 2で割る = 0.5

    _a.position += v * 1.0f;    // 0.5 * 1 = 0.5
    _b.position -= v * 1.0f;    // 0.5 * 1 = 0.5

    // _aの質量1, _bの質量2の場合
    var sumMass = 1.0f + 0.5f;  // 1.5

    v /= sumMass;               // 1.5で割る = 0.6666666...

    _a.position += v * 1.0f;    // 0.666666 * 1.0 = 0.666666...
    _b.position -= v * 0.5f;    // 0.666666 * 0.5 = 0.333333...

質量が大きいほど移動量が少なくなるよう上手く分配できていますね！

この拘束をどの質点間に行うかの繋ぎ方によって、ロープや布などの物体を表現するわけですね。

反復計算

実は拘束の計算は一回では収束しません。
1つの質点が複数の質点と繋がれていた場合に、一度動かした座標が別の拘束で再度動かされて、どれかの拘束は意図しない結果になります。
そのため、計算が収束するまで = 大体収束するであろう適当な回数だけ処理を繰り返します。
CEDEC2017の公演によると、ざっくり2種類の反復方法があるようです。

ガウスザイデル法

拘束を順に実行する方法。これはそのままで、適当な回数計算を繰り返すだけです。
先に動かした座標が次の拘束計算で使われるので、収束が早くなります。
同時に、計算順序によって結果に偏りが出る場合があります。

for(int i = 0; i < iter; i++) // 適当な回数計算
{
    foreach(var constraint in _contraintList)
    {
        constraint.SolvePosition(dt);
    }
}

ヤコビ法

拘束をまとめて実行する方法。拘束の座標移動で一時変数を動かすようにして、

_a.nextPosition += v * _a.invMass;
_b.nextPosition -= v * _b.invMass;

全ての拘束計算が終わった後に座標移動を反映します。
計算の偏りは無くなりますが、収束は遅くなります。

foreach(var massPoint in _massPointList)
{
    massPoint.nextPosition = massPoint.position;    // 一時変数を現在の座標で初期化しておく
}

for(int i = 0; i < iter; i++) // 適当な回数計算
{
    foreach(var constraint in _contraintList)
    {
        constraint.SolvePosition(dt);    // SolvePosition()では、一時変数nextPositionを動かす
    }

    foreach(var massPoint in _massPointList)
    {
        massPoint.position = massPoint.nextPosition;    // 全ての拘束計算が終わった後に現在の座標を更新する
    }
}

…実はヤコビ法は初めて実装しました、多分こんな感じです…！
概ね動いてそうではありましたが、XPBDで奇数回反復したときに計算が不安定になる、SubstepXPBDで反復回数を増やすと不安定になる現象を確認したので、何かを間違えているかも知れません…
写経プロジェクトではガウスザイデル法にしてあります。

シミュレーション全体の流れ

TestPBD.cs
ここまでで必要な要素は揃いました。後は順に実行するだけです。
シミュレーション全体の流れは以下のようになります。

void Start()
{
    // 質点と拘束のセットアップ
}

void FixedUpdate()
{
    float dt = Time.fixedDeltaTime;
    Simulate(dt, _step/*適当な回数*/);
}

private void Simulate(float dt, int iter)
{
    // 座標を更新
    foreach(var massPoint in _massPointList)
    {
        massPoint.prevPosition = position;
        massPoint.position += velocity * dt;
    }

    // コリジョンや座標移動するならこの辺り？？

    // 拘束の反復処理
    for(int i = 0; i < iter; i++)
    {
        foreach(var constraint in _constraintList)
        {
            constraint.SolvePosition(dt);
        }
    }

    // コリジョンや座標移動するならこの辺り？？

    // 速度を更新
    foreach(var massPoint in _massPointList)
    {
        massPoint.velocity = (position - prevPosition) * (1.0f / dt);
    }

    // 速度を変更する場合はこの辺で。重力加速とか
}

長くなりましたが以上がPBDの実装です！
ここからが本題、PBDとXPBD、SubstepXPBDとの違いを見ていきます！

XPBD

PBDでは、拘束の反復回数を増やすとバネが無限に固くなってしまう問題がありました。

それを解消するため生まれたのがXPBD(eXtend PBD)です。

違いは拘束計算のみです。
DistanceConstraint_XPBD.cs

DistanceConstraint_XPBD.cs

/// <summary>
/// ラムダをリセット
/// </summary>
public void InitLambda()
{
    _lambda = 0.0f;
}

/// <summary>
/// 拘束計算
/// </summary>
public void SolvePosition(float dt)
{
    var sumMass = _a.invMass + _b.invMass;
    if(sumMass <= 0.0f) { return; }

    var v = _b.position - _a.position;
    var d = v.magnitude;
    if(d <= 0.0f) { return; }

    var constraint  = d - length;               // 目標の距離
    var compliance  = _compliance / (dt * dt);  // コンプライアンス値にdtを加味する
    var dLambda     = (constraint - compliance * _lambda) / (sumMass + compliance); // 今回近づける量

    v = (v / d) * dLambda;  // 拘束ベクトル
    _lambda += dLambda;     // ラムダを累積

    _a.position += v * _a.invMass;
    _b.position -= v * _b.invMass;
}

_a、_b、lengthはPBDから変化ありません。
stiffnessが消えて、代わりに_compliance、_lambdaが追加されています。
それぞれ順番に見てみます。

コンプライアンス値

_complianceは、コンプライアンス値という剛性の逆数だそうです。
http://blog.mmacklin.com/2016/10/12/xpbd-slides-and-stiffness/
逆数なので0が最も固く、値が大きくなるにつれて軟らかくなります。

Const.cs

/// <summary>
/// Miles Macklin's blog (http://blog.mmacklin.com/2016/10/12/xpbd-slides-and-stiffness/)
/// </summary>
private static readonly float[] COMPLIANCE = new float[(int)Compliance.Max]
{
    0.00000000004f, // 0.04 x 10^(-9) (M^2/N) Concrete
    0.00000000016f, // 0.16 x 10^(-9) (M^2/N) Wood
    0.000000001f,   // 1.0  x 10^(-8) (M^2/N) Leather
    0.000000002f,   // 0.2  x 10^(-7) (M^2/N) Tendon
    0.0000001f,     // 1.0  x 10^(-6) (M^2/N) Rubber
    0.00002f,       // 0.2  x 10^(-3) (M^2/N) Muscle
    0.0001f,        // 1.0  x 10^(-3) (M^2/N) Fat
};

試しにコンプライアンス値をFatの0.0001f、dtに0.02fを代入して確認してみます。

// _complianceは0.0001f
var compliance  = 0.0001f / (0.02f * 0.02f);  // 0.0001 / 0.0004 = 0.25

dtの2乗で割っているので、フレームレートが低いほど割る数が大きくなる = コンプライアンス値が小さくなって固くなる挙動になりそうです。
これはなるほどですね。フレームレートの違いによるシミュレーション結果の変化を抑えられそうに見えます。

そしてstiffnessと同じように係数として使います。乗算が除算に変化しています。
sumMassを2と1の場合で見てみます。

// var dLambda     = (constraint - compliance * _lambda) / (sumMass + compliance);

// sumMass = 2(_a, _bともに質量1)
var aa = (1) / (2.0f + 0.25f);    // 1 / 2.25 * 2 = 0.888888...

// sumMass = 1(_aの質量1、_bの質量∞)
var bb = (1) / (1.0f + 0.25f);    // 1 / 1.25 * 1 = 0.8

質量差により変化していますが、コンプライアンス値Fat、dt = 0.02fで、stiffness換算で0.8〜0.888888くらいの効果がありそうです。

ラムダ

ラムダって何でしょう…？使われ方としては以下。

var constraint  = d - length;               // 目標の距離
var compliance  = _compliance / (dt * dt);  // コンプライアンス値にdtを加味する
var dLambda     = (constraint - compliance * _lambda) / (sumMass + compliance); // 今回近づける量

_lambda += dLambda;     // ラムダを累積

移動量はconstraintそのままだったPBDに対して、XPBDでは(constraint - compliance * _lambda)が最終的な移動量に使われています。
_lambdaは拘束の反復計算前に初期化されていて、反復計算中は累積され続けます。

PBDではlengthに向かってstiffnessの割合で無制限に座標を移動していましたが、XPBDではconstraintとcompliance * _lambdaとの吊り合いが取れた段階で計算が収束しそうです。

しかし、式だけ見ても全然分かりません…また値を代入させてください。
現在の質点間の距離は2m、目標距離は1m、コンプライアンス値0.0001f、dtは0.02f、質量の逆数の合計は1で…

// 反復計算1回目。_lambdaはゼロ
var constraint  = 2.0f - 1.0f;                // 目標との差 = 1.0
var compliance  = 0.0004f / (0.02f * 0.02f);  // 0.25
var dLambda     = (1.0f - 0.25f * 0.0f) / (1.0f + 0.25f); // (1.0 - 0.0) / 1.25 = 0.8

_lambda += dLambda;     // 0.8を加算

// 質点の座標を0.8移動

// 反復計算2回目。_lambdaは0.8
var constraint  = 1.2 - 1.0f;                 // 目標との差 = 0.2
var compliance  = 0.0004f / (0.02f * 0.02f);  // 0.25
var dLambda     = (0.2f - 0.25f * 0.8f) / (1.0f + 0.25f); // (0.2 - 0.2) / 1.25 = 0.0

_lambda += dLambda;     // dLambdaは0.0。目標に到達した

// 質点の移動は無し

おおお、反復2回目にしてcompliance * _lambdaの値が目標との差constraintと一致し、計算が収束しました…！
計算の仕組みはまるで分かってないですが、結果だけ見ればXPBDは、
拘束を『2点間の距離を目標の長さにする』から『2点間の距離を目標の長さに向かって移動している途中の状態にする』に改良したものに思えます！

実行結果はこちら。反復回数を変更しても僅かに動きがあるだけで、PBDのような伸縮はありません。

SubstepXPBD

XPBDは良く出来ていますが、シミュレーションの観点から言うとエネルギー減衰が大きい問題がまだ残っているそうです。
これを解消するのがSubstepXPBDです。

すごく単純な話で、これまで『質点の移動を1回、拘束計算をn回』としていた部分を『シミュレーション全体をn回、拘束計算を1回』に変更します。
その際、dtを反復回数で割った小さなステップでシミュレーションを実行します。

コード的にはこう。

TestPBD.cs

        // シミュレーション全体
#if SUBSTEP
        dt = dt / _step;
        for(int i = 0; i < _step; i++)
        {
            Simulate(dt, 1);
        }
#else
        Simulate(dt, _step);
#endif

何というか、逆転の発想です。
必要だと思っていた反復計算が、シミュレーションステップを細かくすれば無くしても大丈夫だという…
しかも反復計算を1回にする場合は、ラムダの累積は削除できます。何だったんだラムダ…！

実行結果は以下。エネルギー減衰が減ったからなのか、ロープの振動が激しくなっています。
それとも何か実装ミスかな…合ってるか不安です。

SubstepXPBDではdtが非常に小さくなる関係で、floatだと精度が足りない場合があるそうです。
今回は全体的にfloatで実装したのもあって、反復回数を増やすと重力加速しなくなったり露骨に挙動がおかしくなりました。
double型のVector2が欲しくなりました。

おわりに

今回XPBD、SubstepXPBDの写経をしてみましたが、いやすごいですね！
何よりPBDからの変更が少ない！僅かな修正で既存の問題点を解消していて、関心してしまいました。

XPBDなんかはすぐさまPBDと差し替えられそうで、軽率にゲームに仕込みたい欲が湧いてきますね。
仕込めるかな…仕込めたら良いな…！

明日の CYBIRD Advent Calendar 2022 18日目は、@kyorokyoroさんの「UnityでAppleのストアアイコンのA/Bテスト」です！お楽しみに〜！