XPBDの定式化でハマるポイント（後退オイラー法からの導出）
久しぶりに読み直したのでメモ
質点の質量行列を $ M 、質点に加わる非保存力（外力と言及されることが多い）を $ F_{ext} 保存力を $ F , 保存力の位置エネルギーを $ E とする
ステップ n における質点の位置と速度をまとめたベクトルを $ {\boldsymbol x}_n, {\boldsymbol v}_n とする。
code:latex
\begin{aligned}
\frac{ {\boldsymbol x}_n - {\boldsymbol x}_{n-1} }{\Delta t} &= {\boldsymbol v}_n \\
M \frac{ {\boldsymbol v}_n - {\boldsymbol v}_{n-1} }{\Delta t} &= {\boldsymbol F}_{ext} + {\boldsymbol F} = {\boldsymbol F}_{ext} - \nabla E({\boldsymbol x}_n)
\end{aligned}
$ {\boldsymbol v}_n を消去すると、
code:latex
\begin{aligned}
\frac{M}{\Delta t^2} ( {\boldsymbol x}_n - \tilde{{\boldsymbol x}}) = - \nabla E({\boldsymbol x}_n)
\end{aligned}
となる。
ただし、 $ \tilde{{\boldsymbol x}} = 2 {\boldsymbol x}_{n-1} - {\boldsymbol x}_{n-2} + \Delta t^2 M^{-1} {\boldsymbol F}_{ext} = {\boldsymbol x}_{n-1} + \Delta t {\boldsymbol v}_{n-1} + \Delta t^2 M^{-1} {\boldsymbol F}_{ext}
これは以下の最適化問題と見なせる
code:latex
\begin{aligned}
{\boldsymbol x}_n = \underset{{\boldsymbol x}}{\rm argmin} \frac{1}{2 \Delta t^2} \left( {\boldsymbol x} - \tilde{{\boldsymbol x}} \right)^\top M \left( {\boldsymbol x} - \tilde{{\boldsymbol x}} \right) + E({\boldsymbol x})
\end{aligned}
XPBD (Extended Position Based Dynamics) はこの最適化問題を解く。
拘束は $ C = \sqrt{2 E({\boldsymbol x}) } として、エネルギーから導出される関数である。
例えば、距離拘束 $ C = \left\| {\boldsymbol x} - {\boldsymbol p} \right\| は力学的にはバネと同じエネルギーになる。
また、新たにラグランジュの未定乗数（これは拘束力の大きさのスケールファクターとなる） $ \lambda = - \Delta t^2 C({\boldsymbol x}) を導入して $ \nabla E({\boldsymbol x}) = C({\boldsymbol x}) \nabla C({\boldsymbol x}) = - \frac{1}{\Delta t^2} \lambda \nabla C({\boldsymbol x}) とすると、
code:tex
\begin{aligned}
M ( {\boldsymbol x} - \tilde{{\boldsymbol x}}) - \lambda \nabla C ({\boldsymbol x}) &= 0 \\
C({\boldsymbol x}) + \lambda/\Delta t^2 &= 0
\end{aligned}
と書ける。
注意：拘束が複数ある場合は各拘束 $ C_j に対応する $ \lambda_j をそれぞれ定義し、
code:latex
\begin{aligned}
M ( {\boldsymbol x} - \tilde{{\boldsymbol x}}) - \lambda_j \nabla C_j ({\boldsymbol x}) &= 0 \\
C_j({\boldsymbol x}) + \lambda_j / \Delta t^2 &= 0 \quad (全ての j に対して成立）
\end{aligned}
となる。
これを準ニュートン法で反復的に解く。
最適化の各ステップ（タイムステップではない）における、 $ {\boldsymbol x} , $ \lambda の現在の値と、前回の値をそれぞれ、 $ {\boldsymbol x}^i, \lambda^i, {\boldsymbol x}^{i-1}, \lambda^{i-1} とする。
実際にはその差分の $ \Delta {\boldsymbol x} = {\boldsymbol x}^i - {\boldsymbol x}^{i-1}, \Delta \lambda = \lambda^i - \lambda^{i-1} で表記する。
注意：タイムステップの添え字 n ではなく、各 $ {\boldsymbol x} を求める最適化のサブステップの添え字である。
また最適化の初期値は $ {\boldsymbol x}^0 = \tilde{{\boldsymbol x}}, \lambda^0 = 0 とする。
各ステップにおいて、 $ \Delta {\boldsymbol x}, \Delta \lambda は次の式を解くことで得られる。
code:tex
\begin{aligned}
M ( {\boldsymbol x}^{i-1} + \Delta {\boldsymbol x} - \tilde{{\boldsymbol x}}) - \lambda^{i-1} \nabla C ({\boldsymbol x}^{i-1}) - \Delta \lambda \nabla C ({\boldsymbol x}^{i-1}) - \lambda^{i-1} \nabla \nabla C ({\boldsymbol x}^{i-1}) \cdot \Delta {\boldsymbol x} &= 0 \\
C({\boldsymbol x}^{i-1}) + \nabla C ({\boldsymbol x}^{i-1}) \cdot \Delta {\boldsymbol x} + \lambda^{i-1}/\Delta t^2 + \Delta \lambda / \Delta t^2 &= 0
\end{aligned}
これを変形して
code:latex
\begin{aligned}
M \Delta {\boldsymbol x} - \Delta \lambda \nabla C ({\boldsymbol x}^{i-1}) - \lambda^{i-1} \nabla \nabla C({\boldsymbol x}^{i-1}) \cdot \Delta {\boldsymbol x} &= - M({\boldsymbol x}^{i-1} - \tilde{{\boldsymbol x}} ) + \lambda^{i-1} \nabla C ({\boldsymbol x}^{i-1} ) \\
\nabla C ({\boldsymbol x}^{i-1}) \cdot \Delta {\boldsymbol x} + \Delta \lambda / \Delta t^2 &= - C ({\boldsymbol x}^{i-1}) - \lambda^{i-1} / \Delta t^2
\end{aligned}
また XPBD では以下の近似を導入する（ Macklin らによるスライドを参照されたし）
$ \nabla \nabla C({\boldsymbol x}) = 0 拘束のヘッセ行列はゼロ。
非線形すぎるケースで誤差が生じる
$ M ( {\boldsymbol x} - \tilde{{\boldsymbol x}}) - \lambda \nabla C ({\boldsymbol x}) = 0 は常に充足されている。
少なくとも初期値では充足されている。
最適化を進めた場合でも、各更新で $ M \Delta {\boldsymbol x} - \Delta \lambda \nabla C({\boldsymbol x}^{i-1}) = 0 となるように更新するが（後述）、誤差はヘッセ行列の項のみに起因する、従って最初の仮定がある程度妥当であれば、こちらもある程度は正しいと言える（完全にゼロであれば真に充足される）。
それを元に更新式は
code:latex
\begin{aligned}
M \Delta {\boldsymbol x} - \Delta \lambda \nabla C ({\boldsymbol x}^{i-1}) &= 0 \\
\nabla C({\boldsymbol x}^{i-1}) \cdot \Delta {\boldsymbol x} + \Delta \lambda / \Delta t^2 &= - C ( {\boldsymbol x}^{i-1} ) - \lambda^{i-1} / \Delta t^2
\end{aligned}
これを解くと
code:latex
\begin{aligned}
\Delta {\boldsymbol x} &= \Delta \lambda M^{-1} \nabla C ({\boldsymbol x}^{i-1}) \\
\Delta \lambda \left( \nabla C({\boldsymbol x}^{i-1})^\top M^{-1} \nabla C( {\boldsymbol x}^{i-1} ) + \frac{1}{\Delta t^2} \right) &= - C ({\boldsymbol x}^{i-1}) - \lambda^{i-1} / \Delta t^2
\end{aligned}
実装例：
参考文献
Miles Macklin, Matthias Müller, and Nuttapong Chentanez. 2016. XPBD: position-based simulation of compliant constrained dynamics. In Proceedings of the 9th International Conference on Motion in Games (MIG '16). Association for Computing Machinery, New York, NY, USA, 49–54.